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