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