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