1// SPDX-License-Identifier: GPL-2.0
2/*
3 * Intel(R) Trace Hub Software Trace Hub support
4 *
5 * Copyright (C) 2014-2015 Intel Corporation.
6 */
7
8#define pr_fmt(fmt)	KBUILD_MODNAME ": " fmt
9
10#include <linux/types.h>
11#include <linux/module.h>
12#include <linux/device.h>
13#include <linux/io.h>
14#include <linux/mm.h>
15#include <linux/slab.h>
16#include <linux/stm.h>
17
18#include "intel_th.h"
19#include "sth.h"
20
21struct sth_device {
22	void __iomem	*base;
23	void __iomem	*channels;
24	phys_addr_t	channels_phys;
25	struct device	*dev;
26	struct stm_data	stm;
27	unsigned int	sw_nmasters;
28};
29
30static struct intel_th_channel __iomem *
31sth_channel(struct sth_device *sth, unsigned int master, unsigned int channel)
32{
33	struct intel_th_channel __iomem *sw_map = sth->channels;
34
35	return &sw_map[(master - sth->stm.sw_start) * sth->stm.sw_nchannels +
36		       channel];
37}
38
39static void sth_iowrite(void __iomem *dest, const unsigned char *payload,
40			unsigned int size)
41{
42	switch (size) {
43#ifdef CONFIG_64BIT
44	case 8:
45		writeq_relaxed(*(u64 *)payload, dest);
46		break;
47#endif
48	case 4:
49		writel_relaxed(*(u32 *)payload, dest);
50		break;
51	case 2:
52		writew_relaxed(*(u16 *)payload, dest);
53		break;
54	case 1:
55		writeb_relaxed(*(u8 *)payload, dest);
56		break;
57	default:
58		break;
59	}
60}
61
62static ssize_t notrace sth_stm_packet(struct stm_data *stm_data,
63				      unsigned int master,
64				      unsigned int channel,
65				      unsigned int packet,
66				      unsigned int flags,
67				      unsigned int size,
68				      const unsigned char *payload)
69{
70	struct sth_device *sth = container_of(stm_data, struct sth_device, stm);
71	struct intel_th_channel __iomem *out =
72		sth_channel(sth, master, channel);
73	u64 __iomem *outp = &out->Dn;
74	unsigned long reg = REG_STH_TRIG;
75
76#ifndef CONFIG_64BIT
77	if (size > 4)
78		size = 4;
79#endif
80
81	size = rounddown_pow_of_two(size);
82
83	switch (packet) {
84	/* Global packets (GERR, XSYNC, TRIG) are sent with register writes */
85	case STP_PACKET_GERR:
86		reg += 4;
87		fallthrough;
88
89	case STP_PACKET_XSYNC:
90		reg += 8;
91		fallthrough;
92
93	case STP_PACKET_TRIG:
94		if (flags & STP_PACKET_TIMESTAMPED)
95			reg += 4;
96		writeb_relaxed(*payload, sth->base + reg);
97		break;
98
99	case STP_PACKET_MERR:
100		if (size > 4)
101			size = 4;
102
103		sth_iowrite(&out->MERR, payload, size);
104		break;
105
106	case STP_PACKET_FLAG:
107		if (flags & STP_PACKET_TIMESTAMPED)
108			outp = (u64 __iomem *)&out->FLAG_TS;
109		else
110			outp = (u64 __iomem *)&out->FLAG;
111
112		size = 0;
113		writeb_relaxed(0, outp);
114		break;
115
116	case STP_PACKET_USER:
117		if (flags & STP_PACKET_TIMESTAMPED)
118			outp = &out->USER_TS;
119		else
120			outp = &out->USER;
121		sth_iowrite(outp, payload, size);
122		break;
123
124	case STP_PACKET_DATA:
125		outp = &out->Dn;
126
127		if (flags & STP_PACKET_TIMESTAMPED)
128			outp += 2;
129		if (flags & STP_PACKET_MARKED)
130			outp++;
131
132		sth_iowrite(outp, payload, size);
133		break;
134	default:
135		return -ENOTSUPP;
136	}
137
138	return size;
139}
140
141static phys_addr_t
142sth_stm_mmio_addr(struct stm_data *stm_data, unsigned int master,
143		  unsigned int channel, unsigned int nr_chans)
144{
145	struct sth_device *sth = container_of(stm_data, struct sth_device, stm);
146	phys_addr_t addr;
147
148	master -= sth->stm.sw_start;
149	addr = sth->channels_phys + (master * sth->stm.sw_nchannels + channel) *
150		sizeof(struct intel_th_channel);
151
152	if (offset_in_page(addr) ||
153	    offset_in_page(nr_chans * sizeof(struct intel_th_channel)))
154		return 0;
155
156	return addr;
157}
158
159static int sth_stm_link(struct stm_data *stm_data, unsigned int master,
160			 unsigned int channel)
161{
162	struct sth_device *sth = container_of(stm_data, struct sth_device, stm);
163
164	return intel_th_set_output(to_intel_th_device(sth->dev), master);
165}
166
167static int intel_th_sw_init(struct sth_device *sth)
168{
169	u32 reg;
170
171	reg = ioread32(sth->base + REG_STH_STHCAP1);
172	sth->stm.sw_nchannels = reg & 0xff;
173
174	reg = ioread32(sth->base + REG_STH_STHCAP0);
175	sth->stm.sw_start = reg & 0xffff;
176	sth->stm.sw_end = reg >> 16;
177
178	sth->sw_nmasters = sth->stm.sw_end - sth->stm.sw_start;
179	dev_dbg(sth->dev, "sw_start: %x sw_end: %x masters: %x nchannels: %x\n",
180		sth->stm.sw_start, sth->stm.sw_end, sth->sw_nmasters,
181		sth->stm.sw_nchannels);
182
183	return 0;
184}
185
186static int intel_th_sth_probe(struct intel_th_device *thdev)
187{
188	struct device *dev = &thdev->dev;
189	struct sth_device *sth;
190	struct resource *res;
191	void __iomem *base, *channels;
192	int err;
193
194	res = intel_th_device_get_resource(thdev, IORESOURCE_MEM, 0);
195	if (!res)
196		return -ENODEV;
197
198	base = devm_ioremap(dev, res->start, resource_size(res));
199	if (!base)
200		return -ENOMEM;
201
202	res = intel_th_device_get_resource(thdev, IORESOURCE_MEM, 1);
203	if (!res)
204		return -ENODEV;
205
206	channels = devm_ioremap(dev, res->start, resource_size(res));
207	if (!channels)
208		return -ENOMEM;
209
210	sth = devm_kzalloc(dev, sizeof(*sth), GFP_KERNEL);
211	if (!sth)
212		return -ENOMEM;
213
214	sth->dev = dev;
215	sth->base = base;
216	sth->channels = channels;
217	sth->channels_phys = res->start;
218	sth->stm.name = dev_name(dev);
219	sth->stm.packet = sth_stm_packet;
220	sth->stm.mmio_addr = sth_stm_mmio_addr;
221	sth->stm.sw_mmiosz = sizeof(struct intel_th_channel);
222	sth->stm.link = sth_stm_link;
223
224	err = intel_th_sw_init(sth);
225	if (err)
226		return err;
227
228	err = stm_register_device(dev, &sth->stm, THIS_MODULE);
229	if (err) {
230		dev_err(dev, "stm_register_device failed\n");
231		return err;
232	}
233
234	dev_set_drvdata(dev, sth);
235
236	return 0;
237}
238
239static void intel_th_sth_remove(struct intel_th_device *thdev)
240{
241	struct sth_device *sth = dev_get_drvdata(&thdev->dev);
242
243	stm_unregister_device(&sth->stm);
244}
245
246static struct intel_th_driver intel_th_sth_driver = {
247	.probe	= intel_th_sth_probe,
248	.remove	= intel_th_sth_remove,
249	.driver	= {
250		.name	= "sth",
251		.owner	= THIS_MODULE,
252	},
253};
254
255module_driver(intel_th_sth_driver,
256	      intel_th_driver_register,
257	      intel_th_driver_unregister);
258
259MODULE_LICENSE("GPL v2");
260MODULE_DESCRIPTION("Intel(R) Trace Hub Software Trace Hub driver");
261MODULE_AUTHOR("Alexander Shishkin <alexander.shishkin@intel.com>");
262