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 | |
21 | struct 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 | |
30 | static struct intel_th_channel __iomem * |
31 | sth_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 | |
39 | static 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 | |
62 | static 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(dest: &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(dest: 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(dest: outp, payload, size); |
133 | break; |
134 | default: |
135 | return -ENOTSUPP; |
136 | } |
137 | |
138 | return size; |
139 | } |
140 | |
141 | static phys_addr_t |
142 | sth_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 | |
159 | static 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 | |
167 | static 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 | |
186 | static 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, num: 0); |
195 | if (!res) |
196 | return -ENODEV; |
197 | |
198 | base = devm_ioremap(dev, offset: res->start, size: resource_size(res)); |
199 | if (!base) |
200 | return -ENOMEM; |
201 | |
202 | res = intel_th_device_get_resource(thdev, IORESOURCE_MEM, num: 1); |
203 | if (!res) |
204 | return -ENODEV; |
205 | |
206 | channels = devm_ioremap(dev, offset: res->start, size: resource_size(res)); |
207 | if (!channels) |
208 | return -ENOMEM; |
209 | |
210 | sth = devm_kzalloc(dev, size: 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(parent: dev, stm_data: &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, data: sth); |
235 | |
236 | return 0; |
237 | } |
238 | |
239 | static void intel_th_sth_remove(struct intel_th_device *thdev) |
240 | { |
241 | struct sth_device *sth = dev_get_drvdata(dev: &thdev->dev); |
242 | |
243 | stm_unregister_device(stm_data: &sth->stm); |
244 | } |
245 | |
246 | static 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 | |
255 | module_driver(intel_th_sth_driver, |
256 | intel_th_driver_register, |
257 | intel_th_driver_unregister); |
258 | |
259 | MODULE_LICENSE("GPL v2" ); |
260 | MODULE_DESCRIPTION("Intel(R) Trace Hub Software Trace Hub driver" ); |
261 | MODULE_AUTHOR("Alexander Shishkin <alexander.shishkin@intel.com>" ); |
262 | |