Commit | Line | Data |
---|---|---|
2b49d128 YL |
1 | // SPDX-License-Identifier: (GPL-2.0 OR MIT) |
2 | /* Microsemi Ocelot PTP clock driver | |
3 | * | |
4 | * Copyright (c) 2017 Microsemi Corporation | |
5 | * Copyright 2020 NXP | |
6 | */ | |
a460513e AS |
7 | #include <linux/time64.h> |
8 | ||
d50e41bf VO |
9 | #include <linux/dsa/ocelot.h> |
10 | #include <linux/ptp_classify.h> | |
2b49d128 YL |
11 | #include <soc/mscc/ocelot_ptp.h> |
12 | #include <soc/mscc/ocelot_sys.h> | |
d50e41bf | 13 | #include <soc/mscc/ocelot_vcap.h> |
2b49d128 | 14 | #include <soc/mscc/ocelot.h> |
d50e41bf | 15 | #include "ocelot.h" |
2b49d128 YL |
16 | |
17 | int ocelot_ptp_gettime64(struct ptp_clock_info *ptp, struct timespec64 *ts) | |
18 | { | |
19 | struct ocelot *ocelot = container_of(ptp, struct ocelot, ptp_info); | |
20 | unsigned long flags; | |
21 | time64_t s; | |
22 | u32 val; | |
23 | s64 ns; | |
24 | ||
25 | spin_lock_irqsave(&ocelot->ptp_clock_lock, flags); | |
26 | ||
27 | val = ocelot_read_rix(ocelot, PTP_PIN_CFG, TOD_ACC_PIN); | |
28 | val &= ~(PTP_PIN_CFG_SYNC | PTP_PIN_CFG_ACTION_MASK | PTP_PIN_CFG_DOM); | |
29 | val |= PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_SAVE); | |
30 | ocelot_write_rix(ocelot, val, PTP_PIN_CFG, TOD_ACC_PIN); | |
31 | ||
32 | s = ocelot_read_rix(ocelot, PTP_PIN_TOD_SEC_MSB, TOD_ACC_PIN) & 0xffff; | |
33 | s <<= 32; | |
34 | s += ocelot_read_rix(ocelot, PTP_PIN_TOD_SEC_LSB, TOD_ACC_PIN); | |
35 | ns = ocelot_read_rix(ocelot, PTP_PIN_TOD_NSEC, TOD_ACC_PIN); | |
36 | ||
37 | spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags); | |
38 | ||
39 | /* Deal with negative values */ | |
40 | if (ns >= 0x3ffffff0 && ns <= 0x3fffffff) { | |
41 | s--; | |
42 | ns &= 0xf; | |
43 | ns += 999999984; | |
44 | } | |
45 | ||
46 | set_normalized_timespec64(ts, s, ns); | |
47 | return 0; | |
48 | } | |
49 | EXPORT_SYMBOL(ocelot_ptp_gettime64); | |
50 | ||
51 | int ocelot_ptp_settime64(struct ptp_clock_info *ptp, | |
52 | const struct timespec64 *ts) | |
53 | { | |
54 | struct ocelot *ocelot = container_of(ptp, struct ocelot, ptp_info); | |
55 | unsigned long flags; | |
56 | u32 val; | |
57 | ||
58 | spin_lock_irqsave(&ocelot->ptp_clock_lock, flags); | |
59 | ||
60 | val = ocelot_read_rix(ocelot, PTP_PIN_CFG, TOD_ACC_PIN); | |
61 | val &= ~(PTP_PIN_CFG_SYNC | PTP_PIN_CFG_ACTION_MASK | PTP_PIN_CFG_DOM); | |
62 | val |= PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_IDLE); | |
63 | ||
64 | ocelot_write_rix(ocelot, val, PTP_PIN_CFG, TOD_ACC_PIN); | |
65 | ||
66 | ocelot_write_rix(ocelot, lower_32_bits(ts->tv_sec), PTP_PIN_TOD_SEC_LSB, | |
67 | TOD_ACC_PIN); | |
68 | ocelot_write_rix(ocelot, upper_32_bits(ts->tv_sec), PTP_PIN_TOD_SEC_MSB, | |
69 | TOD_ACC_PIN); | |
70 | ocelot_write_rix(ocelot, ts->tv_nsec, PTP_PIN_TOD_NSEC, TOD_ACC_PIN); | |
71 | ||
72 | val = ocelot_read_rix(ocelot, PTP_PIN_CFG, TOD_ACC_PIN); | |
73 | val &= ~(PTP_PIN_CFG_SYNC | PTP_PIN_CFG_ACTION_MASK | PTP_PIN_CFG_DOM); | |
74 | val |= PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_LOAD); | |
75 | ||
76 | ocelot_write_rix(ocelot, val, PTP_PIN_CFG, TOD_ACC_PIN); | |
77 | ||
78 | spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags); | |
8670dc33 XY |
79 | |
80 | if (ocelot->ops->tas_clock_adjust) | |
81 | ocelot->ops->tas_clock_adjust(ocelot); | |
82 | ||
2b49d128 YL |
83 | return 0; |
84 | } | |
85 | EXPORT_SYMBOL(ocelot_ptp_settime64); | |
86 | ||
87 | int ocelot_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta) | |
88 | { | |
89 | if (delta > -(NSEC_PER_SEC / 2) && delta < (NSEC_PER_SEC / 2)) { | |
90 | struct ocelot *ocelot = container_of(ptp, struct ocelot, | |
91 | ptp_info); | |
92 | unsigned long flags; | |
93 | u32 val; | |
94 | ||
95 | spin_lock_irqsave(&ocelot->ptp_clock_lock, flags); | |
96 | ||
97 | val = ocelot_read_rix(ocelot, PTP_PIN_CFG, TOD_ACC_PIN); | |
98 | val &= ~(PTP_PIN_CFG_SYNC | PTP_PIN_CFG_ACTION_MASK | | |
99 | PTP_PIN_CFG_DOM); | |
100 | val |= PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_IDLE); | |
101 | ||
102 | ocelot_write_rix(ocelot, val, PTP_PIN_CFG, TOD_ACC_PIN); | |
103 | ||
104 | ocelot_write_rix(ocelot, 0, PTP_PIN_TOD_SEC_LSB, TOD_ACC_PIN); | |
105 | ocelot_write_rix(ocelot, 0, PTP_PIN_TOD_SEC_MSB, TOD_ACC_PIN); | |
106 | ocelot_write_rix(ocelot, delta, PTP_PIN_TOD_NSEC, TOD_ACC_PIN); | |
107 | ||
108 | val = ocelot_read_rix(ocelot, PTP_PIN_CFG, TOD_ACC_PIN); | |
109 | val &= ~(PTP_PIN_CFG_SYNC | PTP_PIN_CFG_ACTION_MASK | | |
110 | PTP_PIN_CFG_DOM); | |
111 | val |= PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_DELTA); | |
112 | ||
113 | ocelot_write_rix(ocelot, val, PTP_PIN_CFG, TOD_ACC_PIN); | |
114 | ||
115 | spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags); | |
8670dc33 XY |
116 | |
117 | if (ocelot->ops->tas_clock_adjust) | |
118 | ocelot->ops->tas_clock_adjust(ocelot); | |
2b49d128 YL |
119 | } else { |
120 | /* Fall back using ocelot_ptp_settime64 which is not exact. */ | |
121 | struct timespec64 ts; | |
122 | u64 now; | |
123 | ||
124 | ocelot_ptp_gettime64(ptp, &ts); | |
125 | ||
126 | now = ktime_to_ns(timespec64_to_ktime(ts)); | |
127 | ts = ns_to_timespec64(now + delta); | |
128 | ||
129 | ocelot_ptp_settime64(ptp, &ts); | |
130 | } | |
8670dc33 | 131 | |
2b49d128 YL |
132 | return 0; |
133 | } | |
134 | EXPORT_SYMBOL(ocelot_ptp_adjtime); | |
135 | ||
136 | int ocelot_ptp_adjfine(struct ptp_clock_info *ptp, long scaled_ppm) | |
137 | { | |
138 | struct ocelot *ocelot = container_of(ptp, struct ocelot, ptp_info); | |
139 | u32 unit = 0, direction = 0; | |
140 | unsigned long flags; | |
141 | u64 adj = 0; | |
142 | ||
143 | spin_lock_irqsave(&ocelot->ptp_clock_lock, flags); | |
144 | ||
145 | if (!scaled_ppm) | |
146 | goto disable_adj; | |
147 | ||
148 | if (scaled_ppm < 0) { | |
149 | direction = PTP_CFG_CLK_ADJ_CFG_DIR; | |
150 | scaled_ppm = -scaled_ppm; | |
151 | } | |
152 | ||
153 | adj = PSEC_PER_SEC << 16; | |
154 | do_div(adj, scaled_ppm); | |
155 | do_div(adj, 1000); | |
156 | ||
157 | /* If the adjustment value is too large, use ns instead */ | |
158 | if (adj >= (1L << 30)) { | |
159 | unit = PTP_CFG_CLK_ADJ_FREQ_NS; | |
160 | do_div(adj, 1000); | |
161 | } | |
162 | ||
163 | /* Still too big */ | |
164 | if (adj >= (1L << 30)) | |
165 | goto disable_adj; | |
166 | ||
167 | ocelot_write(ocelot, unit | adj, PTP_CLK_CFG_ADJ_FREQ); | |
168 | ocelot_write(ocelot, PTP_CFG_CLK_ADJ_CFG_ENA | direction, | |
169 | PTP_CLK_CFG_ADJ_CFG); | |
170 | ||
171 | spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags); | |
172 | return 0; | |
173 | ||
174 | disable_adj: | |
175 | ocelot_write(ocelot, 0, PTP_CLK_CFG_ADJ_CFG); | |
176 | ||
177 | spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags); | |
178 | return 0; | |
179 | } | |
180 | EXPORT_SYMBOL(ocelot_ptp_adjfine); | |
181 | ||
cc2d87bb YL |
182 | int ocelot_ptp_verify(struct ptp_clock_info *ptp, unsigned int pin, |
183 | enum ptp_pin_function func, unsigned int chan) | |
184 | { | |
185 | switch (func) { | |
186 | case PTP_PF_NONE: | |
187 | case PTP_PF_PEROUT: | |
188 | break; | |
189 | case PTP_PF_EXTTS: | |
190 | case PTP_PF_PHYSYNC: | |
191 | return -1; | |
192 | } | |
193 | return 0; | |
194 | } | |
195 | EXPORT_SYMBOL(ocelot_ptp_verify); | |
196 | ||
197 | int ocelot_ptp_enable(struct ptp_clock_info *ptp, | |
198 | struct ptp_clock_request *rq, int on) | |
199 | { | |
200 | struct ocelot *ocelot = container_of(ptp, struct ocelot, ptp_info); | |
ecf9f9b7 | 201 | struct timespec64 ts_phase, ts_period; |
cc2d87bb YL |
202 | enum ocelot_ptp_pins ptp_pin; |
203 | unsigned long flags; | |
204 | bool pps = false; | |
205 | int pin = -1; | |
ecf9f9b7 VO |
206 | s64 wf_high; |
207 | s64 wf_low; | |
cc2d87bb | 208 | u32 val; |
cc2d87bb YL |
209 | |
210 | switch (rq->type) { | |
211 | case PTP_CLK_REQ_PEROUT: | |
212 | /* Reject requests with unsupported flags */ | |
ecf9f9b7 VO |
213 | if (rq->perout.flags & ~(PTP_PEROUT_DUTY_CYCLE | |
214 | PTP_PEROUT_PHASE)) | |
cc2d87bb YL |
215 | return -EOPNOTSUPP; |
216 | ||
217 | pin = ptp_find_pin(ocelot->ptp_clock, PTP_PF_PEROUT, | |
218 | rq->perout.index); | |
219 | if (pin == 0) | |
220 | ptp_pin = PTP_PIN_0; | |
221 | else if (pin == 1) | |
222 | ptp_pin = PTP_PIN_1; | |
223 | else if (pin == 2) | |
224 | ptp_pin = PTP_PIN_2; | |
225 | else if (pin == 3) | |
226 | ptp_pin = PTP_PIN_3; | |
227 | else | |
228 | return -EBUSY; | |
229 | ||
cc2d87bb YL |
230 | ts_period.tv_sec = rq->perout.period.sec; |
231 | ts_period.tv_nsec = rq->perout.period.nsec; | |
232 | ||
233 | if (ts_period.tv_sec == 1 && ts_period.tv_nsec == 0) | |
234 | pps = true; | |
235 | ||
cc2d87bb YL |
236 | /* Handle turning off */ |
237 | if (!on) { | |
238 | spin_lock_irqsave(&ocelot->ptp_clock_lock, flags); | |
239 | val = PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_IDLE); | |
240 | ocelot_write_rix(ocelot, val, PTP_PIN_CFG, ptp_pin); | |
241 | spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags); | |
242 | break; | |
243 | } | |
244 | ||
ecf9f9b7 VO |
245 | if (rq->perout.flags & PTP_PEROUT_PHASE) { |
246 | ts_phase.tv_sec = rq->perout.phase.sec; | |
247 | ts_phase.tv_nsec = rq->perout.phase.nsec; | |
248 | } else { | |
249 | /* Compatibility */ | |
250 | ts_phase.tv_sec = rq->perout.start.sec; | |
251 | ts_phase.tv_nsec = rq->perout.start.nsec; | |
252 | } | |
253 | if (ts_phase.tv_sec || (ts_phase.tv_nsec && !pps)) { | |
254 | dev_warn(ocelot->dev, | |
255 | "Absolute start time not supported!\n"); | |
256 | dev_warn(ocelot->dev, | |
257 | "Accept nsec for PPS phase adjustment, otherwise start time should be 0 0.\n"); | |
258 | return -EINVAL; | |
259 | } | |
260 | ||
261 | /* Calculate waveform high and low times */ | |
262 | if (rq->perout.flags & PTP_PEROUT_DUTY_CYCLE) { | |
263 | struct timespec64 ts_on; | |
264 | ||
265 | ts_on.tv_sec = rq->perout.on.sec; | |
266 | ts_on.tv_nsec = rq->perout.on.nsec; | |
267 | ||
268 | wf_high = timespec64_to_ns(&ts_on); | |
269 | } else { | |
270 | if (pps) { | |
271 | wf_high = 1000; | |
272 | } else { | |
273 | wf_high = timespec64_to_ns(&ts_period); | |
274 | wf_high = div_s64(wf_high, 2); | |
275 | } | |
276 | } | |
277 | ||
278 | wf_low = timespec64_to_ns(&ts_period); | |
279 | wf_low -= wf_high; | |
280 | ||
cc2d87bb YL |
281 | /* Handle PPS request */ |
282 | if (pps) { | |
283 | spin_lock_irqsave(&ocelot->ptp_clock_lock, flags); | |
ecf9f9b7 | 284 | ocelot_write_rix(ocelot, ts_phase.tv_nsec, |
cc2d87bb | 285 | PTP_PIN_WF_LOW_PERIOD, ptp_pin); |
ecf9f9b7 | 286 | ocelot_write_rix(ocelot, wf_high, |
cc2d87bb YL |
287 | PTP_PIN_WF_HIGH_PERIOD, ptp_pin); |
288 | val = PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_CLOCK); | |
289 | val |= PTP_PIN_CFG_SYNC; | |
290 | ocelot_write_rix(ocelot, val, PTP_PIN_CFG, ptp_pin); | |
291 | spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags); | |
292 | break; | |
293 | } | |
294 | ||
295 | /* Handle periodic clock */ | |
ecf9f9b7 VO |
296 | if (wf_high > 0x3fffffff || wf_high <= 0x6) |
297 | return -EINVAL; | |
298 | if (wf_low > 0x3fffffff || wf_low <= 0x6) | |
cc2d87bb YL |
299 | return -EINVAL; |
300 | ||
301 | spin_lock_irqsave(&ocelot->ptp_clock_lock, flags); | |
ecf9f9b7 VO |
302 | ocelot_write_rix(ocelot, wf_low, PTP_PIN_WF_LOW_PERIOD, |
303 | ptp_pin); | |
304 | ocelot_write_rix(ocelot, wf_high, PTP_PIN_WF_HIGH_PERIOD, | |
305 | ptp_pin); | |
cc2d87bb YL |
306 | val = PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_CLOCK); |
307 | ocelot_write_rix(ocelot, val, PTP_PIN_CFG, ptp_pin); | |
308 | spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags); | |
309 | break; | |
310 | default: | |
311 | return -EOPNOTSUPP; | |
312 | } | |
313 | return 0; | |
314 | } | |
315 | EXPORT_SYMBOL(ocelot_ptp_enable); | |
316 | ||
d50e41bf VO |
317 | static void ocelot_populate_l2_ptp_trap_key(struct ocelot_vcap_filter *trap) |
318 | { | |
319 | trap->key_type = OCELOT_VCAP_KEY_ETYPE; | |
320 | *(__be16 *)trap->key.etype.etype.value = htons(ETH_P_1588); | |
321 | *(__be16 *)trap->key.etype.etype.mask = htons(0xffff); | |
322 | } | |
323 | ||
324 | static void | |
325 | ocelot_populate_ipv4_ptp_event_trap_key(struct ocelot_vcap_filter *trap) | |
326 | { | |
327 | trap->key_type = OCELOT_VCAP_KEY_IPV4; | |
328 | trap->key.ipv4.proto.value[0] = IPPROTO_UDP; | |
329 | trap->key.ipv4.proto.mask[0] = 0xff; | |
330 | trap->key.ipv4.dport.value = PTP_EV_PORT; | |
331 | trap->key.ipv4.dport.mask = 0xffff; | |
332 | } | |
333 | ||
334 | static void | |
335 | ocelot_populate_ipv6_ptp_event_trap_key(struct ocelot_vcap_filter *trap) | |
336 | { | |
337 | trap->key_type = OCELOT_VCAP_KEY_IPV6; | |
2fcde9fe VO |
338 | trap->key.ipv6.proto.value[0] = IPPROTO_UDP; |
339 | trap->key.ipv6.proto.mask[0] = 0xff; | |
d50e41bf VO |
340 | trap->key.ipv6.dport.value = PTP_EV_PORT; |
341 | trap->key.ipv6.dport.mask = 0xffff; | |
342 | } | |
343 | ||
344 | static void | |
345 | ocelot_populate_ipv4_ptp_general_trap_key(struct ocelot_vcap_filter *trap) | |
346 | { | |
347 | trap->key_type = OCELOT_VCAP_KEY_IPV4; | |
348 | trap->key.ipv4.proto.value[0] = IPPROTO_UDP; | |
349 | trap->key.ipv4.proto.mask[0] = 0xff; | |
350 | trap->key.ipv4.dport.value = PTP_GEN_PORT; | |
351 | trap->key.ipv4.dport.mask = 0xffff; | |
352 | } | |
353 | ||
354 | static void | |
355 | ocelot_populate_ipv6_ptp_general_trap_key(struct ocelot_vcap_filter *trap) | |
356 | { | |
357 | trap->key_type = OCELOT_VCAP_KEY_IPV6; | |
2fcde9fe VO |
358 | trap->key.ipv6.proto.value[0] = IPPROTO_UDP; |
359 | trap->key.ipv6.proto.mask[0] = 0xff; | |
d50e41bf VO |
360 | trap->key.ipv6.dport.value = PTP_GEN_PORT; |
361 | trap->key.ipv6.dport.mask = 0xffff; | |
362 | } | |
363 | ||
364 | static int ocelot_l2_ptp_trap_add(struct ocelot *ocelot, int port) | |
365 | { | |
366 | unsigned long l2_cookie = OCELOT_VCAP_IS2_L2_PTP_TRAP(ocelot); | |
367 | ||
368 | return ocelot_trap_add(ocelot, port, l2_cookie, true, | |
369 | ocelot_populate_l2_ptp_trap_key); | |
370 | } | |
371 | ||
372 | static int ocelot_l2_ptp_trap_del(struct ocelot *ocelot, int port) | |
373 | { | |
374 | unsigned long l2_cookie = OCELOT_VCAP_IS2_L2_PTP_TRAP(ocelot); | |
375 | ||
376 | return ocelot_trap_del(ocelot, port, l2_cookie); | |
377 | } | |
378 | ||
379 | static int ocelot_ipv4_ptp_trap_add(struct ocelot *ocelot, int port) | |
380 | { | |
381 | unsigned long ipv4_gen_cookie = OCELOT_VCAP_IS2_IPV4_GEN_PTP_TRAP(ocelot); | |
382 | unsigned long ipv4_ev_cookie = OCELOT_VCAP_IS2_IPV4_EV_PTP_TRAP(ocelot); | |
383 | int err; | |
384 | ||
385 | err = ocelot_trap_add(ocelot, port, ipv4_ev_cookie, true, | |
386 | ocelot_populate_ipv4_ptp_event_trap_key); | |
387 | if (err) | |
388 | return err; | |
389 | ||
390 | err = ocelot_trap_add(ocelot, port, ipv4_gen_cookie, false, | |
391 | ocelot_populate_ipv4_ptp_general_trap_key); | |
392 | if (err) | |
393 | ocelot_trap_del(ocelot, port, ipv4_ev_cookie); | |
394 | ||
395 | return err; | |
396 | } | |
397 | ||
398 | static int ocelot_ipv4_ptp_trap_del(struct ocelot *ocelot, int port) | |
399 | { | |
400 | unsigned long ipv4_gen_cookie = OCELOT_VCAP_IS2_IPV4_GEN_PTP_TRAP(ocelot); | |
401 | unsigned long ipv4_ev_cookie = OCELOT_VCAP_IS2_IPV4_EV_PTP_TRAP(ocelot); | |
402 | int err; | |
403 | ||
404 | err = ocelot_trap_del(ocelot, port, ipv4_ev_cookie); | |
405 | err |= ocelot_trap_del(ocelot, port, ipv4_gen_cookie); | |
406 | return err; | |
407 | } | |
408 | ||
409 | static int ocelot_ipv6_ptp_trap_add(struct ocelot *ocelot, int port) | |
410 | { | |
411 | unsigned long ipv6_gen_cookie = OCELOT_VCAP_IS2_IPV6_GEN_PTP_TRAP(ocelot); | |
412 | unsigned long ipv6_ev_cookie = OCELOT_VCAP_IS2_IPV6_EV_PTP_TRAP(ocelot); | |
413 | int err; | |
414 | ||
415 | err = ocelot_trap_add(ocelot, port, ipv6_ev_cookie, true, | |
416 | ocelot_populate_ipv6_ptp_event_trap_key); | |
417 | if (err) | |
418 | return err; | |
419 | ||
420 | err = ocelot_trap_add(ocelot, port, ipv6_gen_cookie, false, | |
421 | ocelot_populate_ipv6_ptp_general_trap_key); | |
422 | if (err) | |
423 | ocelot_trap_del(ocelot, port, ipv6_ev_cookie); | |
424 | ||
425 | return err; | |
426 | } | |
427 | ||
428 | static int ocelot_ipv6_ptp_trap_del(struct ocelot *ocelot, int port) | |
429 | { | |
430 | unsigned long ipv6_gen_cookie = OCELOT_VCAP_IS2_IPV6_GEN_PTP_TRAP(ocelot); | |
431 | unsigned long ipv6_ev_cookie = OCELOT_VCAP_IS2_IPV6_EV_PTP_TRAP(ocelot); | |
432 | int err; | |
433 | ||
434 | err = ocelot_trap_del(ocelot, port, ipv6_ev_cookie); | |
435 | err |= ocelot_trap_del(ocelot, port, ipv6_gen_cookie); | |
436 | return err; | |
437 | } | |
438 | ||
439 | static int ocelot_setup_ptp_traps(struct ocelot *ocelot, int port, | |
440 | bool l2, bool l4) | |
441 | { | |
442 | int err; | |
443 | ||
444 | if (l2) | |
445 | err = ocelot_l2_ptp_trap_add(ocelot, port); | |
446 | else | |
447 | err = ocelot_l2_ptp_trap_del(ocelot, port); | |
448 | if (err) | |
449 | return err; | |
450 | ||
451 | if (l4) { | |
452 | err = ocelot_ipv4_ptp_trap_add(ocelot, port); | |
453 | if (err) | |
454 | goto err_ipv4; | |
455 | ||
456 | err = ocelot_ipv6_ptp_trap_add(ocelot, port); | |
457 | if (err) | |
458 | goto err_ipv6; | |
459 | } else { | |
460 | err = ocelot_ipv4_ptp_trap_del(ocelot, port); | |
461 | ||
462 | err |= ocelot_ipv6_ptp_trap_del(ocelot, port); | |
463 | } | |
464 | if (err) | |
465 | return err; | |
466 | ||
467 | return 0; | |
468 | ||
469 | err_ipv6: | |
470 | ocelot_ipv4_ptp_trap_del(ocelot, port); | |
471 | err_ipv4: | |
472 | if (l2) | |
473 | ocelot_l2_ptp_trap_del(ocelot, port); | |
474 | return err; | |
475 | } | |
476 | ||
477 | int ocelot_hwstamp_get(struct ocelot *ocelot, int port, struct ifreq *ifr) | |
478 | { | |
479 | return copy_to_user(ifr->ifr_data, &ocelot->hwtstamp_config, | |
480 | sizeof(ocelot->hwtstamp_config)) ? -EFAULT : 0; | |
481 | } | |
482 | EXPORT_SYMBOL(ocelot_hwstamp_get); | |
483 | ||
484 | int ocelot_hwstamp_set(struct ocelot *ocelot, int port, struct ifreq *ifr) | |
485 | { | |
486 | struct ocelot_port *ocelot_port = ocelot->ports[port]; | |
487 | bool l2 = false, l4 = false; | |
488 | struct hwtstamp_config cfg; | |
489 | int err; | |
490 | ||
491 | if (copy_from_user(&cfg, ifr->ifr_data, sizeof(cfg))) | |
492 | return -EFAULT; | |
493 | ||
494 | /* Tx type sanity check */ | |
495 | switch (cfg.tx_type) { | |
496 | case HWTSTAMP_TX_ON: | |
497 | ocelot_port->ptp_cmd = IFH_REW_OP_TWO_STEP_PTP; | |
498 | break; | |
499 | case HWTSTAMP_TX_ONESTEP_SYNC: | |
500 | /* IFH_REW_OP_ONE_STEP_PTP updates the correctional field, we | |
501 | * need to update the origin time. | |
502 | */ | |
503 | ocelot_port->ptp_cmd = IFH_REW_OP_ORIGIN_PTP; | |
504 | break; | |
505 | case HWTSTAMP_TX_OFF: | |
506 | ocelot_port->ptp_cmd = 0; | |
507 | break; | |
508 | default: | |
509 | return -ERANGE; | |
510 | } | |
511 | ||
512 | mutex_lock(&ocelot->ptp_lock); | |
513 | ||
514 | switch (cfg.rx_filter) { | |
515 | case HWTSTAMP_FILTER_NONE: | |
516 | break; | |
517 | case HWTSTAMP_FILTER_PTP_V2_L4_EVENT: | |
518 | case HWTSTAMP_FILTER_PTP_V2_L4_SYNC: | |
519 | case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ: | |
520 | l4 = true; | |
521 | break; | |
522 | case HWTSTAMP_FILTER_PTP_V2_L2_EVENT: | |
523 | case HWTSTAMP_FILTER_PTP_V2_L2_SYNC: | |
524 | case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ: | |
525 | l2 = true; | |
526 | break; | |
527 | case HWTSTAMP_FILTER_PTP_V2_EVENT: | |
528 | case HWTSTAMP_FILTER_PTP_V2_SYNC: | |
529 | case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ: | |
530 | l2 = true; | |
531 | l4 = true; | |
532 | break; | |
533 | default: | |
534 | mutex_unlock(&ocelot->ptp_lock); | |
535 | return -ERANGE; | |
536 | } | |
537 | ||
538 | err = ocelot_setup_ptp_traps(ocelot, port, l2, l4); | |
539 | if (err) { | |
540 | mutex_unlock(&ocelot->ptp_lock); | |
541 | return err; | |
542 | } | |
543 | ||
544 | if (l2 && l4) | |
545 | cfg.rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT; | |
546 | else if (l2) | |
547 | cfg.rx_filter = HWTSTAMP_FILTER_PTP_V2_L2_EVENT; | |
548 | else if (l4) | |
549 | cfg.rx_filter = HWTSTAMP_FILTER_PTP_V2_L4_EVENT; | |
550 | else | |
551 | cfg.rx_filter = HWTSTAMP_FILTER_NONE; | |
552 | ||
553 | /* Commit back the result & save it */ | |
554 | memcpy(&ocelot->hwtstamp_config, &cfg, sizeof(cfg)); | |
555 | mutex_unlock(&ocelot->ptp_lock); | |
556 | ||
557 | return copy_to_user(ifr->ifr_data, &cfg, sizeof(cfg)) ? -EFAULT : 0; | |
558 | } | |
559 | EXPORT_SYMBOL(ocelot_hwstamp_set); | |
560 | ||
561 | int ocelot_get_ts_info(struct ocelot *ocelot, int port, | |
562 | struct ethtool_ts_info *info) | |
563 | { | |
564 | info->phc_index = ocelot->ptp_clock ? | |
565 | ptp_clock_index(ocelot->ptp_clock) : -1; | |
566 | if (info->phc_index == -1) { | |
567 | info->so_timestamping |= SOF_TIMESTAMPING_TX_SOFTWARE | | |
568 | SOF_TIMESTAMPING_RX_SOFTWARE | | |
569 | SOF_TIMESTAMPING_SOFTWARE; | |
570 | return 0; | |
571 | } | |
572 | info->so_timestamping |= SOF_TIMESTAMPING_TX_SOFTWARE | | |
573 | SOF_TIMESTAMPING_RX_SOFTWARE | | |
574 | SOF_TIMESTAMPING_SOFTWARE | | |
575 | SOF_TIMESTAMPING_TX_HARDWARE | | |
576 | SOF_TIMESTAMPING_RX_HARDWARE | | |
577 | SOF_TIMESTAMPING_RAW_HARDWARE; | |
578 | info->tx_types = BIT(HWTSTAMP_TX_OFF) | BIT(HWTSTAMP_TX_ON) | | |
579 | BIT(HWTSTAMP_TX_ONESTEP_SYNC); | |
580 | info->rx_filters = BIT(HWTSTAMP_FILTER_NONE) | | |
581 | BIT(HWTSTAMP_FILTER_PTP_V2_EVENT) | | |
582 | BIT(HWTSTAMP_FILTER_PTP_V2_L2_EVENT) | | |
583 | BIT(HWTSTAMP_FILTER_PTP_V2_L4_EVENT); | |
584 | ||
585 | return 0; | |
586 | } | |
587 | EXPORT_SYMBOL(ocelot_get_ts_info); | |
588 | ||
589 | static int ocelot_port_add_txtstamp_skb(struct ocelot *ocelot, int port, | |
590 | struct sk_buff *clone) | |
591 | { | |
592 | struct ocelot_port *ocelot_port = ocelot->ports[port]; | |
593 | unsigned long flags; | |
594 | ||
595 | spin_lock_irqsave(&ocelot->ts_id_lock, flags); | |
596 | ||
597 | if (ocelot_port->ptp_skbs_in_flight == OCELOT_MAX_PTP_ID || | |
598 | ocelot->ptp_skbs_in_flight == OCELOT_PTP_FIFO_SIZE) { | |
599 | spin_unlock_irqrestore(&ocelot->ts_id_lock, flags); | |
600 | return -EBUSY; | |
601 | } | |
602 | ||
603 | skb_shinfo(clone)->tx_flags |= SKBTX_IN_PROGRESS; | |
604 | /* Store timestamp ID in OCELOT_SKB_CB(clone)->ts_id */ | |
605 | OCELOT_SKB_CB(clone)->ts_id = ocelot_port->ts_id; | |
606 | ||
607 | ocelot_port->ts_id++; | |
608 | if (ocelot_port->ts_id == OCELOT_MAX_PTP_ID) | |
609 | ocelot_port->ts_id = 0; | |
610 | ||
611 | ocelot_port->ptp_skbs_in_flight++; | |
612 | ocelot->ptp_skbs_in_flight++; | |
613 | ||
614 | skb_queue_tail(&ocelot_port->tx_skbs, clone); | |
615 | ||
616 | spin_unlock_irqrestore(&ocelot->ts_id_lock, flags); | |
617 | ||
618 | return 0; | |
619 | } | |
620 | ||
621 | static bool ocelot_ptp_is_onestep_sync(struct sk_buff *skb, | |
622 | unsigned int ptp_class) | |
623 | { | |
624 | struct ptp_header *hdr; | |
625 | u8 msgtype, twostep; | |
626 | ||
627 | hdr = ptp_parse_header(skb, ptp_class); | |
628 | if (!hdr) | |
629 | return false; | |
630 | ||
631 | msgtype = ptp_get_msgtype(hdr, ptp_class); | |
632 | twostep = hdr->flag_field[0] & 0x2; | |
633 | ||
634 | if (msgtype == PTP_MSGTYPE_SYNC && twostep == 0) | |
635 | return true; | |
636 | ||
637 | return false; | |
638 | } | |
639 | ||
640 | int ocelot_port_txtstamp_request(struct ocelot *ocelot, int port, | |
641 | struct sk_buff *skb, | |
642 | struct sk_buff **clone) | |
643 | { | |
644 | struct ocelot_port *ocelot_port = ocelot->ports[port]; | |
645 | u8 ptp_cmd = ocelot_port->ptp_cmd; | |
646 | unsigned int ptp_class; | |
647 | int err; | |
648 | ||
649 | /* Don't do anything if PTP timestamping not enabled */ | |
650 | if (!ptp_cmd) | |
651 | return 0; | |
652 | ||
653 | ptp_class = ptp_classify_raw(skb); | |
654 | if (ptp_class == PTP_CLASS_NONE) | |
655 | return -EINVAL; | |
656 | ||
657 | /* Store ptp_cmd in OCELOT_SKB_CB(skb)->ptp_cmd */ | |
658 | if (ptp_cmd == IFH_REW_OP_ORIGIN_PTP) { | |
659 | if (ocelot_ptp_is_onestep_sync(skb, ptp_class)) { | |
660 | OCELOT_SKB_CB(skb)->ptp_cmd = ptp_cmd; | |
661 | return 0; | |
662 | } | |
663 | ||
664 | /* Fall back to two-step timestamping */ | |
665 | ptp_cmd = IFH_REW_OP_TWO_STEP_PTP; | |
666 | } | |
667 | ||
668 | if (ptp_cmd == IFH_REW_OP_TWO_STEP_PTP) { | |
669 | *clone = skb_clone_sk(skb); | |
670 | if (!(*clone)) | |
671 | return -ENOMEM; | |
672 | ||
673 | err = ocelot_port_add_txtstamp_skb(ocelot, port, *clone); | |
674 | if (err) | |
675 | return err; | |
676 | ||
677 | OCELOT_SKB_CB(skb)->ptp_cmd = ptp_cmd; | |
678 | OCELOT_SKB_CB(*clone)->ptp_class = ptp_class; | |
679 | } | |
680 | ||
681 | return 0; | |
682 | } | |
683 | EXPORT_SYMBOL(ocelot_port_txtstamp_request); | |
684 | ||
685 | static void ocelot_get_hwtimestamp(struct ocelot *ocelot, | |
686 | struct timespec64 *ts) | |
687 | { | |
688 | unsigned long flags; | |
689 | u32 val; | |
690 | ||
691 | spin_lock_irqsave(&ocelot->ptp_clock_lock, flags); | |
692 | ||
693 | /* Read current PTP time to get seconds */ | |
694 | val = ocelot_read_rix(ocelot, PTP_PIN_CFG, TOD_ACC_PIN); | |
695 | ||
696 | val &= ~(PTP_PIN_CFG_SYNC | PTP_PIN_CFG_ACTION_MASK | PTP_PIN_CFG_DOM); | |
697 | val |= PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_SAVE); | |
698 | ocelot_write_rix(ocelot, val, PTP_PIN_CFG, TOD_ACC_PIN); | |
699 | ts->tv_sec = ocelot_read_rix(ocelot, PTP_PIN_TOD_SEC_LSB, TOD_ACC_PIN); | |
700 | ||
701 | /* Read packet HW timestamp from FIFO */ | |
702 | val = ocelot_read(ocelot, SYS_PTP_TXSTAMP); | |
703 | ts->tv_nsec = SYS_PTP_TXSTAMP_PTP_TXSTAMP(val); | |
704 | ||
705 | /* Sec has incremented since the ts was registered */ | |
706 | if ((ts->tv_sec & 0x1) != !!(val & SYS_PTP_TXSTAMP_PTP_TXSTAMP_SEC)) | |
707 | ts->tv_sec--; | |
708 | ||
709 | spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags); | |
710 | } | |
711 | ||
712 | static bool ocelot_validate_ptp_skb(struct sk_buff *clone, u16 seqid) | |
713 | { | |
714 | struct ptp_header *hdr; | |
715 | ||
716 | hdr = ptp_parse_header(clone, OCELOT_SKB_CB(clone)->ptp_class); | |
717 | if (WARN_ON(!hdr)) | |
718 | return false; | |
719 | ||
720 | return seqid == ntohs(hdr->sequence_id); | |
721 | } | |
722 | ||
723 | void ocelot_get_txtstamp(struct ocelot *ocelot) | |
724 | { | |
725 | int budget = OCELOT_PTP_QUEUE_SZ; | |
726 | ||
727 | while (budget--) { | |
728 | struct sk_buff *skb, *skb_tmp, *skb_match = NULL; | |
729 | struct skb_shared_hwtstamps shhwtstamps; | |
730 | u32 val, id, seqid, txport; | |
731 | struct ocelot_port *port; | |
732 | struct timespec64 ts; | |
733 | unsigned long flags; | |
734 | ||
735 | val = ocelot_read(ocelot, SYS_PTP_STATUS); | |
736 | ||
737 | /* Check if a timestamp can be retrieved */ | |
738 | if (!(val & SYS_PTP_STATUS_PTP_MESS_VLD)) | |
739 | break; | |
740 | ||
741 | WARN_ON(val & SYS_PTP_STATUS_PTP_OVFL); | |
742 | ||
743 | /* Retrieve the ts ID and Tx port */ | |
744 | id = SYS_PTP_STATUS_PTP_MESS_ID_X(val); | |
745 | txport = SYS_PTP_STATUS_PTP_MESS_TXPORT_X(val); | |
746 | seqid = SYS_PTP_STATUS_PTP_MESS_SEQ_ID(val); | |
747 | ||
748 | port = ocelot->ports[txport]; | |
749 | ||
750 | spin_lock(&ocelot->ts_id_lock); | |
751 | port->ptp_skbs_in_flight--; | |
752 | ocelot->ptp_skbs_in_flight--; | |
753 | spin_unlock(&ocelot->ts_id_lock); | |
754 | ||
755 | /* Retrieve its associated skb */ | |
756 | try_again: | |
757 | spin_lock_irqsave(&port->tx_skbs.lock, flags); | |
758 | ||
759 | skb_queue_walk_safe(&port->tx_skbs, skb, skb_tmp) { | |
760 | if (OCELOT_SKB_CB(skb)->ts_id != id) | |
761 | continue; | |
762 | __skb_unlink(skb, &port->tx_skbs); | |
763 | skb_match = skb; | |
764 | break; | |
765 | } | |
766 | ||
767 | spin_unlock_irqrestore(&port->tx_skbs.lock, flags); | |
768 | ||
769 | if (WARN_ON(!skb_match)) | |
770 | continue; | |
771 | ||
772 | if (!ocelot_validate_ptp_skb(skb_match, seqid)) { | |
773 | dev_err_ratelimited(ocelot->dev, | |
774 | "port %d received stale TX timestamp for seqid %d, discarding\n", | |
775 | txport, seqid); | |
776 | dev_kfree_skb_any(skb); | |
777 | goto try_again; | |
778 | } | |
779 | ||
780 | /* Get the h/w timestamp */ | |
781 | ocelot_get_hwtimestamp(ocelot, &ts); | |
782 | ||
783 | /* Set the timestamp into the skb */ | |
784 | memset(&shhwtstamps, 0, sizeof(shhwtstamps)); | |
785 | shhwtstamps.hwtstamp = ktime_set(ts.tv_sec, ts.tv_nsec); | |
786 | skb_complete_tx_timestamp(skb_match, &shhwtstamps); | |
787 | ||
788 | /* Next ts */ | |
789 | ocelot_write(ocelot, SYS_PTP_NXT_PTP_NXT, SYS_PTP_NXT); | |
790 | } | |
791 | } | |
792 | EXPORT_SYMBOL(ocelot_get_txtstamp); | |
793 | ||
881321b6 VO |
794 | int ocelot_init_timestamp(struct ocelot *ocelot, |
795 | const struct ptp_clock_info *info) | |
2b49d128 YL |
796 | { |
797 | struct ptp_clock *ptp_clock; | |
cc2d87bb | 798 | int i; |
2b49d128 YL |
799 | |
800 | ocelot->ptp_info = *info; | |
cc2d87bb YL |
801 | |
802 | for (i = 0; i < OCELOT_PTP_PINS_NUM; i++) { | |
803 | struct ptp_pin_desc *p = &ocelot->ptp_pins[i]; | |
804 | ||
805 | snprintf(p->name, sizeof(p->name), "switch_1588_dat%d", i); | |
806 | p->index = i; | |
807 | p->func = PTP_PF_NONE; | |
808 | } | |
809 | ||
810 | ocelot->ptp_info.pin_config = &ocelot->ptp_pins[0]; | |
811 | ||
2b49d128 YL |
812 | ptp_clock = ptp_clock_register(&ocelot->ptp_info, ocelot->dev); |
813 | if (IS_ERR(ptp_clock)) | |
814 | return PTR_ERR(ptp_clock); | |
815 | /* Check if PHC support is missing at the configuration level */ | |
816 | if (!ptp_clock) | |
817 | return 0; | |
818 | ||
819 | ocelot->ptp_clock = ptp_clock; | |
820 | ||
821 | ocelot_write(ocelot, SYS_PTP_CFG_PTP_STAMP_WID(30), SYS_PTP_CFG); | |
822 | ocelot_write(ocelot, 0xffffffff, ANA_TABLES_PTP_ID_LOW); | |
823 | ocelot_write(ocelot, 0xffffffff, ANA_TABLES_PTP_ID_HIGH); | |
824 | ||
825 | ocelot_write(ocelot, PTP_CFG_MISC_PTP_EN, PTP_CFG_MISC); | |
826 | ||
827 | /* There is no device reconfiguration, PTP Rx stamping is always | |
828 | * enabled. | |
829 | */ | |
830 | ocelot->hwtstamp_config.rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT; | |
831 | ||
832 | return 0; | |
833 | } | |
834 | EXPORT_SYMBOL(ocelot_init_timestamp); | |
835 | ||
836 | int ocelot_deinit_timestamp(struct ocelot *ocelot) | |
837 | { | |
838 | if (ocelot->ptp_clock) | |
839 | ptp_clock_unregister(ocelot->ptp_clock); | |
840 | return 0; | |
841 | } | |
842 | EXPORT_SYMBOL(ocelot_deinit_timestamp); |