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 | */ |
7 | #include <linux/time64.h> |
8 | |
9 | #include <linux/dsa/ocelot.h> |
10 | #include <linux/ptp_classify.h> |
11 | #include <soc/mscc/ocelot_ptp.h> |
12 | #include <soc/mscc/ocelot_sys.h> |
13 | #include <soc/mscc/ocelot_vcap.h> |
14 | #include <soc/mscc/ocelot.h> |
15 | #include "ocelot.h" |
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(lock: &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, sec: s, nsec: 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(lock: &ocelot->ptp_clock_lock, flags); |
79 | |
80 | if (ocelot->ops->tas_clock_adjust) |
81 | ocelot->ops->tas_clock_adjust(ocelot); |
82 | |
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(lock: &ocelot->ptp_clock_lock, flags); |
116 | |
117 | if (ocelot->ops->tas_clock_adjust) |
118 | ocelot->ops->tas_clock_adjust(ocelot); |
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(kt: timespec64_to_ktime(ts)); |
127 | ts = ns_to_timespec64(nsec: now + delta); |
128 | |
129 | ocelot_ptp_settime64(ptp, &ts); |
130 | } |
131 | |
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(lock: &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(lock: &ocelot->ptp_clock_lock, flags); |
178 | return 0; |
179 | } |
180 | EXPORT_SYMBOL(ocelot_ptp_adjfine); |
181 | |
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); |
201 | struct timespec64 ts_phase, ts_period; |
202 | enum ocelot_ptp_pins ptp_pin; |
203 | unsigned long flags; |
204 | bool pps = false; |
205 | int pin = -1; |
206 | s64 wf_high; |
207 | s64 wf_low; |
208 | u32 val; |
209 | |
210 | switch (rq->type) { |
211 | case PTP_CLK_REQ_PEROUT: |
212 | /* Reject requests with unsupported flags */ |
213 | if (rq->perout.flags & ~(PTP_PEROUT_DUTY_CYCLE | |
214 | PTP_PEROUT_PHASE)) |
215 | return -EOPNOTSUPP; |
216 | |
217 | pin = ptp_find_pin(ptp: ocelot->ptp_clock, func: PTP_PF_PEROUT, |
218 | chan: 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 | |
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 | |
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(lock: &ocelot->ptp_clock_lock, flags); |
242 | break; |
243 | } |
244 | |
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: &ts_on); |
269 | } else { |
270 | if (pps) { |
271 | wf_high = 1000; |
272 | } else { |
273 | wf_high = timespec64_to_ns(ts: &ts_period); |
274 | wf_high = div_s64(dividend: wf_high, divisor: 2); |
275 | } |
276 | } |
277 | |
278 | wf_low = timespec64_to_ns(ts: &ts_period); |
279 | wf_low -= wf_high; |
280 | |
281 | /* Handle PPS request */ |
282 | if (pps) { |
283 | spin_lock_irqsave(&ocelot->ptp_clock_lock, flags); |
284 | ocelot_write_rix(ocelot, ts_phase.tv_nsec, |
285 | PTP_PIN_WF_LOW_PERIOD, ptp_pin); |
286 | ocelot_write_rix(ocelot, wf_high, |
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(lock: &ocelot->ptp_clock_lock, flags); |
292 | break; |
293 | } |
294 | |
295 | /* Handle periodic clock */ |
296 | if (wf_high > 0x3fffffff || wf_high <= 0x6) |
297 | return -EINVAL; |
298 | if (wf_low > 0x3fffffff || wf_low <= 0x6) |
299 | return -EINVAL; |
300 | |
301 | spin_lock_irqsave(&ocelot->ptp_clock_lock, flags); |
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); |
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(lock: &ocelot->ptp_clock_lock, flags); |
309 | break; |
310 | default: |
311 | return -EOPNOTSUPP; |
312 | } |
313 | return 0; |
314 | } |
315 | EXPORT_SYMBOL(ocelot_ptp_enable); |
316 | |
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; |
338 | trap->key.ipv6.proto.value[0] = IPPROTO_UDP; |
339 | trap->key.ipv6.proto.mask[0] = 0xff; |
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; |
358 | trap->key.ipv6.proto.value[0] = IPPROTO_UDP; |
359 | trap->key.ipv6.proto.mask[0] = 0xff; |
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, cookie: l2_cookie, take_ts: true, |
369 | populate: 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, cookie: 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, cookie: ipv4_ev_cookie, take_ts: true, |
386 | populate: ocelot_populate_ipv4_ptp_event_trap_key); |
387 | if (err) |
388 | return err; |
389 | |
390 | err = ocelot_trap_add(ocelot, port, cookie: ipv4_gen_cookie, take_ts: false, |
391 | populate: ocelot_populate_ipv4_ptp_general_trap_key); |
392 | if (err) |
393 | ocelot_trap_del(ocelot, port, cookie: 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, cookie: ipv4_ev_cookie); |
405 | err |= ocelot_trap_del(ocelot, port, cookie: 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, cookie: ipv6_ev_cookie, take_ts: true, |
416 | populate: ocelot_populate_ipv6_ptp_event_trap_key); |
417 | if (err) |
418 | return err; |
419 | |
420 | err = ocelot_trap_add(ocelot, port, cookie: ipv6_gen_cookie, take_ts: false, |
421 | populate: ocelot_populate_ipv6_ptp_general_trap_key); |
422 | if (err) |
423 | ocelot_trap_del(ocelot, port, cookie: 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, cookie: ipv6_ev_cookie); |
435 | err |= ocelot_trap_del(ocelot, port, cookie: 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 | struct ocelot_port *ocelot_port = ocelot->ports[port]; |
443 | int err; |
444 | |
445 | ocelot_port->trap_proto &= ~(OCELOT_PROTO_PTP_L2 | |
446 | OCELOT_PROTO_PTP_L4); |
447 | |
448 | if (l2) |
449 | err = ocelot_l2_ptp_trap_add(ocelot, port); |
450 | else |
451 | err = ocelot_l2_ptp_trap_del(ocelot, port); |
452 | if (err) |
453 | return err; |
454 | |
455 | if (l4) { |
456 | err = ocelot_ipv4_ptp_trap_add(ocelot, port); |
457 | if (err) |
458 | goto err_ipv4; |
459 | |
460 | err = ocelot_ipv6_ptp_trap_add(ocelot, port); |
461 | if (err) |
462 | goto err_ipv6; |
463 | } else { |
464 | err = ocelot_ipv4_ptp_trap_del(ocelot, port); |
465 | |
466 | err |= ocelot_ipv6_ptp_trap_del(ocelot, port); |
467 | } |
468 | if (err) |
469 | return err; |
470 | |
471 | if (l2) |
472 | ocelot_port->trap_proto |= OCELOT_PROTO_PTP_L2; |
473 | if (l4) |
474 | ocelot_port->trap_proto |= OCELOT_PROTO_PTP_L4; |
475 | |
476 | return 0; |
477 | |
478 | err_ipv6: |
479 | ocelot_ipv4_ptp_trap_del(ocelot, port); |
480 | err_ipv4: |
481 | if (l2) |
482 | ocelot_l2_ptp_trap_del(ocelot, port); |
483 | return err; |
484 | } |
485 | |
486 | static int ocelot_traps_to_ptp_rx_filter(unsigned int proto) |
487 | { |
488 | if ((proto & OCELOT_PROTO_PTP_L2) && (proto & OCELOT_PROTO_PTP_L4)) |
489 | return HWTSTAMP_FILTER_PTP_V2_EVENT; |
490 | else if (proto & OCELOT_PROTO_PTP_L2) |
491 | return HWTSTAMP_FILTER_PTP_V2_L2_EVENT; |
492 | else if (proto & OCELOT_PROTO_PTP_L4) |
493 | return HWTSTAMP_FILTER_PTP_V2_L4_EVENT; |
494 | |
495 | return HWTSTAMP_FILTER_NONE; |
496 | } |
497 | |
498 | int ocelot_hwstamp_get(struct ocelot *ocelot, int port, struct ifreq *ifr) |
499 | { |
500 | struct ocelot_port *ocelot_port = ocelot->ports[port]; |
501 | struct hwtstamp_config cfg = {}; |
502 | |
503 | switch (ocelot_port->ptp_cmd) { |
504 | case IFH_REW_OP_TWO_STEP_PTP: |
505 | cfg.tx_type = HWTSTAMP_TX_ON; |
506 | break; |
507 | case IFH_REW_OP_ORIGIN_PTP: |
508 | cfg.tx_type = HWTSTAMP_TX_ONESTEP_SYNC; |
509 | break; |
510 | default: |
511 | cfg.tx_type = HWTSTAMP_TX_OFF; |
512 | break; |
513 | } |
514 | |
515 | cfg.rx_filter = ocelot_traps_to_ptp_rx_filter(proto: ocelot_port->trap_proto); |
516 | |
517 | return copy_to_user(to: ifr->ifr_data, from: &cfg, n: sizeof(cfg)) ? -EFAULT : 0; |
518 | } |
519 | EXPORT_SYMBOL(ocelot_hwstamp_get); |
520 | |
521 | int ocelot_hwstamp_set(struct ocelot *ocelot, int port, struct ifreq *ifr) |
522 | { |
523 | struct ocelot_port *ocelot_port = ocelot->ports[port]; |
524 | bool l2 = false, l4 = false; |
525 | struct hwtstamp_config cfg; |
526 | int err; |
527 | |
528 | if (copy_from_user(to: &cfg, from: ifr->ifr_data, n: sizeof(cfg))) |
529 | return -EFAULT; |
530 | |
531 | /* Tx type sanity check */ |
532 | switch (cfg.tx_type) { |
533 | case HWTSTAMP_TX_ON: |
534 | ocelot_port->ptp_cmd = IFH_REW_OP_TWO_STEP_PTP; |
535 | break; |
536 | case HWTSTAMP_TX_ONESTEP_SYNC: |
537 | /* IFH_REW_OP_ONE_STEP_PTP updates the correctional field, we |
538 | * need to update the origin time. |
539 | */ |
540 | ocelot_port->ptp_cmd = IFH_REW_OP_ORIGIN_PTP; |
541 | break; |
542 | case HWTSTAMP_TX_OFF: |
543 | ocelot_port->ptp_cmd = 0; |
544 | break; |
545 | default: |
546 | return -ERANGE; |
547 | } |
548 | |
549 | switch (cfg.rx_filter) { |
550 | case HWTSTAMP_FILTER_NONE: |
551 | break; |
552 | case HWTSTAMP_FILTER_PTP_V2_L4_EVENT: |
553 | case HWTSTAMP_FILTER_PTP_V2_L4_SYNC: |
554 | case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ: |
555 | l4 = true; |
556 | break; |
557 | case HWTSTAMP_FILTER_PTP_V2_L2_EVENT: |
558 | case HWTSTAMP_FILTER_PTP_V2_L2_SYNC: |
559 | case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ: |
560 | l2 = true; |
561 | break; |
562 | case HWTSTAMP_FILTER_PTP_V2_EVENT: |
563 | case HWTSTAMP_FILTER_PTP_V2_SYNC: |
564 | case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ: |
565 | l2 = true; |
566 | l4 = true; |
567 | break; |
568 | default: |
569 | return -ERANGE; |
570 | } |
571 | |
572 | err = ocelot_setup_ptp_traps(ocelot, port, l2, l4); |
573 | if (err) |
574 | return err; |
575 | |
576 | cfg.rx_filter = ocelot_traps_to_ptp_rx_filter(proto: ocelot_port->trap_proto); |
577 | |
578 | return copy_to_user(to: ifr->ifr_data, from: &cfg, n: sizeof(cfg)) ? -EFAULT : 0; |
579 | } |
580 | EXPORT_SYMBOL(ocelot_hwstamp_set); |
581 | |
582 | int ocelot_get_ts_info(struct ocelot *ocelot, int port, |
583 | struct ethtool_ts_info *info) |
584 | { |
585 | info->phc_index = ocelot->ptp_clock ? |
586 | ptp_clock_index(ptp: ocelot->ptp_clock) : -1; |
587 | if (info->phc_index == -1) { |
588 | info->so_timestamping |= SOF_TIMESTAMPING_TX_SOFTWARE | |
589 | SOF_TIMESTAMPING_RX_SOFTWARE | |
590 | SOF_TIMESTAMPING_SOFTWARE; |
591 | return 0; |
592 | } |
593 | info->so_timestamping |= SOF_TIMESTAMPING_TX_SOFTWARE | |
594 | SOF_TIMESTAMPING_RX_SOFTWARE | |
595 | SOF_TIMESTAMPING_SOFTWARE | |
596 | SOF_TIMESTAMPING_TX_HARDWARE | |
597 | SOF_TIMESTAMPING_RX_HARDWARE | |
598 | SOF_TIMESTAMPING_RAW_HARDWARE; |
599 | info->tx_types = BIT(HWTSTAMP_TX_OFF) | BIT(HWTSTAMP_TX_ON) | |
600 | BIT(HWTSTAMP_TX_ONESTEP_SYNC); |
601 | info->rx_filters = BIT(HWTSTAMP_FILTER_NONE) | |
602 | BIT(HWTSTAMP_FILTER_PTP_V2_EVENT) | |
603 | BIT(HWTSTAMP_FILTER_PTP_V2_L2_EVENT) | |
604 | BIT(HWTSTAMP_FILTER_PTP_V2_L4_EVENT); |
605 | |
606 | return 0; |
607 | } |
608 | EXPORT_SYMBOL(ocelot_get_ts_info); |
609 | |
610 | static int ocelot_port_add_txtstamp_skb(struct ocelot *ocelot, int port, |
611 | struct sk_buff *clone) |
612 | { |
613 | struct ocelot_port *ocelot_port = ocelot->ports[port]; |
614 | unsigned long flags; |
615 | |
616 | spin_lock_irqsave(&ocelot->ts_id_lock, flags); |
617 | |
618 | if (ocelot_port->ptp_skbs_in_flight == OCELOT_MAX_PTP_ID || |
619 | ocelot->ptp_skbs_in_flight == OCELOT_PTP_FIFO_SIZE) { |
620 | spin_unlock_irqrestore(lock: &ocelot->ts_id_lock, flags); |
621 | return -EBUSY; |
622 | } |
623 | |
624 | skb_shinfo(clone)->tx_flags |= SKBTX_IN_PROGRESS; |
625 | /* Store timestamp ID in OCELOT_SKB_CB(clone)->ts_id */ |
626 | OCELOT_SKB_CB(clone)->ts_id = ocelot_port->ts_id; |
627 | |
628 | ocelot_port->ts_id++; |
629 | if (ocelot_port->ts_id == OCELOT_MAX_PTP_ID) |
630 | ocelot_port->ts_id = 0; |
631 | |
632 | ocelot_port->ptp_skbs_in_flight++; |
633 | ocelot->ptp_skbs_in_flight++; |
634 | |
635 | skb_queue_tail(list: &ocelot_port->tx_skbs, newsk: clone); |
636 | |
637 | spin_unlock_irqrestore(lock: &ocelot->ts_id_lock, flags); |
638 | |
639 | return 0; |
640 | } |
641 | |
642 | static bool ocelot_ptp_is_onestep_sync(struct sk_buff *skb, |
643 | unsigned int ptp_class) |
644 | { |
645 | struct ptp_header *hdr; |
646 | u8 msgtype, twostep; |
647 | |
648 | hdr = ptp_parse_header(skb, type: ptp_class); |
649 | if (!hdr) |
650 | return false; |
651 | |
652 | msgtype = ptp_get_msgtype(hdr, type: ptp_class); |
653 | twostep = hdr->flag_field[0] & 0x2; |
654 | |
655 | if (msgtype == PTP_MSGTYPE_SYNC && twostep == 0) |
656 | return true; |
657 | |
658 | return false; |
659 | } |
660 | |
661 | int ocelot_port_txtstamp_request(struct ocelot *ocelot, int port, |
662 | struct sk_buff *skb, |
663 | struct sk_buff **clone) |
664 | { |
665 | struct ocelot_port *ocelot_port = ocelot->ports[port]; |
666 | u8 ptp_cmd = ocelot_port->ptp_cmd; |
667 | unsigned int ptp_class; |
668 | int err; |
669 | |
670 | /* Don't do anything if PTP timestamping not enabled */ |
671 | if (!ptp_cmd) |
672 | return 0; |
673 | |
674 | ptp_class = ptp_classify_raw(skb); |
675 | if (ptp_class == PTP_CLASS_NONE) |
676 | return -EINVAL; |
677 | |
678 | /* Store ptp_cmd in OCELOT_SKB_CB(skb)->ptp_cmd */ |
679 | if (ptp_cmd == IFH_REW_OP_ORIGIN_PTP) { |
680 | if (ocelot_ptp_is_onestep_sync(skb, ptp_class)) { |
681 | OCELOT_SKB_CB(skb)->ptp_cmd = ptp_cmd; |
682 | return 0; |
683 | } |
684 | |
685 | /* Fall back to two-step timestamping */ |
686 | ptp_cmd = IFH_REW_OP_TWO_STEP_PTP; |
687 | } |
688 | |
689 | if (ptp_cmd == IFH_REW_OP_TWO_STEP_PTP) { |
690 | *clone = skb_clone_sk(skb); |
691 | if (!(*clone)) |
692 | return -ENOMEM; |
693 | |
694 | err = ocelot_port_add_txtstamp_skb(ocelot, port, clone: *clone); |
695 | if (err) |
696 | return err; |
697 | |
698 | OCELOT_SKB_CB(skb)->ptp_cmd = ptp_cmd; |
699 | OCELOT_SKB_CB(*clone)->ptp_class = ptp_class; |
700 | } |
701 | |
702 | return 0; |
703 | } |
704 | EXPORT_SYMBOL(ocelot_port_txtstamp_request); |
705 | |
706 | static void ocelot_get_hwtimestamp(struct ocelot *ocelot, |
707 | struct timespec64 *ts) |
708 | { |
709 | unsigned long flags; |
710 | u32 val; |
711 | |
712 | spin_lock_irqsave(&ocelot->ptp_clock_lock, flags); |
713 | |
714 | /* Read current PTP time to get seconds */ |
715 | val = ocelot_read_rix(ocelot, PTP_PIN_CFG, TOD_ACC_PIN); |
716 | |
717 | val &= ~(PTP_PIN_CFG_SYNC | PTP_PIN_CFG_ACTION_MASK | PTP_PIN_CFG_DOM); |
718 | val |= PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_SAVE); |
719 | ocelot_write_rix(ocelot, val, PTP_PIN_CFG, TOD_ACC_PIN); |
720 | ts->tv_sec = ocelot_read_rix(ocelot, PTP_PIN_TOD_SEC_LSB, TOD_ACC_PIN); |
721 | |
722 | /* Read packet HW timestamp from FIFO */ |
723 | val = ocelot_read(ocelot, SYS_PTP_TXSTAMP); |
724 | ts->tv_nsec = SYS_PTP_TXSTAMP_PTP_TXSTAMP(val); |
725 | |
726 | /* Sec has incremented since the ts was registered */ |
727 | if ((ts->tv_sec & 0x1) != !!(val & SYS_PTP_TXSTAMP_PTP_TXSTAMP_SEC)) |
728 | ts->tv_sec--; |
729 | |
730 | spin_unlock_irqrestore(lock: &ocelot->ptp_clock_lock, flags); |
731 | } |
732 | |
733 | static bool ocelot_validate_ptp_skb(struct sk_buff *clone, u16 seqid) |
734 | { |
735 | struct ptp_header *hdr; |
736 | |
737 | hdr = ptp_parse_header(skb: clone, OCELOT_SKB_CB(clone)->ptp_class); |
738 | if (WARN_ON(!hdr)) |
739 | return false; |
740 | |
741 | return seqid == ntohs(hdr->sequence_id); |
742 | } |
743 | |
744 | void ocelot_get_txtstamp(struct ocelot *ocelot) |
745 | { |
746 | int budget = OCELOT_PTP_QUEUE_SZ; |
747 | |
748 | while (budget--) { |
749 | struct sk_buff *skb, *skb_tmp, *skb_match = NULL; |
750 | struct skb_shared_hwtstamps shhwtstamps; |
751 | u32 val, id, seqid, txport; |
752 | struct ocelot_port *port; |
753 | struct timespec64 ts; |
754 | unsigned long flags; |
755 | |
756 | val = ocelot_read(ocelot, SYS_PTP_STATUS); |
757 | |
758 | /* Check if a timestamp can be retrieved */ |
759 | if (!(val & SYS_PTP_STATUS_PTP_MESS_VLD)) |
760 | break; |
761 | |
762 | WARN_ON(val & SYS_PTP_STATUS_PTP_OVFL); |
763 | |
764 | /* Retrieve the ts ID and Tx port */ |
765 | id = SYS_PTP_STATUS_PTP_MESS_ID_X(val); |
766 | txport = SYS_PTP_STATUS_PTP_MESS_TXPORT_X(val); |
767 | seqid = SYS_PTP_STATUS_PTP_MESS_SEQ_ID(val); |
768 | |
769 | port = ocelot->ports[txport]; |
770 | |
771 | spin_lock(lock: &ocelot->ts_id_lock); |
772 | port->ptp_skbs_in_flight--; |
773 | ocelot->ptp_skbs_in_flight--; |
774 | spin_unlock(lock: &ocelot->ts_id_lock); |
775 | |
776 | /* Retrieve its associated skb */ |
777 | try_again: |
778 | spin_lock_irqsave(&port->tx_skbs.lock, flags); |
779 | |
780 | skb_queue_walk_safe(&port->tx_skbs, skb, skb_tmp) { |
781 | if (OCELOT_SKB_CB(skb)->ts_id != id) |
782 | continue; |
783 | __skb_unlink(skb, list: &port->tx_skbs); |
784 | skb_match = skb; |
785 | break; |
786 | } |
787 | |
788 | spin_unlock_irqrestore(lock: &port->tx_skbs.lock, flags); |
789 | |
790 | if (WARN_ON(!skb_match)) |
791 | continue; |
792 | |
793 | if (!ocelot_validate_ptp_skb(clone: skb_match, seqid)) { |
794 | dev_err_ratelimited(ocelot->dev, |
795 | "port %d received stale TX timestamp for seqid %d, discarding\n" , |
796 | txport, seqid); |
797 | dev_kfree_skb_any(skb); |
798 | goto try_again; |
799 | } |
800 | |
801 | /* Get the h/w timestamp */ |
802 | ocelot_get_hwtimestamp(ocelot, ts: &ts); |
803 | |
804 | /* Set the timestamp into the skb */ |
805 | memset(&shhwtstamps, 0, sizeof(shhwtstamps)); |
806 | shhwtstamps.hwtstamp = ktime_set(secs: ts.tv_sec, nsecs: ts.tv_nsec); |
807 | skb_complete_tx_timestamp(skb: skb_match, hwtstamps: &shhwtstamps); |
808 | |
809 | /* Next ts */ |
810 | ocelot_write(ocelot, SYS_PTP_NXT_PTP_NXT, SYS_PTP_NXT); |
811 | } |
812 | } |
813 | EXPORT_SYMBOL(ocelot_get_txtstamp); |
814 | |
815 | int ocelot_init_timestamp(struct ocelot *ocelot, |
816 | const struct ptp_clock_info *info) |
817 | { |
818 | struct ptp_clock *ptp_clock; |
819 | int i; |
820 | |
821 | ocelot->ptp_info = *info; |
822 | |
823 | for (i = 0; i < OCELOT_PTP_PINS_NUM; i++) { |
824 | struct ptp_pin_desc *p = &ocelot->ptp_pins[i]; |
825 | |
826 | snprintf(buf: p->name, size: sizeof(p->name), fmt: "switch_1588_dat%d" , i); |
827 | p->index = i; |
828 | p->func = PTP_PF_NONE; |
829 | } |
830 | |
831 | ocelot->ptp_info.pin_config = &ocelot->ptp_pins[0]; |
832 | |
833 | ptp_clock = ptp_clock_register(info: &ocelot->ptp_info, parent: ocelot->dev); |
834 | if (IS_ERR(ptr: ptp_clock)) |
835 | return PTR_ERR(ptr: ptp_clock); |
836 | /* Check if PHC support is missing at the configuration level */ |
837 | if (!ptp_clock) |
838 | return 0; |
839 | |
840 | ocelot->ptp_clock = ptp_clock; |
841 | |
842 | ocelot_write(ocelot, SYS_PTP_CFG_PTP_STAMP_WID(30), SYS_PTP_CFG); |
843 | ocelot_write(ocelot, 0xffffffff, ANA_TABLES_PTP_ID_LOW); |
844 | ocelot_write(ocelot, 0xffffffff, ANA_TABLES_PTP_ID_HIGH); |
845 | |
846 | ocelot_write(ocelot, PTP_CFG_MISC_PTP_EN, PTP_CFG_MISC); |
847 | |
848 | return 0; |
849 | } |
850 | EXPORT_SYMBOL(ocelot_init_timestamp); |
851 | |
852 | int ocelot_deinit_timestamp(struct ocelot *ocelot) |
853 | { |
854 | if (ocelot->ptp_clock) |
855 | ptp_clock_unregister(ptp: ocelot->ptp_clock); |
856 | return 0; |
857 | } |
858 | EXPORT_SYMBOL(ocelot_deinit_timestamp); |
859 | |