1 // SPDX-License-Identifier: GPL-2.0+
2 /* Microchip Sparx5 Switch driver
3 *
4 * Copyright (c) 2021 Microchip Technology Inc. and its subsidiaries.
5 *
6 * The Sparx5 Chip Register Model can be browsed at this location:
7 * https://github.com/microchip-ung/sparx-5_reginfo
8 */
9 #include <linux/ptp_classify.h>
10
11 #include "sparx5_main_regs.h"
12 #include "sparx5_main.h"
13
14 #define TOD_ACC_PIN 0x4
15
16 enum {
17 PTP_PIN_ACTION_IDLE = 0,
18 PTP_PIN_ACTION_LOAD,
19 PTP_PIN_ACTION_SAVE,
20 PTP_PIN_ACTION_CLOCK,
21 PTP_PIN_ACTION_DELTA,
22 PTP_PIN_ACTION_TOD
23 };
24
sparx5_ptp_get_1ppm(struct sparx5 * sparx5)25 static u64 sparx5_ptp_get_1ppm(struct sparx5 *sparx5)
26 {
27 /* Represents 1ppm adjustment in 2^59 format with 1.59687500000(625)
28 * 1.99609375000(500), 3.99218750000(250) as reference
29 * The value is calculated as following:
30 * (1/1000000)/((2^-59)/X)
31 */
32
33 u64 res = 0;
34
35 switch (sparx5->coreclock) {
36 case SPX5_CORE_CLOCK_250MHZ:
37 res = 2301339409586;
38 break;
39 case SPX5_CORE_CLOCK_328MHZ:
40 res = 1756832768924;
41 break;
42 case SPX5_CORE_CLOCK_500MHZ:
43 res = 1150669704793;
44 break;
45 case SPX5_CORE_CLOCK_625MHZ:
46 res = 920535763834;
47 break;
48 default:
49 WARN(1, "Invalid core clock");
50 break;
51 }
52
53 return res;
54 }
55
sparx5_ptp_get_nominal_value(struct sparx5 * sparx5)56 static u64 sparx5_ptp_get_nominal_value(struct sparx5 *sparx5)
57 {
58 u64 res = 0;
59
60 switch (sparx5->coreclock) {
61 case SPX5_CORE_CLOCK_250MHZ:
62 res = 0x1FF0000000000000;
63 break;
64 case SPX5_CORE_CLOCK_328MHZ:
65 res = 0x18604697DD0F9B5B;
66 break;
67 case SPX5_CORE_CLOCK_500MHZ:
68 res = 0x0FF8000000000000;
69 break;
70 case SPX5_CORE_CLOCK_625MHZ:
71 res = 0x0CC6666666666666;
72 break;
73 default:
74 WARN(1, "Invalid core clock");
75 break;
76 }
77
78 return res;
79 }
80
sparx5_ptp_hwtstamp_set(struct sparx5_port * port,struct kernel_hwtstamp_config * cfg,struct netlink_ext_ack * extack)81 int sparx5_ptp_hwtstamp_set(struct sparx5_port *port,
82 struct kernel_hwtstamp_config *cfg,
83 struct netlink_ext_ack *extack)
84 {
85 struct sparx5 *sparx5 = port->sparx5;
86 struct sparx5_phc *phc;
87
88 /* For now don't allow to run ptp on ports that are part of a bridge,
89 * because in case of transparent clock the HW will still forward the
90 * frames, so there would be duplicate frames
91 */
92
93 if (test_bit(port->portno, sparx5->bridge_mask))
94 return -EINVAL;
95
96 switch (cfg->tx_type) {
97 case HWTSTAMP_TX_ON:
98 port->ptp_cmd = IFH_REW_OP_TWO_STEP_PTP;
99 break;
100 case HWTSTAMP_TX_ONESTEP_SYNC:
101 port->ptp_cmd = IFH_REW_OP_ONE_STEP_PTP;
102 break;
103 case HWTSTAMP_TX_OFF:
104 port->ptp_cmd = IFH_REW_OP_NOOP;
105 break;
106 default:
107 return -ERANGE;
108 }
109
110 switch (cfg->rx_filter) {
111 case HWTSTAMP_FILTER_NONE:
112 break;
113 case HWTSTAMP_FILTER_ALL:
114 case HWTSTAMP_FILTER_PTP_V1_L4_EVENT:
115 case HWTSTAMP_FILTER_PTP_V1_L4_SYNC:
116 case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ:
117 case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
118 case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
119 case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
120 case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
121 case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
122 case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
123 case HWTSTAMP_FILTER_PTP_V2_EVENT:
124 case HWTSTAMP_FILTER_PTP_V2_SYNC:
125 case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
126 case HWTSTAMP_FILTER_NTP_ALL:
127 cfg->rx_filter = HWTSTAMP_FILTER_ALL;
128 break;
129 default:
130 return -ERANGE;
131 }
132
133 /* Commit back the result & save it */
134 mutex_lock(&sparx5->ptp_lock);
135 phc = &sparx5->phc[SPARX5_PHC_PORT];
136 phc->hwtstamp_config = *cfg;
137 mutex_unlock(&sparx5->ptp_lock);
138
139 return 0;
140 }
141
sparx5_ptp_hwtstamp_get(struct sparx5_port * port,struct kernel_hwtstamp_config * cfg)142 void sparx5_ptp_hwtstamp_get(struct sparx5_port *port,
143 struct kernel_hwtstamp_config *cfg)
144 {
145 struct sparx5 *sparx5 = port->sparx5;
146 struct sparx5_phc *phc;
147
148 phc = &sparx5->phc[SPARX5_PHC_PORT];
149 *cfg = phc->hwtstamp_config;
150 }
151
sparx5_ptp_classify(struct sparx5_port * port,struct sk_buff * skb,u8 * rew_op,u8 * pdu_type,u8 * pdu_w16_offset)152 static void sparx5_ptp_classify(struct sparx5_port *port, struct sk_buff *skb,
153 u8 *rew_op, u8 *pdu_type, u8 *pdu_w16_offset)
154 {
155 struct ptp_header *header;
156 u8 msgtype;
157 int type;
158
159 if (port->ptp_cmd == IFH_REW_OP_NOOP) {
160 *rew_op = IFH_REW_OP_NOOP;
161 *pdu_type = IFH_PDU_TYPE_NONE;
162 *pdu_w16_offset = 0;
163 return;
164 }
165
166 type = ptp_classify_raw(skb);
167 if (type == PTP_CLASS_NONE) {
168 *rew_op = IFH_REW_OP_NOOP;
169 *pdu_type = IFH_PDU_TYPE_NONE;
170 *pdu_w16_offset = 0;
171 return;
172 }
173
174 header = ptp_parse_header(skb, type);
175 if (!header) {
176 *rew_op = IFH_REW_OP_NOOP;
177 *pdu_type = IFH_PDU_TYPE_NONE;
178 *pdu_w16_offset = 0;
179 return;
180 }
181
182 *pdu_w16_offset = 7;
183 if (type & PTP_CLASS_L2)
184 *pdu_type = IFH_PDU_TYPE_PTP;
185 if (type & PTP_CLASS_IPV4)
186 *pdu_type = IFH_PDU_TYPE_IPV4_UDP_PTP;
187 if (type & PTP_CLASS_IPV6)
188 *pdu_type = IFH_PDU_TYPE_IPV6_UDP_PTP;
189
190 if (port->ptp_cmd == IFH_REW_OP_TWO_STEP_PTP) {
191 *rew_op = IFH_REW_OP_TWO_STEP_PTP;
192 return;
193 }
194
195 /* If it is sync and run 1 step then set the correct operation,
196 * otherwise run as 2 step
197 */
198 msgtype = ptp_get_msgtype(header, type);
199 if ((msgtype & 0xf) == 0) {
200 *rew_op = IFH_REW_OP_ONE_STEP_PTP;
201 return;
202 }
203
204 *rew_op = IFH_REW_OP_TWO_STEP_PTP;
205 }
206
sparx5_ptp_txtstamp_old_release(struct sparx5_port * port)207 static void sparx5_ptp_txtstamp_old_release(struct sparx5_port *port)
208 {
209 struct sk_buff *skb, *skb_tmp;
210 unsigned long flags;
211
212 spin_lock_irqsave(&port->tx_skbs.lock, flags);
213 skb_queue_walk_safe(&port->tx_skbs, skb, skb_tmp) {
214 if time_after(SPARX5_SKB_CB(skb)->jiffies + SPARX5_PTP_TIMEOUT,
215 jiffies)
216 break;
217
218 __skb_unlink(skb, &port->tx_skbs);
219 dev_kfree_skb_any(skb);
220 }
221 spin_unlock_irqrestore(&port->tx_skbs.lock, flags);
222 }
223
sparx5_ptp_txtstamp_request(struct sparx5_port * port,struct sk_buff * skb)224 int sparx5_ptp_txtstamp_request(struct sparx5_port *port,
225 struct sk_buff *skb)
226 {
227 struct sparx5 *sparx5 = port->sparx5;
228 u8 rew_op, pdu_type, pdu_w16_offset;
229 unsigned long flags;
230
231 sparx5_ptp_classify(port, skb, &rew_op, &pdu_type, &pdu_w16_offset);
232 SPARX5_SKB_CB(skb)->rew_op = rew_op;
233 SPARX5_SKB_CB(skb)->pdu_type = pdu_type;
234 SPARX5_SKB_CB(skb)->pdu_w16_offset = pdu_w16_offset;
235
236 if (rew_op != IFH_REW_OP_TWO_STEP_PTP)
237 return 0;
238
239 sparx5_ptp_txtstamp_old_release(port);
240
241 spin_lock_irqsave(&sparx5->ptp_ts_id_lock, flags);
242 if (sparx5->ptp_skbs == SPARX5_MAX_PTP_ID) {
243 spin_unlock_irqrestore(&sparx5->ptp_ts_id_lock, flags);
244 return -EBUSY;
245 }
246
247 skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
248
249 skb_queue_tail(&port->tx_skbs, skb);
250 SPARX5_SKB_CB(skb)->ts_id = port->ts_id;
251 SPARX5_SKB_CB(skb)->jiffies = jiffies;
252
253 sparx5->ptp_skbs++;
254 port->ts_id++;
255 if (port->ts_id == SPARX5_MAX_PTP_ID)
256 port->ts_id = 0;
257
258 spin_unlock_irqrestore(&sparx5->ptp_ts_id_lock, flags);
259
260 return 0;
261 }
262
sparx5_ptp_txtstamp_release(struct sparx5_port * port,struct sk_buff * skb)263 void sparx5_ptp_txtstamp_release(struct sparx5_port *port,
264 struct sk_buff *skb)
265 {
266 struct sparx5 *sparx5 = port->sparx5;
267 unsigned long flags;
268
269 spin_lock_irqsave(&sparx5->ptp_ts_id_lock, flags);
270 port->ts_id--;
271 sparx5->ptp_skbs--;
272 skb_unlink(skb, &port->tx_skbs);
273 spin_unlock_irqrestore(&sparx5->ptp_ts_id_lock, flags);
274 }
275
sparx5_get_hwtimestamp(struct sparx5 * sparx5,struct timespec64 * ts,u32 nsec)276 void sparx5_get_hwtimestamp(struct sparx5 *sparx5,
277 struct timespec64 *ts,
278 u32 nsec)
279 {
280 /* Read current PTP time to get seconds */
281 const struct sparx5_consts *consts = sparx5->data->consts;
282 unsigned long flags;
283 u32 curr_nsec;
284
285 spin_lock_irqsave(&sparx5->ptp_clock_lock, flags);
286
287 spx5_rmw(PTP_PTP_PIN_CFG_PTP_PIN_ACTION_SET(PTP_PIN_ACTION_SAVE) |
288 PTP_PTP_PIN_CFG_PTP_PIN_DOM_SET(SPARX5_PHC_PORT) |
289 PTP_PTP_PIN_CFG_PTP_PIN_SYNC_SET(0),
290 PTP_PTP_PIN_CFG_PTP_PIN_ACTION |
291 PTP_PTP_PIN_CFG_PTP_PIN_DOM |
292 PTP_PTP_PIN_CFG_PTP_PIN_SYNC,
293 sparx5, PTP_PTP_PIN_CFG(consts->tod_pin));
294
295 ts->tv_sec = spx5_rd(sparx5, PTP_PTP_TOD_SEC_LSB(consts->tod_pin));
296 curr_nsec = spx5_rd(sparx5, PTP_PTP_TOD_NSEC(consts->tod_pin));
297
298 ts->tv_nsec = nsec;
299
300 /* Sec has incremented since the ts was registered */
301 if (curr_nsec < nsec)
302 ts->tv_sec--;
303
304 spin_unlock_irqrestore(&sparx5->ptp_clock_lock, flags);
305 }
306
sparx5_ptp_irq_handler(int irq,void * args)307 irqreturn_t sparx5_ptp_irq_handler(int irq, void *args)
308 {
309 int budget = SPARX5_MAX_PTP_ID;
310 struct sparx5 *sparx5 = args;
311
312 while (budget--) {
313 struct sk_buff *skb, *skb_tmp, *skb_match = NULL;
314 struct skb_shared_hwtstamps shhwtstamps;
315 struct sparx5_port *port;
316 struct timespec64 ts;
317 unsigned long flags;
318 u32 val, id, txport;
319 u32 delay;
320
321 val = spx5_rd(sparx5, REW_PTP_TWOSTEP_CTRL);
322
323 /* Check if a timestamp can be retrieved */
324 if (!(val & REW_PTP_TWOSTEP_CTRL_PTP_VLD))
325 break;
326
327 WARN_ON(val & REW_PTP_TWOSTEP_CTRL_PTP_OVFL);
328
329 if (!(val & REW_PTP_TWOSTEP_CTRL_STAMP_TX))
330 continue;
331
332 /* Retrieve the ts Tx port */
333 txport = REW_PTP_TWOSTEP_CTRL_STAMP_PORT_GET(val);
334
335 /* Retrieve its associated skb */
336 port = sparx5->ports[txport];
337
338 /* Retrieve the delay */
339 delay = spx5_rd(sparx5, REW_PTP_TWOSTEP_STAMP);
340 delay = REW_PTP_TWOSTEP_STAMP_STAMP_NSEC_GET(delay);
341
342 /* Get next timestamp from fifo, which needs to be the
343 * rx timestamp which represents the id of the frame
344 */
345 spx5_rmw(REW_PTP_TWOSTEP_CTRL_PTP_NXT_SET(1),
346 REW_PTP_TWOSTEP_CTRL_PTP_NXT,
347 sparx5, REW_PTP_TWOSTEP_CTRL);
348
349 val = spx5_rd(sparx5, REW_PTP_TWOSTEP_CTRL);
350
351 /* Check if a timestamp can be retried */
352 if (!(val & REW_PTP_TWOSTEP_CTRL_PTP_VLD))
353 break;
354
355 /* Read RX timestamping to get the ID */
356 id = spx5_rd(sparx5, REW_PTP_TWOSTEP_STAMP);
357 id <<= 8;
358 id |= spx5_rd(sparx5, REW_PTP_TWOSTEP_STAMP_SUBNS);
359
360 spin_lock_irqsave(&port->tx_skbs.lock, flags);
361 skb_queue_walk_safe(&port->tx_skbs, skb, skb_tmp) {
362 if (SPARX5_SKB_CB(skb)->ts_id != id)
363 continue;
364
365 __skb_unlink(skb, &port->tx_skbs);
366 skb_match = skb;
367 break;
368 }
369 spin_unlock_irqrestore(&port->tx_skbs.lock, flags);
370
371 /* Next ts */
372 spx5_rmw(REW_PTP_TWOSTEP_CTRL_PTP_NXT_SET(1),
373 REW_PTP_TWOSTEP_CTRL_PTP_NXT,
374 sparx5, REW_PTP_TWOSTEP_CTRL);
375
376 if (WARN_ON(!skb_match))
377 continue;
378
379 spin_lock(&sparx5->ptp_ts_id_lock);
380 sparx5->ptp_skbs--;
381 spin_unlock(&sparx5->ptp_ts_id_lock);
382
383 /* Get the h/w timestamp */
384 sparx5_get_hwtimestamp(sparx5, &ts, delay);
385
386 /* Set the timestamp into the skb */
387 shhwtstamps.hwtstamp = ktime_set(ts.tv_sec, ts.tv_nsec);
388 skb_tstamp_tx(skb_match, &shhwtstamps);
389
390 dev_kfree_skb_any(skb_match);
391 }
392
393 return IRQ_HANDLED;
394 }
395
sparx5_ptp_adjfine(struct ptp_clock_info * ptp,long scaled_ppm)396 static int sparx5_ptp_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)
397 {
398 struct sparx5_phc *phc = container_of(ptp, struct sparx5_phc, info);
399 struct sparx5 *sparx5 = phc->sparx5;
400 unsigned long flags;
401 bool neg_adj = 0;
402 u64 tod_inc;
403 u64 ref;
404
405 if (!scaled_ppm)
406 return 0;
407
408 if (scaled_ppm < 0) {
409 neg_adj = 1;
410 scaled_ppm = -scaled_ppm;
411 }
412
413 tod_inc = sparx5_ptp_get_nominal_value(sparx5);
414
415 /* The multiplication is split in 2 separate additions because of
416 * overflow issues. If scaled_ppm with 16bit fractional part was bigger
417 * than 20ppm then we got overflow.
418 */
419 ref = sparx5_ptp_get_1ppm(sparx5) * (scaled_ppm >> 16);
420 ref += (sparx5_ptp_get_1ppm(sparx5) * (0xffff & scaled_ppm)) >> 16;
421 tod_inc = neg_adj ? tod_inc - ref : tod_inc + ref;
422
423 spin_lock_irqsave(&sparx5->ptp_clock_lock, flags);
424
425 spx5_rmw(PTP_PTP_DOM_CFG_PTP_CLKCFG_DIS_SET(1 << BIT(phc->index)),
426 PTP_PTP_DOM_CFG_PTP_CLKCFG_DIS,
427 sparx5, PTP_PTP_DOM_CFG);
428
429 spx5_wr((u32)tod_inc & 0xFFFFFFFF, sparx5,
430 PTP_CLK_PER_CFG(phc->index, 0));
431 spx5_wr((u32)(tod_inc >> 32), sparx5,
432 PTP_CLK_PER_CFG(phc->index, 1));
433
434 spx5_rmw(PTP_PTP_DOM_CFG_PTP_CLKCFG_DIS_SET(0),
435 PTP_PTP_DOM_CFG_PTP_CLKCFG_DIS, sparx5,
436 PTP_PTP_DOM_CFG);
437
438 spin_unlock_irqrestore(&sparx5->ptp_clock_lock, flags);
439
440 return 0;
441 }
442
sparx5_ptp_settime64(struct ptp_clock_info * ptp,const struct timespec64 * ts)443 static int sparx5_ptp_settime64(struct ptp_clock_info *ptp,
444 const struct timespec64 *ts)
445 {
446 struct sparx5_phc *phc = container_of(ptp, struct sparx5_phc, info);
447 struct sparx5 *sparx5 = phc->sparx5;
448 const struct sparx5_consts *consts;
449 unsigned long flags;
450
451 consts = sparx5->data->consts;
452
453 spin_lock_irqsave(&sparx5->ptp_clock_lock, flags);
454
455 /* Must be in IDLE mode before the time can be loaded */
456 spx5_rmw(PTP_PTP_PIN_CFG_PTP_PIN_ACTION_SET(PTP_PIN_ACTION_IDLE) |
457 PTP_PTP_PIN_CFG_PTP_PIN_DOM_SET(phc->index) |
458 PTP_PTP_PIN_CFG_PTP_PIN_SYNC_SET(0),
459 PTP_PTP_PIN_CFG_PTP_PIN_ACTION |
460 PTP_PTP_PIN_CFG_PTP_PIN_DOM |
461 PTP_PTP_PIN_CFG_PTP_PIN_SYNC,
462 sparx5, PTP_PTP_PIN_CFG(consts->tod_pin));
463
464 /* Set new value */
465 spx5_wr(PTP_PTP_TOD_SEC_MSB_PTP_TOD_SEC_MSB_SET(upper_32_bits(ts->tv_sec)),
466 sparx5, PTP_PTP_TOD_SEC_MSB(consts->tod_pin));
467 spx5_wr(lower_32_bits(ts->tv_sec),
468 sparx5, PTP_PTP_TOD_SEC_LSB(consts->tod_pin));
469 spx5_wr(ts->tv_nsec, sparx5, PTP_PTP_TOD_NSEC(consts->tod_pin));
470
471 /* Apply new values */
472 spx5_rmw(PTP_PTP_PIN_CFG_PTP_PIN_ACTION_SET(PTP_PIN_ACTION_LOAD) |
473 PTP_PTP_PIN_CFG_PTP_PIN_DOM_SET(phc->index) |
474 PTP_PTP_PIN_CFG_PTP_PIN_SYNC_SET(0),
475 PTP_PTP_PIN_CFG_PTP_PIN_ACTION |
476 PTP_PTP_PIN_CFG_PTP_PIN_DOM |
477 PTP_PTP_PIN_CFG_PTP_PIN_SYNC,
478 sparx5, PTP_PTP_PIN_CFG(consts->tod_pin));
479
480 spin_unlock_irqrestore(&sparx5->ptp_clock_lock, flags);
481
482 return 0;
483 }
484
sparx5_ptp_gettime64(struct ptp_clock_info * ptp,struct timespec64 * ts)485 int sparx5_ptp_gettime64(struct ptp_clock_info *ptp, struct timespec64 *ts)
486 {
487 struct sparx5_phc *phc = container_of(ptp, struct sparx5_phc, info);
488 struct sparx5 *sparx5 = phc->sparx5;
489 const struct sparx5_consts *consts;
490 unsigned long flags;
491 time64_t s;
492 s64 ns;
493
494 consts = sparx5->data->consts;
495
496 spin_lock_irqsave(&sparx5->ptp_clock_lock, flags);
497
498 spx5_rmw(PTP_PTP_PIN_CFG_PTP_PIN_ACTION_SET(PTP_PIN_ACTION_SAVE) |
499 PTP_PTP_PIN_CFG_PTP_PIN_DOM_SET(phc->index) |
500 PTP_PTP_PIN_CFG_PTP_PIN_SYNC_SET(0),
501 PTP_PTP_PIN_CFG_PTP_PIN_ACTION |
502 PTP_PTP_PIN_CFG_PTP_PIN_DOM |
503 PTP_PTP_PIN_CFG_PTP_PIN_SYNC,
504 sparx5, PTP_PTP_PIN_CFG(consts->tod_pin));
505
506 s = spx5_rd(sparx5, PTP_PTP_TOD_SEC_MSB(consts->tod_pin));
507 s <<= 32;
508 s |= spx5_rd(sparx5, PTP_PTP_TOD_SEC_LSB(consts->tod_pin));
509 ns = spx5_rd(sparx5, PTP_PTP_TOD_NSEC(consts->tod_pin));
510 ns &= PTP_PTP_TOD_NSEC_PTP_TOD_NSEC;
511
512 spin_unlock_irqrestore(&sparx5->ptp_clock_lock, flags);
513
514 /* Deal with negative values */
515 if ((ns & 0xFFFFFFF0) == 0x3FFFFFF0) {
516 s--;
517 ns &= 0xf;
518 ns += 999999984;
519 }
520
521 set_normalized_timespec64(ts, s, ns);
522 return 0;
523 }
524
sparx5_ptp_adjtime(struct ptp_clock_info * ptp,s64 delta)525 static int sparx5_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
526 {
527 struct sparx5_phc *phc = container_of(ptp, struct sparx5_phc, info);
528 struct sparx5 *sparx5 = phc->sparx5;
529 const struct sparx5_consts *consts;
530
531 consts = sparx5->data->consts;
532
533 if (delta > -(NSEC_PER_SEC / 2) && delta < (NSEC_PER_SEC / 2)) {
534 unsigned long flags;
535
536 spin_lock_irqsave(&sparx5->ptp_clock_lock, flags);
537
538 /* Must be in IDLE mode before the time can be loaded */
539 spx5_rmw(PTP_PTP_PIN_CFG_PTP_PIN_ACTION_SET(PTP_PIN_ACTION_IDLE) |
540 PTP_PTP_PIN_CFG_PTP_PIN_DOM_SET(phc->index) |
541 PTP_PTP_PIN_CFG_PTP_PIN_SYNC_SET(0),
542 PTP_PTP_PIN_CFG_PTP_PIN_ACTION |
543 PTP_PTP_PIN_CFG_PTP_PIN_DOM |
544 PTP_PTP_PIN_CFG_PTP_PIN_SYNC,
545 sparx5, PTP_PTP_PIN_CFG(consts->tod_pin));
546
547 spx5_wr(PTP_PTP_TOD_NSEC_PTP_TOD_NSEC_SET(delta),
548 sparx5, PTP_PTP_TOD_NSEC(consts->tod_pin));
549
550 /* Adjust time with the value of PTP_TOD_NSEC */
551 spx5_rmw(PTP_PTP_PIN_CFG_PTP_PIN_ACTION_SET(PTP_PIN_ACTION_DELTA) |
552 PTP_PTP_PIN_CFG_PTP_PIN_DOM_SET(phc->index) |
553 PTP_PTP_PIN_CFG_PTP_PIN_SYNC_SET(0),
554 PTP_PTP_PIN_CFG_PTP_PIN_ACTION |
555 PTP_PTP_PIN_CFG_PTP_PIN_DOM |
556 PTP_PTP_PIN_CFG_PTP_PIN_SYNC,
557 sparx5, PTP_PTP_PIN_CFG(consts->tod_pin));
558
559 spin_unlock_irqrestore(&sparx5->ptp_clock_lock, flags);
560 } else {
561 /* Fall back using sparx5_ptp_settime64 which is not exact */
562 struct timespec64 ts;
563 u64 now;
564
565 sparx5_ptp_gettime64(ptp, &ts);
566
567 now = ktime_to_ns(timespec64_to_ktime(ts));
568 ts = ns_to_timespec64(now + delta);
569
570 sparx5_ptp_settime64(ptp, &ts);
571 }
572
573 return 0;
574 }
575
576 static struct ptp_clock_info sparx5_ptp_clock_info = {
577 .owner = THIS_MODULE,
578 .name = "sparx5 ptp",
579 .max_adj = 200000,
580 .gettime64 = sparx5_ptp_gettime64,
581 .settime64 = sparx5_ptp_settime64,
582 .adjtime = sparx5_ptp_adjtime,
583 .adjfine = sparx5_ptp_adjfine,
584 };
585
sparx5_ptp_phc_init(struct sparx5 * sparx5,int index,struct ptp_clock_info * clock_info)586 static int sparx5_ptp_phc_init(struct sparx5 *sparx5,
587 int index,
588 struct ptp_clock_info *clock_info)
589 {
590 struct sparx5_phc *phc = &sparx5->phc[index];
591
592 phc->info = *clock_info;
593 phc->clock = ptp_clock_register(&phc->info, sparx5->dev);
594 if (IS_ERR(phc->clock))
595 return PTR_ERR(phc->clock);
596
597 phc->index = index;
598 phc->sparx5 = sparx5;
599
600 /* PTP Rx stamping is always enabled. */
601 phc->hwtstamp_config.rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
602
603 return 0;
604 }
605
sparx5_ptp_init(struct sparx5 * sparx5)606 int sparx5_ptp_init(struct sparx5 *sparx5)
607 {
608 u64 tod_adj = sparx5_ptp_get_nominal_value(sparx5);
609 struct sparx5_port *port;
610 int err, i;
611
612 if (!sparx5->ptp)
613 return 0;
614
615 for (i = 0; i < SPARX5_PHC_COUNT; ++i) {
616 err = sparx5_ptp_phc_init(sparx5, i, &sparx5_ptp_clock_info);
617 if (err)
618 return err;
619 }
620
621 spin_lock_init(&sparx5->ptp_clock_lock);
622 spin_lock_init(&sparx5->ptp_ts_id_lock);
623 mutex_init(&sparx5->ptp_lock);
624
625 /* Disable master counters */
626 spx5_wr(PTP_PTP_DOM_CFG_PTP_ENA_SET(0), sparx5, PTP_PTP_DOM_CFG);
627
628 /* Configure the nominal TOD increment per clock cycle */
629 spx5_rmw(PTP_PTP_DOM_CFG_PTP_CLKCFG_DIS_SET(0x7),
630 PTP_PTP_DOM_CFG_PTP_CLKCFG_DIS,
631 sparx5, PTP_PTP_DOM_CFG);
632
633 for (i = 0; i < SPARX5_PHC_COUNT; ++i) {
634 spx5_wr((u32)tod_adj & 0xFFFFFFFF, sparx5,
635 PTP_CLK_PER_CFG(i, 0));
636 spx5_wr((u32)(tod_adj >> 32), sparx5,
637 PTP_CLK_PER_CFG(i, 1));
638 }
639
640 spx5_rmw(PTP_PTP_DOM_CFG_PTP_CLKCFG_DIS_SET(0),
641 PTP_PTP_DOM_CFG_PTP_CLKCFG_DIS,
642 sparx5, PTP_PTP_DOM_CFG);
643
644 /* Enable master counters */
645 spx5_wr(PTP_PTP_DOM_CFG_PTP_ENA_SET(0x7), sparx5, PTP_PTP_DOM_CFG);
646
647 for (i = 0; i < sparx5->data->consts->n_ports; i++) {
648 port = sparx5->ports[i];
649 if (!port)
650 continue;
651
652 skb_queue_head_init(&port->tx_skbs);
653 }
654
655 return 0;
656 }
657
sparx5_ptp_deinit(struct sparx5 * sparx5)658 void sparx5_ptp_deinit(struct sparx5 *sparx5)
659 {
660 struct sparx5_port *port;
661 int i;
662
663 for (i = 0; i < sparx5->data->consts->n_ports; i++) {
664 port = sparx5->ports[i];
665 if (!port)
666 continue;
667
668 skb_queue_purge(&port->tx_skbs);
669 }
670
671 for (i = 0; i < SPARX5_PHC_COUNT; ++i)
672 ptp_clock_unregister(sparx5->phc[i].clock);
673 }
674
sparx5_ptp_rxtstamp(struct sparx5 * sparx5,struct sk_buff * skb,u64 timestamp)675 void sparx5_ptp_rxtstamp(struct sparx5 *sparx5, struct sk_buff *skb,
676 u64 timestamp)
677 {
678 struct skb_shared_hwtstamps *shhwtstamps;
679 struct sparx5_phc *phc;
680 struct timespec64 ts;
681 u64 full_ts_in_ns;
682
683 if (!sparx5->ptp)
684 return;
685
686 phc = &sparx5->phc[SPARX5_PHC_PORT];
687 sparx5_ptp_gettime64(&phc->info, &ts);
688
689 if (ts.tv_nsec < timestamp)
690 ts.tv_sec--;
691 ts.tv_nsec = timestamp;
692 full_ts_in_ns = ktime_set(ts.tv_sec, ts.tv_nsec);
693
694 shhwtstamps = skb_hwtstamps(skb);
695 shhwtstamps->hwtstamp = full_ts_in_ns;
696 }
697