1 // SPDX-License-Identifier: GPL-2.0+
2
3 #include <linux/ptp_classify.h>
4
5 #include "lan966x_main.h"
6 #include "vcap_api.h"
7 #include "vcap_api_client.h"
8
9 #define LAN966X_MAX_PTP_ID 512
10
11 /* Represents 1ppm adjustment in 2^59 format with 6.037735849ns as reference
12 * The value is calculated as following: (1/1000000)/((2^-59)/6.037735849)
13 */
14 #define LAN966X_1PPM_FORMAT 3480517749723LL
15
16 /* Represents 1ppb adjustment in 2^29 format with 6.037735849ns as reference
17 * The value is calculated as following: (1/1000000000)/((2^59)/6.037735849)
18 */
19 #define LAN966X_1PPB_FORMAT 3480517749LL
20
21 #define TOD_ACC_PIN 0x7
22
23 /* This represents the base rule ID for the PTP rules that are added in the
24 * VCAP to trap frames to CPU. This number needs to be bigger than the maximum
25 * number of entries that can exist in the VCAP.
26 */
27 #define LAN966X_VCAP_PTP_RULE_ID 1000000
28 #define LAN966X_VCAP_L2_PTP_TRAP (LAN966X_VCAP_PTP_RULE_ID + 0)
29 #define LAN966X_VCAP_IPV4_EV_PTP_TRAP (LAN966X_VCAP_PTP_RULE_ID + 1)
30 #define LAN966X_VCAP_IPV4_GEN_PTP_TRAP (LAN966X_VCAP_PTP_RULE_ID + 2)
31 #define LAN966X_VCAP_IPV6_EV_PTP_TRAP (LAN966X_VCAP_PTP_RULE_ID + 3)
32 #define LAN966X_VCAP_IPV6_GEN_PTP_TRAP (LAN966X_VCAP_PTP_RULE_ID + 4)
33
34 enum {
35 PTP_PIN_ACTION_IDLE = 0,
36 PTP_PIN_ACTION_LOAD,
37 PTP_PIN_ACTION_SAVE,
38 PTP_PIN_ACTION_CLOCK,
39 PTP_PIN_ACTION_DELTA,
40 PTP_PIN_ACTION_TOD
41 };
42
lan966x_ptp_get_nominal_value(void)43 static u64 lan966x_ptp_get_nominal_value(void)
44 {
45 /* This is the default value that for each system clock, the time of day
46 * is increased. It has the format 5.59 nanosecond.
47 */
48 return 0x304d4873ecade305;
49 }
50
lan966x_ptp_add_trap(struct lan966x_port * port,int (* add_ptp_key)(struct vcap_rule * vrule,struct lan966x_port *),u32 rule_id,u16 proto)51 static int lan966x_ptp_add_trap(struct lan966x_port *port,
52 int (*add_ptp_key)(struct vcap_rule *vrule,
53 struct lan966x_port*),
54 u32 rule_id,
55 u16 proto)
56 {
57 struct lan966x *lan966x = port->lan966x;
58 struct vcap_rule *vrule;
59 int err;
60
61 vrule = vcap_get_rule(lan966x->vcap_ctrl, rule_id);
62 if (!IS_ERR(vrule)) {
63 u32 value, mask;
64
65 /* Just modify the ingress port mask and exit */
66 vcap_rule_get_key_u32(vrule, VCAP_KF_IF_IGR_PORT_MASK,
67 &value, &mask);
68 mask &= ~BIT(port->chip_port);
69 vcap_rule_mod_key_u32(vrule, VCAP_KF_IF_IGR_PORT_MASK,
70 value, mask);
71
72 err = vcap_mod_rule(vrule);
73 goto free_rule;
74 }
75
76 vrule = vcap_alloc_rule(lan966x->vcap_ctrl, port->dev,
77 LAN966X_VCAP_CID_IS2_L0,
78 VCAP_USER_PTP, 0, rule_id);
79 if (IS_ERR(vrule))
80 return PTR_ERR(vrule);
81
82 err = add_ptp_key(vrule, port);
83 if (err)
84 goto free_rule;
85
86 err = vcap_rule_add_action_bit(vrule, VCAP_AF_CPU_COPY_ENA, VCAP_BIT_1);
87 err |= vcap_rule_add_action_u32(vrule, VCAP_AF_MASK_MODE, LAN966X_PMM_REPLACE);
88 err |= vcap_val_rule(vrule, proto);
89 if (err)
90 goto free_rule;
91
92 err = vcap_add_rule(vrule);
93
94 free_rule:
95 /* Free the local copy of the rule */
96 vcap_free_rule(vrule);
97 return err;
98 }
99
lan966x_ptp_del_trap(struct lan966x_port * port,u32 rule_id)100 static int lan966x_ptp_del_trap(struct lan966x_port *port,
101 u32 rule_id)
102 {
103 struct lan966x *lan966x = port->lan966x;
104 struct vcap_rule *vrule;
105 u32 value, mask;
106 int err;
107
108 vrule = vcap_get_rule(lan966x->vcap_ctrl, rule_id);
109 if (IS_ERR(vrule))
110 return -EEXIST;
111
112 vcap_rule_get_key_u32(vrule, VCAP_KF_IF_IGR_PORT_MASK, &value, &mask);
113 mask |= BIT(port->chip_port);
114
115 /* No other port requires this trap, so it is safe to remove it */
116 if (mask == GENMASK(lan966x->num_phys_ports, 0)) {
117 err = vcap_del_rule(lan966x->vcap_ctrl, port->dev, rule_id);
118 goto free_rule;
119 }
120
121 vcap_rule_mod_key_u32(vrule, VCAP_KF_IF_IGR_PORT_MASK, value, mask);
122 err = vcap_mod_rule(vrule);
123
124 free_rule:
125 vcap_free_rule(vrule);
126 return err;
127 }
128
lan966x_ptp_add_l2_key(struct vcap_rule * vrule,struct lan966x_port * port)129 static int lan966x_ptp_add_l2_key(struct vcap_rule *vrule,
130 struct lan966x_port *port)
131 {
132 return vcap_rule_add_key_u32(vrule, VCAP_KF_ETYPE, ETH_P_1588, ~0);
133 }
134
lan966x_ptp_add_ip_event_key(struct vcap_rule * vrule,struct lan966x_port * port)135 static int lan966x_ptp_add_ip_event_key(struct vcap_rule *vrule,
136 struct lan966x_port *port)
137 {
138 return vcap_rule_add_key_u32(vrule, VCAP_KF_L4_DPORT, PTP_EV_PORT, ~0) ||
139 vcap_rule_add_key_bit(vrule, VCAP_KF_TCP_IS, VCAP_BIT_0);
140 }
141
lan966x_ptp_add_ip_general_key(struct vcap_rule * vrule,struct lan966x_port * port)142 static int lan966x_ptp_add_ip_general_key(struct vcap_rule *vrule,
143 struct lan966x_port *port)
144 {
145 return vcap_rule_add_key_u32(vrule, VCAP_KF_L4_DPORT, PTP_GEN_PORT, ~0) ||
146 vcap_rule_add_key_bit(vrule, VCAP_KF_TCP_IS, VCAP_BIT_0);
147 }
148
lan966x_ptp_add_l2_rule(struct lan966x_port * port)149 static int lan966x_ptp_add_l2_rule(struct lan966x_port *port)
150 {
151 return lan966x_ptp_add_trap(port, lan966x_ptp_add_l2_key,
152 LAN966X_VCAP_L2_PTP_TRAP, ETH_P_ALL);
153 }
154
lan966x_ptp_add_ipv4_rules(struct lan966x_port * port)155 static int lan966x_ptp_add_ipv4_rules(struct lan966x_port *port)
156 {
157 int err;
158
159 err = lan966x_ptp_add_trap(port, lan966x_ptp_add_ip_event_key,
160 LAN966X_VCAP_IPV4_EV_PTP_TRAP, ETH_P_IP);
161 if (err)
162 return err;
163
164 err = lan966x_ptp_add_trap(port, lan966x_ptp_add_ip_general_key,
165 LAN966X_VCAP_IPV4_GEN_PTP_TRAP, ETH_P_IP);
166 if (err)
167 lan966x_ptp_del_trap(port, LAN966X_VCAP_IPV4_EV_PTP_TRAP);
168
169 return err;
170 }
171
lan966x_ptp_add_ipv6_rules(struct lan966x_port * port)172 static int lan966x_ptp_add_ipv6_rules(struct lan966x_port *port)
173 {
174 int err;
175
176 err = lan966x_ptp_add_trap(port, lan966x_ptp_add_ip_event_key,
177 LAN966X_VCAP_IPV6_EV_PTP_TRAP, ETH_P_IPV6);
178 if (err)
179 return err;
180
181 err = lan966x_ptp_add_trap(port, lan966x_ptp_add_ip_general_key,
182 LAN966X_VCAP_IPV6_GEN_PTP_TRAP, ETH_P_IPV6);
183 if (err)
184 lan966x_ptp_del_trap(port, LAN966X_VCAP_IPV6_EV_PTP_TRAP);
185
186 return err;
187 }
188
lan966x_ptp_del_l2_rule(struct lan966x_port * port)189 static int lan966x_ptp_del_l2_rule(struct lan966x_port *port)
190 {
191 return lan966x_ptp_del_trap(port, LAN966X_VCAP_L2_PTP_TRAP);
192 }
193
lan966x_ptp_del_ipv4_rules(struct lan966x_port * port)194 static int lan966x_ptp_del_ipv4_rules(struct lan966x_port *port)
195 {
196 int err;
197
198 err = lan966x_ptp_del_trap(port, LAN966X_VCAP_IPV4_EV_PTP_TRAP);
199 err |= lan966x_ptp_del_trap(port, LAN966X_VCAP_IPV4_GEN_PTP_TRAP);
200
201 return err;
202 }
203
lan966x_ptp_del_ipv6_rules(struct lan966x_port * port)204 static int lan966x_ptp_del_ipv6_rules(struct lan966x_port *port)
205 {
206 int err;
207
208 err = lan966x_ptp_del_trap(port, LAN966X_VCAP_IPV6_EV_PTP_TRAP);
209 err |= lan966x_ptp_del_trap(port, LAN966X_VCAP_IPV6_GEN_PTP_TRAP);
210
211 return err;
212 }
213
lan966x_ptp_add_traps(struct lan966x_port * port)214 static int lan966x_ptp_add_traps(struct lan966x_port *port)
215 {
216 int err;
217
218 err = lan966x_ptp_add_l2_rule(port);
219 if (err)
220 goto err_l2;
221
222 err = lan966x_ptp_add_ipv4_rules(port);
223 if (err)
224 goto err_ipv4;
225
226 err = lan966x_ptp_add_ipv6_rules(port);
227 if (err)
228 goto err_ipv6;
229
230 return err;
231
232 err_ipv6:
233 lan966x_ptp_del_ipv4_rules(port);
234 err_ipv4:
235 lan966x_ptp_del_l2_rule(port);
236 err_l2:
237 return err;
238 }
239
lan966x_ptp_del_traps(struct lan966x_port * port)240 int lan966x_ptp_del_traps(struct lan966x_port *port)
241 {
242 int err;
243
244 err = lan966x_ptp_del_l2_rule(port);
245 err |= lan966x_ptp_del_ipv4_rules(port);
246 err |= lan966x_ptp_del_ipv6_rules(port);
247
248 return err;
249 }
250
lan966x_ptp_setup_traps(struct lan966x_port * port,struct kernel_hwtstamp_config * cfg)251 int lan966x_ptp_setup_traps(struct lan966x_port *port,
252 struct kernel_hwtstamp_config *cfg)
253 {
254 if (cfg->rx_filter == HWTSTAMP_FILTER_NONE)
255 return lan966x_ptp_del_traps(port);
256 else
257 return lan966x_ptp_add_traps(port);
258 }
259
lan966x_ptp_hwtstamp_set(struct lan966x_port * port,struct kernel_hwtstamp_config * cfg,struct netlink_ext_ack * extack)260 int lan966x_ptp_hwtstamp_set(struct lan966x_port *port,
261 struct kernel_hwtstamp_config *cfg,
262 struct netlink_ext_ack *extack)
263 {
264 struct lan966x *lan966x = port->lan966x;
265 struct lan966x_phc *phc;
266
267 switch (cfg->tx_type) {
268 case HWTSTAMP_TX_ON:
269 port->ptp_tx_cmd = IFH_REW_OP_TWO_STEP_PTP;
270 break;
271 case HWTSTAMP_TX_ONESTEP_SYNC:
272 port->ptp_tx_cmd = IFH_REW_OP_ONE_STEP_PTP;
273 break;
274 case HWTSTAMP_TX_OFF:
275 port->ptp_tx_cmd = IFH_REW_OP_NOOP;
276 break;
277 default:
278 return -ERANGE;
279 }
280
281 switch (cfg->rx_filter) {
282 case HWTSTAMP_FILTER_NONE:
283 port->ptp_rx_cmd = false;
284 break;
285 case HWTSTAMP_FILTER_ALL:
286 case HWTSTAMP_FILTER_PTP_V1_L4_EVENT:
287 case HWTSTAMP_FILTER_PTP_V1_L4_SYNC:
288 case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ:
289 case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
290 case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
291 case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
292 case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
293 case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
294 case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
295 case HWTSTAMP_FILTER_PTP_V2_EVENT:
296 case HWTSTAMP_FILTER_PTP_V2_SYNC:
297 case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
298 case HWTSTAMP_FILTER_NTP_ALL:
299 port->ptp_rx_cmd = true;
300 cfg->rx_filter = HWTSTAMP_FILTER_ALL;
301 break;
302 default:
303 return -ERANGE;
304 }
305
306 /* Commit back the result & save it */
307 mutex_lock(&lan966x->ptp_lock);
308 phc = &lan966x->phc[LAN966X_PHC_PORT];
309 phc->hwtstamp_config = *cfg;
310 mutex_unlock(&lan966x->ptp_lock);
311
312 return 0;
313 }
314
lan966x_ptp_hwtstamp_get(struct lan966x_port * port,struct kernel_hwtstamp_config * cfg)315 void lan966x_ptp_hwtstamp_get(struct lan966x_port *port,
316 struct kernel_hwtstamp_config *cfg)
317 {
318 struct lan966x *lan966x = port->lan966x;
319 struct lan966x_phc *phc;
320
321 phc = &lan966x->phc[LAN966X_PHC_PORT];
322 *cfg = phc->hwtstamp_config;
323 }
324
lan966x_ptp_classify(struct lan966x_port * port,struct sk_buff * skb,u8 * rew_op,u8 * pdu_type)325 static void lan966x_ptp_classify(struct lan966x_port *port, struct sk_buff *skb,
326 u8 *rew_op, u8 *pdu_type)
327 {
328 struct ptp_header *header;
329 u8 msgtype;
330 int type;
331
332 if (port->ptp_tx_cmd == IFH_REW_OP_NOOP) {
333 *rew_op = IFH_REW_OP_NOOP;
334 *pdu_type = IFH_PDU_TYPE_NONE;
335 return;
336 }
337
338 type = ptp_classify_raw(skb);
339 if (type == PTP_CLASS_NONE) {
340 *rew_op = IFH_REW_OP_NOOP;
341 *pdu_type = IFH_PDU_TYPE_NONE;
342 return;
343 }
344
345 header = ptp_parse_header(skb, type);
346 if (!header) {
347 *rew_op = IFH_REW_OP_NOOP;
348 *pdu_type = IFH_PDU_TYPE_NONE;
349 return;
350 }
351
352 if (type & PTP_CLASS_L2)
353 *pdu_type = IFH_PDU_TYPE_NONE;
354 if (type & PTP_CLASS_IPV4)
355 *pdu_type = IFH_PDU_TYPE_IPV4;
356 if (type & PTP_CLASS_IPV6)
357 *pdu_type = IFH_PDU_TYPE_IPV6;
358
359 if (port->ptp_tx_cmd == IFH_REW_OP_TWO_STEP_PTP) {
360 *rew_op = IFH_REW_OP_TWO_STEP_PTP;
361 return;
362 }
363
364 /* If it is sync and run 1 step then set the correct operation,
365 * otherwise run as 2 step
366 */
367 msgtype = ptp_get_msgtype(header, type);
368 if ((msgtype & 0xf) == 0) {
369 *rew_op = IFH_REW_OP_ONE_STEP_PTP;
370 return;
371 }
372
373 *rew_op = IFH_REW_OP_TWO_STEP_PTP;
374 }
375
lan966x_ptp_txtstamp_old_release(struct lan966x_port * port)376 static void lan966x_ptp_txtstamp_old_release(struct lan966x_port *port)
377 {
378 struct sk_buff *skb, *skb_tmp;
379 unsigned long flags;
380
381 spin_lock_irqsave(&port->tx_skbs.lock, flags);
382 skb_queue_walk_safe(&port->tx_skbs, skb, skb_tmp) {
383 if time_after(LAN966X_SKB_CB(skb)->jiffies + LAN966X_PTP_TIMEOUT,
384 jiffies)
385 break;
386
387 __skb_unlink(skb, &port->tx_skbs);
388 dev_kfree_skb_any(skb);
389 }
390 spin_unlock_irqrestore(&port->tx_skbs.lock, flags);
391 }
392
lan966x_ptp_txtstamp_request(struct lan966x_port * port,struct sk_buff * skb)393 int lan966x_ptp_txtstamp_request(struct lan966x_port *port,
394 struct sk_buff *skb)
395 {
396 struct lan966x *lan966x = port->lan966x;
397 unsigned long flags;
398 u8 pdu_type;
399 u8 rew_op;
400
401 lan966x_ptp_classify(port, skb, &rew_op, &pdu_type);
402 LAN966X_SKB_CB(skb)->rew_op = rew_op;
403 LAN966X_SKB_CB(skb)->pdu_type = pdu_type;
404
405 if (rew_op != IFH_REW_OP_TWO_STEP_PTP)
406 return 0;
407
408 lan966x_ptp_txtstamp_old_release(port);
409
410 spin_lock_irqsave(&lan966x->ptp_ts_id_lock, flags);
411 if (lan966x->ptp_skbs == LAN966X_MAX_PTP_ID) {
412 spin_unlock_irqrestore(&lan966x->ptp_ts_id_lock, flags);
413 return -EBUSY;
414 }
415
416 skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
417
418 skb_queue_tail(&port->tx_skbs, skb);
419 LAN966X_SKB_CB(skb)->ts_id = port->ts_id;
420 LAN966X_SKB_CB(skb)->jiffies = jiffies;
421
422 lan966x->ptp_skbs++;
423 port->ts_id++;
424 if (port->ts_id == LAN966X_MAX_PTP_ID)
425 port->ts_id = 0;
426
427 spin_unlock_irqrestore(&lan966x->ptp_ts_id_lock, flags);
428
429 return 0;
430 }
431
lan966x_ptp_txtstamp_release(struct lan966x_port * port,struct sk_buff * skb)432 void lan966x_ptp_txtstamp_release(struct lan966x_port *port,
433 struct sk_buff *skb)
434 {
435 struct lan966x *lan966x = port->lan966x;
436 unsigned long flags;
437
438 spin_lock_irqsave(&lan966x->ptp_ts_id_lock, flags);
439 port->ts_id--;
440 lan966x->ptp_skbs--;
441 skb_unlink(skb, &port->tx_skbs);
442 spin_unlock_irqrestore(&lan966x->ptp_ts_id_lock, flags);
443 }
444
lan966x_get_hwtimestamp(struct lan966x * lan966x,struct timespec64 * ts,u32 nsec)445 static void lan966x_get_hwtimestamp(struct lan966x *lan966x,
446 struct timespec64 *ts,
447 u32 nsec)
448 {
449 /* Read current PTP time to get seconds */
450 unsigned long flags;
451 u32 curr_nsec;
452
453 spin_lock_irqsave(&lan966x->ptp_clock_lock, flags);
454
455 lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_SAVE) |
456 PTP_PIN_CFG_PIN_DOM_SET(LAN966X_PHC_PORT) |
457 PTP_PIN_CFG_PIN_SYNC_SET(0),
458 PTP_PIN_CFG_PIN_ACTION |
459 PTP_PIN_CFG_PIN_DOM |
460 PTP_PIN_CFG_PIN_SYNC,
461 lan966x, PTP_PIN_CFG(TOD_ACC_PIN));
462
463 ts->tv_sec = lan_rd(lan966x, PTP_TOD_SEC_LSB(TOD_ACC_PIN));
464 curr_nsec = lan_rd(lan966x, PTP_TOD_NSEC(TOD_ACC_PIN));
465
466 ts->tv_nsec = nsec;
467
468 /* Sec has incremented since the ts was registered */
469 if (curr_nsec < nsec)
470 ts->tv_sec--;
471
472 spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags);
473 }
474
lan966x_ptp_irq_handler(int irq,void * args)475 irqreturn_t lan966x_ptp_irq_handler(int irq, void *args)
476 {
477 int budget = LAN966X_MAX_PTP_ID;
478 struct lan966x *lan966x = args;
479
480 while (budget--) {
481 struct sk_buff *skb, *skb_tmp, *skb_match = NULL;
482 struct skb_shared_hwtstamps shhwtstamps;
483 struct lan966x_port *port;
484 struct timespec64 ts;
485 unsigned long flags;
486 u32 val, id, txport;
487 u32 delay;
488
489 val = lan_rd(lan966x, PTP_TWOSTEP_CTRL);
490
491 /* Check if a timestamp can be retrieved */
492 if (!(val & PTP_TWOSTEP_CTRL_VLD))
493 break;
494
495 WARN_ON(val & PTP_TWOSTEP_CTRL_OVFL);
496
497 if (!(val & PTP_TWOSTEP_CTRL_STAMP_TX))
498 continue;
499
500 /* Retrieve the ts Tx port */
501 txport = PTP_TWOSTEP_CTRL_STAMP_PORT_GET(val);
502
503 /* Retrieve its associated skb */
504 port = lan966x->ports[txport];
505
506 /* Retrieve the delay */
507 delay = lan_rd(lan966x, PTP_TWOSTEP_STAMP);
508 delay = PTP_TWOSTEP_STAMP_STAMP_NSEC_GET(delay);
509
510 /* Get next timestamp from fifo, which needs to be the
511 * rx timestamp which represents the id of the frame
512 */
513 lan_rmw(PTP_TWOSTEP_CTRL_NXT_SET(1),
514 PTP_TWOSTEP_CTRL_NXT,
515 lan966x, PTP_TWOSTEP_CTRL);
516
517 val = lan_rd(lan966x, PTP_TWOSTEP_CTRL);
518
519 /* Check if a timestamp can be retried */
520 if (!(val & PTP_TWOSTEP_CTRL_VLD))
521 break;
522
523 /* Read RX timestamping to get the ID */
524 id = lan_rd(lan966x, PTP_TWOSTEP_STAMP);
525
526 spin_lock_irqsave(&port->tx_skbs.lock, flags);
527 skb_queue_walk_safe(&port->tx_skbs, skb, skb_tmp) {
528 if (LAN966X_SKB_CB(skb)->ts_id != id)
529 continue;
530
531 __skb_unlink(skb, &port->tx_skbs);
532 skb_match = skb;
533 break;
534 }
535 spin_unlock_irqrestore(&port->tx_skbs.lock, flags);
536
537 /* Next ts */
538 lan_rmw(PTP_TWOSTEP_CTRL_NXT_SET(1),
539 PTP_TWOSTEP_CTRL_NXT,
540 lan966x, PTP_TWOSTEP_CTRL);
541
542 if (WARN_ON(!skb_match))
543 continue;
544
545 spin_lock_irqsave(&lan966x->ptp_ts_id_lock, flags);
546 lan966x->ptp_skbs--;
547 spin_unlock_irqrestore(&lan966x->ptp_ts_id_lock, flags);
548
549 /* Get the h/w timestamp */
550 lan966x_get_hwtimestamp(lan966x, &ts, delay);
551
552 /* Set the timestamp into the skb */
553 shhwtstamps.hwtstamp = ktime_set(ts.tv_sec, ts.tv_nsec);
554 skb_tstamp_tx(skb_match, &shhwtstamps);
555
556 dev_kfree_skb_any(skb_match);
557 }
558
559 return IRQ_HANDLED;
560 }
561
lan966x_ptp_ext_irq_handler(int irq,void * args)562 irqreturn_t lan966x_ptp_ext_irq_handler(int irq, void *args)
563 {
564 struct lan966x *lan966x = args;
565 struct lan966x_phc *phc;
566 unsigned long flags;
567 u64 time = 0;
568 time64_t s;
569 int pin, i;
570 s64 ns;
571
572 if (!(lan_rd(lan966x, PTP_PIN_INTR)))
573 return IRQ_NONE;
574
575 /* Go through all domains and see which pin generated the interrupt */
576 for (i = 0; i < LAN966X_PHC_COUNT; ++i) {
577 struct ptp_clock_event ptp_event = {0};
578
579 phc = &lan966x->phc[i];
580 pin = ptp_find_pin_unlocked(phc->clock, PTP_PF_EXTTS, 0);
581 if (pin == -1)
582 continue;
583
584 if (!(lan_rd(lan966x, PTP_PIN_INTR) & BIT(pin)))
585 continue;
586
587 spin_lock_irqsave(&lan966x->ptp_clock_lock, flags);
588
589 /* Enable to get the new interrupt.
590 * By writing 1 it clears the bit
591 */
592 lan_wr(BIT(pin), lan966x, PTP_PIN_INTR);
593
594 /* Get current time */
595 s = lan_rd(lan966x, PTP_TOD_SEC_MSB(pin));
596 s <<= 32;
597 s |= lan_rd(lan966x, PTP_TOD_SEC_LSB(pin));
598 ns = lan_rd(lan966x, PTP_TOD_NSEC(pin));
599 ns &= PTP_TOD_NSEC_TOD_NSEC;
600
601 spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags);
602
603 if ((ns & 0xFFFFFFF0) == 0x3FFFFFF0) {
604 s--;
605 ns &= 0xf;
606 ns += 999999984;
607 }
608 time = ktime_set(s, ns);
609
610 ptp_event.index = pin;
611 ptp_event.timestamp = time;
612 ptp_event.type = PTP_CLOCK_EXTTS;
613 ptp_clock_event(phc->clock, &ptp_event);
614 }
615
616 return IRQ_HANDLED;
617 }
618
lan966x_ptp_adjfine(struct ptp_clock_info * ptp,long scaled_ppm)619 static int lan966x_ptp_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)
620 {
621 struct lan966x_phc *phc = container_of(ptp, struct lan966x_phc, info);
622 struct lan966x *lan966x = phc->lan966x;
623 unsigned long flags;
624 bool neg_adj = 0;
625 u64 tod_inc;
626 u64 ref;
627
628 if (!scaled_ppm)
629 return 0;
630
631 if (scaled_ppm < 0) {
632 neg_adj = 1;
633 scaled_ppm = -scaled_ppm;
634 }
635
636 tod_inc = lan966x_ptp_get_nominal_value();
637
638 /* The multiplication is split in 2 separate additions because of
639 * overflow issues. If scaled_ppm with 16bit fractional part was bigger
640 * than 20ppm then we got overflow.
641 */
642 ref = LAN966X_1PPM_FORMAT * (scaled_ppm >> 16);
643 ref += (LAN966X_1PPM_FORMAT * (0xffff & scaled_ppm)) >> 16;
644 tod_inc = neg_adj ? tod_inc - ref : tod_inc + ref;
645
646 spin_lock_irqsave(&lan966x->ptp_clock_lock, flags);
647
648 lan_rmw(PTP_DOM_CFG_CLKCFG_DIS_SET(1 << BIT(phc->index)),
649 PTP_DOM_CFG_CLKCFG_DIS,
650 lan966x, PTP_DOM_CFG);
651
652 lan_wr((u32)tod_inc & 0xFFFFFFFF, lan966x,
653 PTP_CLK_PER_CFG(phc->index, 0));
654 lan_wr((u32)(tod_inc >> 32), lan966x,
655 PTP_CLK_PER_CFG(phc->index, 1));
656
657 lan_rmw(PTP_DOM_CFG_CLKCFG_DIS_SET(0),
658 PTP_DOM_CFG_CLKCFG_DIS,
659 lan966x, PTP_DOM_CFG);
660
661 spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags);
662
663 return 0;
664 }
665
lan966x_ptp_settime64(struct ptp_clock_info * ptp,const struct timespec64 * ts)666 static int lan966x_ptp_settime64(struct ptp_clock_info *ptp,
667 const struct timespec64 *ts)
668 {
669 struct lan966x_phc *phc = container_of(ptp, struct lan966x_phc, info);
670 struct lan966x *lan966x = phc->lan966x;
671 unsigned long flags;
672
673 spin_lock_irqsave(&lan966x->ptp_clock_lock, flags);
674
675 /* Must be in IDLE mode before the time can be loaded */
676 lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_IDLE) |
677 PTP_PIN_CFG_PIN_DOM_SET(phc->index) |
678 PTP_PIN_CFG_PIN_SYNC_SET(0),
679 PTP_PIN_CFG_PIN_ACTION |
680 PTP_PIN_CFG_PIN_DOM |
681 PTP_PIN_CFG_PIN_SYNC,
682 lan966x, PTP_PIN_CFG(TOD_ACC_PIN));
683
684 /* Set new value */
685 lan_wr(PTP_TOD_SEC_MSB_TOD_SEC_MSB_SET(upper_32_bits(ts->tv_sec)),
686 lan966x, PTP_TOD_SEC_MSB(TOD_ACC_PIN));
687 lan_wr(lower_32_bits(ts->tv_sec),
688 lan966x, PTP_TOD_SEC_LSB(TOD_ACC_PIN));
689 lan_wr(ts->tv_nsec, lan966x, PTP_TOD_NSEC(TOD_ACC_PIN));
690
691 /* Apply new values */
692 lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_LOAD) |
693 PTP_PIN_CFG_PIN_DOM_SET(phc->index) |
694 PTP_PIN_CFG_PIN_SYNC_SET(0),
695 PTP_PIN_CFG_PIN_ACTION |
696 PTP_PIN_CFG_PIN_DOM |
697 PTP_PIN_CFG_PIN_SYNC,
698 lan966x, PTP_PIN_CFG(TOD_ACC_PIN));
699
700 spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags);
701
702 return 0;
703 }
704
lan966x_ptp_gettime64(struct ptp_clock_info * ptp,struct timespec64 * ts)705 int lan966x_ptp_gettime64(struct ptp_clock_info *ptp, struct timespec64 *ts)
706 {
707 struct lan966x_phc *phc = container_of(ptp, struct lan966x_phc, info);
708 struct lan966x *lan966x = phc->lan966x;
709 unsigned long flags;
710 time64_t s;
711 s64 ns;
712
713 spin_lock_irqsave(&lan966x->ptp_clock_lock, flags);
714
715 lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_SAVE) |
716 PTP_PIN_CFG_PIN_DOM_SET(phc->index) |
717 PTP_PIN_CFG_PIN_SYNC_SET(0),
718 PTP_PIN_CFG_PIN_ACTION |
719 PTP_PIN_CFG_PIN_DOM |
720 PTP_PIN_CFG_PIN_SYNC,
721 lan966x, PTP_PIN_CFG(TOD_ACC_PIN));
722
723 s = lan_rd(lan966x, PTP_TOD_SEC_MSB(TOD_ACC_PIN));
724 s <<= 32;
725 s |= lan_rd(lan966x, PTP_TOD_SEC_LSB(TOD_ACC_PIN));
726 ns = lan_rd(lan966x, PTP_TOD_NSEC(TOD_ACC_PIN));
727 ns &= PTP_TOD_NSEC_TOD_NSEC;
728
729 spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags);
730
731 /* Deal with negative values */
732 if ((ns & 0xFFFFFFF0) == 0x3FFFFFF0) {
733 s--;
734 ns &= 0xf;
735 ns += 999999984;
736 }
737
738 set_normalized_timespec64(ts, s, ns);
739 return 0;
740 }
741
lan966x_ptp_adjtime(struct ptp_clock_info * ptp,s64 delta)742 static int lan966x_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
743 {
744 struct lan966x_phc *phc = container_of(ptp, struct lan966x_phc, info);
745 struct lan966x *lan966x = phc->lan966x;
746
747 if (delta > -(NSEC_PER_SEC / 2) && delta < (NSEC_PER_SEC / 2)) {
748 unsigned long flags;
749
750 spin_lock_irqsave(&lan966x->ptp_clock_lock, flags);
751
752 /* Must be in IDLE mode before the time can be loaded */
753 lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_IDLE) |
754 PTP_PIN_CFG_PIN_DOM_SET(phc->index) |
755 PTP_PIN_CFG_PIN_SYNC_SET(0),
756 PTP_PIN_CFG_PIN_ACTION |
757 PTP_PIN_CFG_PIN_DOM |
758 PTP_PIN_CFG_PIN_SYNC,
759 lan966x, PTP_PIN_CFG(TOD_ACC_PIN));
760
761 lan_wr(PTP_TOD_NSEC_TOD_NSEC_SET(delta),
762 lan966x, PTP_TOD_NSEC(TOD_ACC_PIN));
763
764 /* Adjust time with the value of PTP_TOD_NSEC */
765 lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_DELTA) |
766 PTP_PIN_CFG_PIN_DOM_SET(phc->index) |
767 PTP_PIN_CFG_PIN_SYNC_SET(0),
768 PTP_PIN_CFG_PIN_ACTION |
769 PTP_PIN_CFG_PIN_DOM |
770 PTP_PIN_CFG_PIN_SYNC,
771 lan966x, PTP_PIN_CFG(TOD_ACC_PIN));
772
773 spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags);
774 } else {
775 /* Fall back using lan966x_ptp_settime64 which is not exact */
776 struct timespec64 ts;
777 u64 now;
778
779 lan966x_ptp_gettime64(ptp, &ts);
780
781 now = ktime_to_ns(timespec64_to_ktime(ts));
782 ts = ns_to_timespec64(now + delta);
783
784 lan966x_ptp_settime64(ptp, &ts);
785 }
786
787 return 0;
788 }
789
lan966x_ptp_verify(struct ptp_clock_info * ptp,unsigned int pin,enum ptp_pin_function func,unsigned int chan)790 static int lan966x_ptp_verify(struct ptp_clock_info *ptp, unsigned int pin,
791 enum ptp_pin_function func, unsigned int chan)
792 {
793 struct lan966x_phc *phc = container_of(ptp, struct lan966x_phc, info);
794 struct lan966x *lan966x = phc->lan966x;
795 struct ptp_clock_info *info;
796 int i;
797
798 /* Currently support only 1 channel */
799 if (chan != 0)
800 return -1;
801
802 switch (func) {
803 case PTP_PF_NONE:
804 case PTP_PF_PEROUT:
805 case PTP_PF_EXTTS:
806 break;
807 default:
808 return -1;
809 }
810
811 /* The PTP pins are shared by all the PHC. So it is required to see if
812 * the pin is connected to another PHC. The pin is connected to another
813 * PHC if that pin already has a function on that PHC.
814 */
815 for (i = 0; i < LAN966X_PHC_COUNT; ++i) {
816 info = &lan966x->phc[i].info;
817
818 /* Ignore the check with ourself */
819 if (ptp == info)
820 continue;
821
822 if (info->pin_config[pin].func == PTP_PF_PEROUT ||
823 info->pin_config[pin].func == PTP_PF_EXTTS)
824 return -1;
825 }
826
827 return 0;
828 }
829
lan966x_ptp_perout(struct ptp_clock_info * ptp,struct ptp_clock_request * rq,int on)830 static int lan966x_ptp_perout(struct ptp_clock_info *ptp,
831 struct ptp_clock_request *rq, int on)
832 {
833 struct lan966x_phc *phc = container_of(ptp, struct lan966x_phc, info);
834 struct lan966x *lan966x = phc->lan966x;
835 struct timespec64 ts_phase, ts_period;
836 unsigned long flags;
837 s64 wf_high, wf_low;
838 bool pps = false;
839 int pin;
840
841 if (rq->perout.flags & ~(PTP_PEROUT_DUTY_CYCLE |
842 PTP_PEROUT_PHASE))
843 return -EOPNOTSUPP;
844
845 pin = ptp_find_pin(phc->clock, PTP_PF_PEROUT, rq->perout.index);
846 if (pin == -1 || pin >= LAN966X_PHC_PINS_NUM)
847 return -EINVAL;
848
849 if (!on) {
850 spin_lock_irqsave(&lan966x->ptp_clock_lock, flags);
851 lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_IDLE) |
852 PTP_PIN_CFG_PIN_DOM_SET(phc->index) |
853 PTP_PIN_CFG_PIN_SYNC_SET(0),
854 PTP_PIN_CFG_PIN_ACTION |
855 PTP_PIN_CFG_PIN_DOM |
856 PTP_PIN_CFG_PIN_SYNC,
857 lan966x, PTP_PIN_CFG(pin));
858 spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags);
859 return 0;
860 }
861
862 if (rq->perout.period.sec == 1 &&
863 rq->perout.period.nsec == 0)
864 pps = true;
865
866 if (rq->perout.flags & PTP_PEROUT_PHASE) {
867 ts_phase.tv_sec = rq->perout.phase.sec;
868 ts_phase.tv_nsec = rq->perout.phase.nsec;
869 } else {
870 ts_phase.tv_sec = rq->perout.start.sec;
871 ts_phase.tv_nsec = rq->perout.start.nsec;
872 }
873
874 if (ts_phase.tv_sec || (ts_phase.tv_nsec && !pps)) {
875 dev_warn(lan966x->dev,
876 "Absolute time not supported!\n");
877 return -EINVAL;
878 }
879
880 if (rq->perout.flags & PTP_PEROUT_DUTY_CYCLE) {
881 struct timespec64 ts_on;
882
883 ts_on.tv_sec = rq->perout.on.sec;
884 ts_on.tv_nsec = rq->perout.on.nsec;
885
886 wf_high = timespec64_to_ns(&ts_on);
887 } else {
888 wf_high = 5000;
889 }
890
891 if (pps) {
892 spin_lock_irqsave(&lan966x->ptp_clock_lock, flags);
893 lan_wr(PTP_WF_LOW_PERIOD_PIN_WFL(ts_phase.tv_nsec),
894 lan966x, PTP_WF_LOW_PERIOD(pin));
895 lan_wr(PTP_WF_HIGH_PERIOD_PIN_WFH(wf_high),
896 lan966x, PTP_WF_HIGH_PERIOD(pin));
897 lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_CLOCK) |
898 PTP_PIN_CFG_PIN_DOM_SET(phc->index) |
899 PTP_PIN_CFG_PIN_SYNC_SET(3),
900 PTP_PIN_CFG_PIN_ACTION |
901 PTP_PIN_CFG_PIN_DOM |
902 PTP_PIN_CFG_PIN_SYNC,
903 lan966x, PTP_PIN_CFG(pin));
904 spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags);
905 return 0;
906 }
907
908 ts_period.tv_sec = rq->perout.period.sec;
909 ts_period.tv_nsec = rq->perout.period.nsec;
910
911 wf_low = timespec64_to_ns(&ts_period);
912 wf_low -= wf_high;
913
914 spin_lock_irqsave(&lan966x->ptp_clock_lock, flags);
915 lan_wr(PTP_WF_LOW_PERIOD_PIN_WFL(wf_low),
916 lan966x, PTP_WF_LOW_PERIOD(pin));
917 lan_wr(PTP_WF_HIGH_PERIOD_PIN_WFH(wf_high),
918 lan966x, PTP_WF_HIGH_PERIOD(pin));
919 lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_CLOCK) |
920 PTP_PIN_CFG_PIN_DOM_SET(phc->index) |
921 PTP_PIN_CFG_PIN_SYNC_SET(0),
922 PTP_PIN_CFG_PIN_ACTION |
923 PTP_PIN_CFG_PIN_DOM |
924 PTP_PIN_CFG_PIN_SYNC,
925 lan966x, PTP_PIN_CFG(pin));
926 spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags);
927
928 return 0;
929 }
930
lan966x_ptp_extts(struct ptp_clock_info * ptp,struct ptp_clock_request * rq,int on)931 static int lan966x_ptp_extts(struct ptp_clock_info *ptp,
932 struct ptp_clock_request *rq, int on)
933 {
934 struct lan966x_phc *phc = container_of(ptp, struct lan966x_phc, info);
935 struct lan966x *lan966x = phc->lan966x;
936 unsigned long flags;
937 int pin;
938 u32 val;
939
940 if (lan966x->ptp_ext_irq <= 0)
941 return -EOPNOTSUPP;
942
943 /* Reject requests with unsupported flags */
944 if (rq->extts.flags & ~(PTP_ENABLE_FEATURE |
945 PTP_RISING_EDGE |
946 PTP_STRICT_FLAGS))
947 return -EOPNOTSUPP;
948
949 pin = ptp_find_pin(phc->clock, PTP_PF_EXTTS, rq->extts.index);
950 if (pin == -1 || pin >= LAN966X_PHC_PINS_NUM)
951 return -EINVAL;
952
953 spin_lock_irqsave(&lan966x->ptp_clock_lock, flags);
954 lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_SAVE) |
955 PTP_PIN_CFG_PIN_SYNC_SET(on ? 3 : 0) |
956 PTP_PIN_CFG_PIN_DOM_SET(phc->index) |
957 PTP_PIN_CFG_PIN_SELECT_SET(pin),
958 PTP_PIN_CFG_PIN_ACTION |
959 PTP_PIN_CFG_PIN_SYNC |
960 PTP_PIN_CFG_PIN_DOM |
961 PTP_PIN_CFG_PIN_SELECT,
962 lan966x, PTP_PIN_CFG(pin));
963
964 val = lan_rd(lan966x, PTP_PIN_INTR_ENA);
965 if (on)
966 val |= BIT(pin);
967 else
968 val &= ~BIT(pin);
969 lan_wr(val, lan966x, PTP_PIN_INTR_ENA);
970
971 spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags);
972
973 return 0;
974 }
975
lan966x_ptp_enable(struct ptp_clock_info * ptp,struct ptp_clock_request * rq,int on)976 static int lan966x_ptp_enable(struct ptp_clock_info *ptp,
977 struct ptp_clock_request *rq, int on)
978 {
979 switch (rq->type) {
980 case PTP_CLK_REQ_PEROUT:
981 return lan966x_ptp_perout(ptp, rq, on);
982 case PTP_CLK_REQ_EXTTS:
983 return lan966x_ptp_extts(ptp, rq, on);
984 default:
985 return -EOPNOTSUPP;
986 }
987
988 return 0;
989 }
990
991 static struct ptp_clock_info lan966x_ptp_clock_info = {
992 .owner = THIS_MODULE,
993 .name = "lan966x ptp",
994 .max_adj = 200000,
995 .gettime64 = lan966x_ptp_gettime64,
996 .settime64 = lan966x_ptp_settime64,
997 .adjtime = lan966x_ptp_adjtime,
998 .adjfine = lan966x_ptp_adjfine,
999 .verify = lan966x_ptp_verify,
1000 .enable = lan966x_ptp_enable,
1001 .n_per_out = LAN966X_PHC_PINS_NUM,
1002 .n_ext_ts = LAN966X_PHC_PINS_NUM,
1003 .n_pins = LAN966X_PHC_PINS_NUM,
1004 };
1005
lan966x_ptp_phc_init(struct lan966x * lan966x,int index,struct ptp_clock_info * clock_info)1006 static int lan966x_ptp_phc_init(struct lan966x *lan966x,
1007 int index,
1008 struct ptp_clock_info *clock_info)
1009 {
1010 struct lan966x_phc *phc = &lan966x->phc[index];
1011 struct ptp_pin_desc *p;
1012 int i;
1013
1014 for (i = 0; i < LAN966X_PHC_PINS_NUM; i++) {
1015 p = &phc->pins[i];
1016
1017 snprintf(p->name, sizeof(p->name), "pin%d", i);
1018 p->index = i;
1019 p->func = PTP_PF_NONE;
1020 }
1021
1022 phc->info = *clock_info;
1023 phc->info.pin_config = &phc->pins[0];
1024 phc->clock = ptp_clock_register(&phc->info, lan966x->dev);
1025 if (IS_ERR(phc->clock))
1026 return PTR_ERR(phc->clock);
1027
1028 phc->index = index;
1029 phc->lan966x = lan966x;
1030
1031 return 0;
1032 }
1033
lan966x_ptp_init(struct lan966x * lan966x)1034 int lan966x_ptp_init(struct lan966x *lan966x)
1035 {
1036 u64 tod_adj = lan966x_ptp_get_nominal_value();
1037 struct lan966x_port *port;
1038 int err, i;
1039
1040 if (!lan966x->ptp)
1041 return 0;
1042
1043 for (i = 0; i < LAN966X_PHC_COUNT; ++i) {
1044 err = lan966x_ptp_phc_init(lan966x, i, &lan966x_ptp_clock_info);
1045 if (err)
1046 return err;
1047 }
1048
1049 spin_lock_init(&lan966x->ptp_clock_lock);
1050 spin_lock_init(&lan966x->ptp_ts_id_lock);
1051 mutex_init(&lan966x->ptp_lock);
1052
1053 /* Disable master counters */
1054 lan_wr(PTP_DOM_CFG_ENA_SET(0), lan966x, PTP_DOM_CFG);
1055
1056 /* Configure the nominal TOD increment per clock cycle */
1057 lan_rmw(PTP_DOM_CFG_CLKCFG_DIS_SET(0x7),
1058 PTP_DOM_CFG_CLKCFG_DIS,
1059 lan966x, PTP_DOM_CFG);
1060
1061 for (i = 0; i < LAN966X_PHC_COUNT; ++i) {
1062 lan_wr((u32)tod_adj & 0xFFFFFFFF, lan966x,
1063 PTP_CLK_PER_CFG(i, 0));
1064 lan_wr((u32)(tod_adj >> 32), lan966x,
1065 PTP_CLK_PER_CFG(i, 1));
1066 }
1067
1068 lan_rmw(PTP_DOM_CFG_CLKCFG_DIS_SET(0),
1069 PTP_DOM_CFG_CLKCFG_DIS,
1070 lan966x, PTP_DOM_CFG);
1071
1072 /* Enable master counters */
1073 lan_wr(PTP_DOM_CFG_ENA_SET(0x7), lan966x, PTP_DOM_CFG);
1074
1075 for (i = 0; i < lan966x->num_phys_ports; i++) {
1076 port = lan966x->ports[i];
1077 if (!port)
1078 continue;
1079
1080 skb_queue_head_init(&port->tx_skbs);
1081 }
1082
1083 return 0;
1084 }
1085
lan966x_ptp_deinit(struct lan966x * lan966x)1086 void lan966x_ptp_deinit(struct lan966x *lan966x)
1087 {
1088 struct lan966x_port *port;
1089 int i;
1090
1091 if (!lan966x->ptp)
1092 return;
1093
1094 for (i = 0; i < lan966x->num_phys_ports; i++) {
1095 port = lan966x->ports[i];
1096 if (!port)
1097 continue;
1098
1099 skb_queue_purge(&port->tx_skbs);
1100 }
1101
1102 for (i = 0; i < LAN966X_PHC_COUNT; ++i)
1103 ptp_clock_unregister(lan966x->phc[i].clock);
1104 }
1105
lan966x_ptp_rxtstamp(struct lan966x * lan966x,struct sk_buff * skb,u64 src_port,u64 timestamp)1106 void lan966x_ptp_rxtstamp(struct lan966x *lan966x, struct sk_buff *skb,
1107 u64 src_port, u64 timestamp)
1108 {
1109 struct skb_shared_hwtstamps *shhwtstamps;
1110 struct lan966x_phc *phc;
1111 struct timespec64 ts;
1112 u64 full_ts_in_ns;
1113
1114 if (!lan966x->ptp ||
1115 !lan966x->ports[src_port]->ptp_rx_cmd)
1116 return;
1117
1118 phc = &lan966x->phc[LAN966X_PHC_PORT];
1119 lan966x_ptp_gettime64(&phc->info, &ts);
1120
1121 /* Drop the sub-ns precision */
1122 timestamp = timestamp >> 2;
1123 if (ts.tv_nsec < timestamp)
1124 ts.tv_sec--;
1125 ts.tv_nsec = timestamp;
1126 full_ts_in_ns = ktime_set(ts.tv_sec, ts.tv_nsec);
1127
1128 shhwtstamps = skb_hwtstamps(skb);
1129 shhwtstamps->hwtstamp = full_ts_in_ns;
1130 }
1131
lan966x_ptp_get_period_ps(void)1132 u32 lan966x_ptp_get_period_ps(void)
1133 {
1134 /* This represents the system clock period in picoseconds */
1135 return 15125;
1136 }
1137