Commit | Line | Data |
---|---|---|
50352fa7 | 1 | // SPDX-License-Identifier: GPL-2.0 |
f04e449f AS |
2 | /* |
3 | * Intel(R) Trace Hub Software Trace Hub support | |
4 | * | |
5 | * Copyright (C) 2014-2015 Intel Corporation. | |
f04e449f AS |
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 | ||
22975be2 CZ |
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) | |
f04e449f AS |
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 | case STP_PACKET_XSYNC: | |
88 | reg += 8; | |
89 | case STP_PACKET_TRIG: | |
90 | if (flags & STP_PACKET_TIMESTAMPED) | |
91 | reg += 4; | |
97007500 | 92 | writeb_relaxed(*payload, sth->base + reg); |
f04e449f AS |
93 | break; |
94 | ||
95 | case STP_PACKET_MERR: | |
97007500 AS |
96 | if (size > 4) |
97 | size = 4; | |
98 | ||
f04e449f AS |
99 | sth_iowrite(&out->MERR, payload, size); |
100 | break; | |
101 | ||
102 | case STP_PACKET_FLAG: | |
103 | if (flags & STP_PACKET_TIMESTAMPED) | |
104 | outp = (u64 __iomem *)&out->FLAG_TS; | |
105 | else | |
106 | outp = (u64 __iomem *)&out->FLAG; | |
107 | ||
97007500 AS |
108 | size = 0; |
109 | writeb_relaxed(0, outp); | |
f04e449f AS |
110 | break; |
111 | ||
112 | case STP_PACKET_USER: | |
113 | if (flags & STP_PACKET_TIMESTAMPED) | |
114 | outp = &out->USER_TS; | |
115 | else | |
116 | outp = &out->USER; | |
117 | sth_iowrite(outp, payload, size); | |
118 | break; | |
119 | ||
120 | case STP_PACKET_DATA: | |
121 | outp = &out->Dn; | |
122 | ||
123 | if (flags & STP_PACKET_TIMESTAMPED) | |
124 | outp += 2; | |
125 | if (flags & STP_PACKET_MARKED) | |
126 | outp++; | |
127 | ||
128 | sth_iowrite(outp, payload, size); | |
129 | break; | |
97007500 AS |
130 | default: |
131 | return -ENOTSUPP; | |
f04e449f AS |
132 | } |
133 | ||
134 | return size; | |
135 | } | |
136 | ||
137 | static phys_addr_t | |
138 | sth_stm_mmio_addr(struct stm_data *stm_data, unsigned int master, | |
139 | unsigned int channel, unsigned int nr_chans) | |
140 | { | |
141 | struct sth_device *sth = container_of(stm_data, struct sth_device, stm); | |
142 | phys_addr_t addr; | |
143 | ||
144 | master -= sth->stm.sw_start; | |
145 | addr = sth->channels_phys + (master * sth->stm.sw_nchannels + channel) * | |
146 | sizeof(struct intel_th_channel); | |
147 | ||
148 | if (offset_in_page(addr) || | |
149 | offset_in_page(nr_chans * sizeof(struct intel_th_channel))) | |
150 | return 0; | |
151 | ||
152 | return addr; | |
153 | } | |
154 | ||
155 | static int sth_stm_link(struct stm_data *stm_data, unsigned int master, | |
156 | unsigned int channel) | |
157 | { | |
158 | struct sth_device *sth = container_of(stm_data, struct sth_device, stm); | |
159 | ||
160 | intel_th_set_output(to_intel_th_device(sth->dev), master); | |
161 | ||
162 | return 0; | |
163 | } | |
164 | ||
165 | static int intel_th_sw_init(struct sth_device *sth) | |
166 | { | |
167 | u32 reg; | |
168 | ||
169 | reg = ioread32(sth->base + REG_STH_STHCAP1); | |
170 | sth->stm.sw_nchannels = reg & 0xff; | |
171 | ||
172 | reg = ioread32(sth->base + REG_STH_STHCAP0); | |
173 | sth->stm.sw_start = reg & 0xffff; | |
174 | sth->stm.sw_end = reg >> 16; | |
175 | ||
176 | sth->sw_nmasters = sth->stm.sw_end - sth->stm.sw_start; | |
177 | dev_dbg(sth->dev, "sw_start: %x sw_end: %x masters: %x nchannels: %x\n", | |
178 | sth->stm.sw_start, sth->stm.sw_end, sth->sw_nmasters, | |
179 | sth->stm.sw_nchannels); | |
180 | ||
181 | return 0; | |
182 | } | |
183 | ||
184 | static int intel_th_sth_probe(struct intel_th_device *thdev) | |
185 | { | |
186 | struct device *dev = &thdev->dev; | |
187 | struct sth_device *sth; | |
188 | struct resource *res; | |
189 | void __iomem *base, *channels; | |
190 | int err; | |
191 | ||
192 | res = intel_th_device_get_resource(thdev, IORESOURCE_MEM, 0); | |
193 | if (!res) | |
194 | return -ENODEV; | |
195 | ||
196 | base = devm_ioremap(dev, res->start, resource_size(res)); | |
73061da0 DC |
197 | if (!base) |
198 | return -ENOMEM; | |
f04e449f AS |
199 | |
200 | res = intel_th_device_get_resource(thdev, IORESOURCE_MEM, 1); | |
201 | if (!res) | |
202 | return -ENODEV; | |
203 | ||
204 | channels = devm_ioremap(dev, res->start, resource_size(res)); | |
73061da0 DC |
205 | if (!channels) |
206 | return -ENOMEM; | |
f04e449f AS |
207 | |
208 | sth = devm_kzalloc(dev, sizeof(*sth), GFP_KERNEL); | |
209 | if (!sth) | |
210 | return -ENOMEM; | |
211 | ||
212 | sth->dev = dev; | |
213 | sth->base = base; | |
214 | sth->channels = channels; | |
215 | sth->channels_phys = res->start; | |
216 | sth->stm.name = dev_name(dev); | |
217 | sth->stm.packet = sth_stm_packet; | |
218 | sth->stm.mmio_addr = sth_stm_mmio_addr; | |
219 | sth->stm.sw_mmiosz = sizeof(struct intel_th_channel); | |
220 | sth->stm.link = sth_stm_link; | |
221 | ||
222 | err = intel_th_sw_init(sth); | |
223 | if (err) | |
224 | return err; | |
225 | ||
226 | err = stm_register_device(dev, &sth->stm, THIS_MODULE); | |
227 | if (err) { | |
228 | dev_err(dev, "stm_register_device failed\n"); | |
229 | return err; | |
230 | } | |
231 | ||
232 | dev_set_drvdata(dev, sth); | |
233 | ||
234 | return 0; | |
235 | } | |
236 | ||
237 | static void intel_th_sth_remove(struct intel_th_device *thdev) | |
238 | { | |
239 | struct sth_device *sth = dev_get_drvdata(&thdev->dev); | |
240 | ||
241 | stm_unregister_device(&sth->stm); | |
242 | } | |
243 | ||
244 | static struct intel_th_driver intel_th_sth_driver = { | |
245 | .probe = intel_th_sth_probe, | |
246 | .remove = intel_th_sth_remove, | |
247 | .driver = { | |
248 | .name = "sth", | |
249 | .owner = THIS_MODULE, | |
250 | }, | |
251 | }; | |
252 | ||
253 | module_driver(intel_th_sth_driver, | |
254 | intel_th_driver_register, | |
255 | intel_th_driver_unregister); | |
256 | ||
257 | MODULE_LICENSE("GPL v2"); | |
258 | MODULE_DESCRIPTION("Intel(R) Trace Hub Software Trace Hub driver"); | |
259 | MODULE_AUTHOR("Alexander Shishkin <alexander.shishkin@intel.com>"); |