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 
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 
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 
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 
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 
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 
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 
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 
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 
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 
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 
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 
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 
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 
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 
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 
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 
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 
325 static int lan966x_ptp_classify(struct lan966x_port *port, struct sk_buff *skb)
326 {
327 	struct ptp_header *header;
328 	u8 msgtype;
329 	int type;
330 
331 	if (port->ptp_tx_cmd == IFH_REW_OP_NOOP)
332 		return IFH_REW_OP_NOOP;
333 
334 	type = ptp_classify_raw(skb);
335 	if (type == PTP_CLASS_NONE)
336 		return IFH_REW_OP_NOOP;
337 
338 	header = ptp_parse_header(skb, type);
339 	if (!header)
340 		return IFH_REW_OP_NOOP;
341 
342 	if (port->ptp_tx_cmd == IFH_REW_OP_TWO_STEP_PTP)
343 		return IFH_REW_OP_TWO_STEP_PTP;
344 
345 	/* If it is sync and run 1 step then set the correct operation,
346 	 * otherwise run as 2 step
347 	 */
348 	msgtype = ptp_get_msgtype(header, type);
349 	if ((msgtype & 0xf) == 0)
350 		return IFH_REW_OP_ONE_STEP_PTP;
351 
352 	return IFH_REW_OP_TWO_STEP_PTP;
353 }
354 
355 static void lan966x_ptp_txtstamp_old_release(struct lan966x_port *port)
356 {
357 	struct sk_buff *skb, *skb_tmp;
358 	unsigned long flags;
359 
360 	spin_lock_irqsave(&port->tx_skbs.lock, flags);
361 	skb_queue_walk_safe(&port->tx_skbs, skb, skb_tmp) {
362 		if time_after(LAN966X_SKB_CB(skb)->jiffies + LAN966X_PTP_TIMEOUT,
363 			      jiffies)
364 			break;
365 
366 		__skb_unlink(skb, &port->tx_skbs);
367 		dev_kfree_skb_any(skb);
368 	}
369 	spin_unlock_irqrestore(&port->tx_skbs.lock, flags);
370 }
371 
372 int lan966x_ptp_txtstamp_request(struct lan966x_port *port,
373 				 struct sk_buff *skb)
374 {
375 	struct lan966x *lan966x = port->lan966x;
376 	unsigned long flags;
377 	u8 rew_op;
378 
379 	rew_op = lan966x_ptp_classify(port, skb);
380 	LAN966X_SKB_CB(skb)->rew_op = rew_op;
381 
382 	if (rew_op != IFH_REW_OP_TWO_STEP_PTP)
383 		return 0;
384 
385 	lan966x_ptp_txtstamp_old_release(port);
386 
387 	spin_lock_irqsave(&lan966x->ptp_ts_id_lock, flags);
388 	if (lan966x->ptp_skbs == LAN966X_MAX_PTP_ID) {
389 		spin_unlock_irqrestore(&lan966x->ptp_ts_id_lock, flags);
390 		return -EBUSY;
391 	}
392 
393 	skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
394 
395 	skb_queue_tail(&port->tx_skbs, skb);
396 	LAN966X_SKB_CB(skb)->ts_id = port->ts_id;
397 	LAN966X_SKB_CB(skb)->jiffies = jiffies;
398 
399 	lan966x->ptp_skbs++;
400 	port->ts_id++;
401 	if (port->ts_id == LAN966X_MAX_PTP_ID)
402 		port->ts_id = 0;
403 
404 	spin_unlock_irqrestore(&lan966x->ptp_ts_id_lock, flags);
405 
406 	return 0;
407 }
408 
409 void lan966x_ptp_txtstamp_release(struct lan966x_port *port,
410 				  struct sk_buff *skb)
411 {
412 	struct lan966x *lan966x = port->lan966x;
413 	unsigned long flags;
414 
415 	spin_lock_irqsave(&lan966x->ptp_ts_id_lock, flags);
416 	port->ts_id--;
417 	lan966x->ptp_skbs--;
418 	skb_unlink(skb, &port->tx_skbs);
419 	spin_unlock_irqrestore(&lan966x->ptp_ts_id_lock, flags);
420 }
421 
422 static void lan966x_get_hwtimestamp(struct lan966x *lan966x,
423 				    struct timespec64 *ts,
424 				    u32 nsec)
425 {
426 	/* Read current PTP time to get seconds */
427 	unsigned long flags;
428 	u32 curr_nsec;
429 
430 	spin_lock_irqsave(&lan966x->ptp_clock_lock, flags);
431 
432 	lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_SAVE) |
433 		PTP_PIN_CFG_PIN_DOM_SET(LAN966X_PHC_PORT) |
434 		PTP_PIN_CFG_PIN_SYNC_SET(0),
435 		PTP_PIN_CFG_PIN_ACTION |
436 		PTP_PIN_CFG_PIN_DOM |
437 		PTP_PIN_CFG_PIN_SYNC,
438 		lan966x, PTP_PIN_CFG(TOD_ACC_PIN));
439 
440 	ts->tv_sec = lan_rd(lan966x, PTP_TOD_SEC_LSB(TOD_ACC_PIN));
441 	curr_nsec = lan_rd(lan966x, PTP_TOD_NSEC(TOD_ACC_PIN));
442 
443 	ts->tv_nsec = nsec;
444 
445 	/* Sec has incremented since the ts was registered */
446 	if (curr_nsec < nsec)
447 		ts->tv_sec--;
448 
449 	spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags);
450 }
451 
452 irqreturn_t lan966x_ptp_irq_handler(int irq, void *args)
453 {
454 	int budget = LAN966X_MAX_PTP_ID;
455 	struct lan966x *lan966x = args;
456 
457 	while (budget--) {
458 		struct sk_buff *skb, *skb_tmp, *skb_match = NULL;
459 		struct skb_shared_hwtstamps shhwtstamps;
460 		struct lan966x_port *port;
461 		struct timespec64 ts;
462 		unsigned long flags;
463 		u32 val, id, txport;
464 		u32 delay;
465 
466 		val = lan_rd(lan966x, PTP_TWOSTEP_CTRL);
467 
468 		/* Check if a timestamp can be retrieved */
469 		if (!(val & PTP_TWOSTEP_CTRL_VLD))
470 			break;
471 
472 		WARN_ON(val & PTP_TWOSTEP_CTRL_OVFL);
473 
474 		if (!(val & PTP_TWOSTEP_CTRL_STAMP_TX))
475 			continue;
476 
477 		/* Retrieve the ts Tx port */
478 		txport = PTP_TWOSTEP_CTRL_STAMP_PORT_GET(val);
479 
480 		/* Retrieve its associated skb */
481 		port = lan966x->ports[txport];
482 
483 		/* Retrieve the delay */
484 		delay = lan_rd(lan966x, PTP_TWOSTEP_STAMP);
485 		delay = PTP_TWOSTEP_STAMP_STAMP_NSEC_GET(delay);
486 
487 		/* Get next timestamp from fifo, which needs to be the
488 		 * rx timestamp which represents the id of the frame
489 		 */
490 		lan_rmw(PTP_TWOSTEP_CTRL_NXT_SET(1),
491 			PTP_TWOSTEP_CTRL_NXT,
492 			lan966x, PTP_TWOSTEP_CTRL);
493 
494 		val = lan_rd(lan966x, PTP_TWOSTEP_CTRL);
495 
496 		/* Check if a timestamp can be retried */
497 		if (!(val & PTP_TWOSTEP_CTRL_VLD))
498 			break;
499 
500 		/* Read RX timestamping to get the ID */
501 		id = lan_rd(lan966x, PTP_TWOSTEP_STAMP);
502 
503 		spin_lock_irqsave(&port->tx_skbs.lock, flags);
504 		skb_queue_walk_safe(&port->tx_skbs, skb, skb_tmp) {
505 			if (LAN966X_SKB_CB(skb)->ts_id != id)
506 				continue;
507 
508 			__skb_unlink(skb, &port->tx_skbs);
509 			skb_match = skb;
510 			break;
511 		}
512 		spin_unlock_irqrestore(&port->tx_skbs.lock, flags);
513 
514 		/* Next ts */
515 		lan_rmw(PTP_TWOSTEP_CTRL_NXT_SET(1),
516 			PTP_TWOSTEP_CTRL_NXT,
517 			lan966x, PTP_TWOSTEP_CTRL);
518 
519 		if (WARN_ON(!skb_match))
520 			continue;
521 
522 		spin_lock_irqsave(&lan966x->ptp_ts_id_lock, flags);
523 		lan966x->ptp_skbs--;
524 		spin_unlock_irqrestore(&lan966x->ptp_ts_id_lock, flags);
525 
526 		/* Get the h/w timestamp */
527 		lan966x_get_hwtimestamp(lan966x, &ts, delay);
528 
529 		/* Set the timestamp into the skb */
530 		shhwtstamps.hwtstamp = ktime_set(ts.tv_sec, ts.tv_nsec);
531 		skb_tstamp_tx(skb_match, &shhwtstamps);
532 
533 		dev_kfree_skb_any(skb_match);
534 	}
535 
536 	return IRQ_HANDLED;
537 }
538 
539 irqreturn_t lan966x_ptp_ext_irq_handler(int irq, void *args)
540 {
541 	struct lan966x *lan966x = args;
542 	struct lan966x_phc *phc;
543 	unsigned long flags;
544 	u64 time = 0;
545 	time64_t s;
546 	int pin, i;
547 	s64 ns;
548 
549 	if (!(lan_rd(lan966x, PTP_PIN_INTR)))
550 		return IRQ_NONE;
551 
552 	/* Go through all domains and see which pin generated the interrupt */
553 	for (i = 0; i < LAN966X_PHC_COUNT; ++i) {
554 		struct ptp_clock_event ptp_event = {0};
555 
556 		phc = &lan966x->phc[i];
557 		pin = ptp_find_pin_unlocked(phc->clock, PTP_PF_EXTTS, 0);
558 		if (pin == -1)
559 			continue;
560 
561 		if (!(lan_rd(lan966x, PTP_PIN_INTR) & BIT(pin)))
562 			continue;
563 
564 		spin_lock_irqsave(&lan966x->ptp_clock_lock, flags);
565 
566 		/* Enable to get the new interrupt.
567 		 * By writing 1 it clears the bit
568 		 */
569 		lan_wr(BIT(pin), lan966x, PTP_PIN_INTR);
570 
571 		/* Get current time */
572 		s = lan_rd(lan966x, PTP_TOD_SEC_MSB(pin));
573 		s <<= 32;
574 		s |= lan_rd(lan966x, PTP_TOD_SEC_LSB(pin));
575 		ns = lan_rd(lan966x, PTP_TOD_NSEC(pin));
576 		ns &= PTP_TOD_NSEC_TOD_NSEC;
577 
578 		spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags);
579 
580 		if ((ns & 0xFFFFFFF0) == 0x3FFFFFF0) {
581 			s--;
582 			ns &= 0xf;
583 			ns += 999999984;
584 		}
585 		time = ktime_set(s, ns);
586 
587 		ptp_event.index = pin;
588 		ptp_event.timestamp = time;
589 		ptp_event.type = PTP_CLOCK_EXTTS;
590 		ptp_clock_event(phc->clock, &ptp_event);
591 	}
592 
593 	return IRQ_HANDLED;
594 }
595 
596 static int lan966x_ptp_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)
597 {
598 	struct lan966x_phc *phc = container_of(ptp, struct lan966x_phc, info);
599 	struct lan966x *lan966x = phc->lan966x;
600 	unsigned long flags;
601 	bool neg_adj = 0;
602 	u64 tod_inc;
603 	u64 ref;
604 
605 	if (!scaled_ppm)
606 		return 0;
607 
608 	if (scaled_ppm < 0) {
609 		neg_adj = 1;
610 		scaled_ppm = -scaled_ppm;
611 	}
612 
613 	tod_inc = lan966x_ptp_get_nominal_value();
614 
615 	/* The multiplication is split in 2 separate additions because of
616 	 * overflow issues. If scaled_ppm with 16bit fractional part was bigger
617 	 * than 20ppm then we got overflow.
618 	 */
619 	ref = LAN966X_1PPM_FORMAT * (scaled_ppm >> 16);
620 	ref += (LAN966X_1PPM_FORMAT * (0xffff & scaled_ppm)) >> 16;
621 	tod_inc = neg_adj ? tod_inc - ref : tod_inc + ref;
622 
623 	spin_lock_irqsave(&lan966x->ptp_clock_lock, flags);
624 
625 	lan_rmw(PTP_DOM_CFG_CLKCFG_DIS_SET(1 << BIT(phc->index)),
626 		PTP_DOM_CFG_CLKCFG_DIS,
627 		lan966x, PTP_DOM_CFG);
628 
629 	lan_wr((u32)tod_inc & 0xFFFFFFFF, lan966x,
630 	       PTP_CLK_PER_CFG(phc->index, 0));
631 	lan_wr((u32)(tod_inc >> 32), lan966x,
632 	       PTP_CLK_PER_CFG(phc->index, 1));
633 
634 	lan_rmw(PTP_DOM_CFG_CLKCFG_DIS_SET(0),
635 		PTP_DOM_CFG_CLKCFG_DIS,
636 		lan966x, PTP_DOM_CFG);
637 
638 	spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags);
639 
640 	return 0;
641 }
642 
643 static int lan966x_ptp_settime64(struct ptp_clock_info *ptp,
644 				 const struct timespec64 *ts)
645 {
646 	struct lan966x_phc *phc = container_of(ptp, struct lan966x_phc, info);
647 	struct lan966x *lan966x = phc->lan966x;
648 	unsigned long flags;
649 
650 	spin_lock_irqsave(&lan966x->ptp_clock_lock, flags);
651 
652 	/* Must be in IDLE mode before the time can be loaded */
653 	lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_IDLE) |
654 		PTP_PIN_CFG_PIN_DOM_SET(phc->index) |
655 		PTP_PIN_CFG_PIN_SYNC_SET(0),
656 		PTP_PIN_CFG_PIN_ACTION |
657 		PTP_PIN_CFG_PIN_DOM |
658 		PTP_PIN_CFG_PIN_SYNC,
659 		lan966x, PTP_PIN_CFG(TOD_ACC_PIN));
660 
661 	/* Set new value */
662 	lan_wr(PTP_TOD_SEC_MSB_TOD_SEC_MSB_SET(upper_32_bits(ts->tv_sec)),
663 	       lan966x, PTP_TOD_SEC_MSB(TOD_ACC_PIN));
664 	lan_wr(lower_32_bits(ts->tv_sec),
665 	       lan966x, PTP_TOD_SEC_LSB(TOD_ACC_PIN));
666 	lan_wr(ts->tv_nsec, lan966x, PTP_TOD_NSEC(TOD_ACC_PIN));
667 
668 	/* Apply new values */
669 	lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_LOAD) |
670 		PTP_PIN_CFG_PIN_DOM_SET(phc->index) |
671 		PTP_PIN_CFG_PIN_SYNC_SET(0),
672 		PTP_PIN_CFG_PIN_ACTION |
673 		PTP_PIN_CFG_PIN_DOM |
674 		PTP_PIN_CFG_PIN_SYNC,
675 		lan966x, PTP_PIN_CFG(TOD_ACC_PIN));
676 
677 	spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags);
678 
679 	return 0;
680 }
681 
682 int lan966x_ptp_gettime64(struct ptp_clock_info *ptp, struct timespec64 *ts)
683 {
684 	struct lan966x_phc *phc = container_of(ptp, struct lan966x_phc, info);
685 	struct lan966x *lan966x = phc->lan966x;
686 	unsigned long flags;
687 	time64_t s;
688 	s64 ns;
689 
690 	spin_lock_irqsave(&lan966x->ptp_clock_lock, flags);
691 
692 	lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_SAVE) |
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 	s = lan_rd(lan966x, PTP_TOD_SEC_MSB(TOD_ACC_PIN));
701 	s <<= 32;
702 	s |= lan_rd(lan966x, PTP_TOD_SEC_LSB(TOD_ACC_PIN));
703 	ns = lan_rd(lan966x, PTP_TOD_NSEC(TOD_ACC_PIN));
704 	ns &= PTP_TOD_NSEC_TOD_NSEC;
705 
706 	spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags);
707 
708 	/* Deal with negative values */
709 	if ((ns & 0xFFFFFFF0) == 0x3FFFFFF0) {
710 		s--;
711 		ns &= 0xf;
712 		ns += 999999984;
713 	}
714 
715 	set_normalized_timespec64(ts, s, ns);
716 	return 0;
717 }
718 
719 static int lan966x_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
720 {
721 	struct lan966x_phc *phc = container_of(ptp, struct lan966x_phc, info);
722 	struct lan966x *lan966x = phc->lan966x;
723 
724 	if (delta > -(NSEC_PER_SEC / 2) && delta < (NSEC_PER_SEC / 2)) {
725 		unsigned long flags;
726 
727 		spin_lock_irqsave(&lan966x->ptp_clock_lock, flags);
728 
729 		/* Must be in IDLE mode before the time can be loaded */
730 		lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_IDLE) |
731 			PTP_PIN_CFG_PIN_DOM_SET(phc->index) |
732 			PTP_PIN_CFG_PIN_SYNC_SET(0),
733 			PTP_PIN_CFG_PIN_ACTION |
734 			PTP_PIN_CFG_PIN_DOM |
735 			PTP_PIN_CFG_PIN_SYNC,
736 			lan966x, PTP_PIN_CFG(TOD_ACC_PIN));
737 
738 		lan_wr(PTP_TOD_NSEC_TOD_NSEC_SET(delta),
739 		       lan966x, PTP_TOD_NSEC(TOD_ACC_PIN));
740 
741 		/* Adjust time with the value of PTP_TOD_NSEC */
742 		lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_DELTA) |
743 			PTP_PIN_CFG_PIN_DOM_SET(phc->index) |
744 			PTP_PIN_CFG_PIN_SYNC_SET(0),
745 			PTP_PIN_CFG_PIN_ACTION |
746 			PTP_PIN_CFG_PIN_DOM |
747 			PTP_PIN_CFG_PIN_SYNC,
748 			lan966x, PTP_PIN_CFG(TOD_ACC_PIN));
749 
750 		spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags);
751 	} else {
752 		/* Fall back using lan966x_ptp_settime64 which is not exact */
753 		struct timespec64 ts;
754 		u64 now;
755 
756 		lan966x_ptp_gettime64(ptp, &ts);
757 
758 		now = ktime_to_ns(timespec64_to_ktime(ts));
759 		ts = ns_to_timespec64(now + delta);
760 
761 		lan966x_ptp_settime64(ptp, &ts);
762 	}
763 
764 	return 0;
765 }
766 
767 static int lan966x_ptp_verify(struct ptp_clock_info *ptp, unsigned int pin,
768 			      enum ptp_pin_function func, unsigned int chan)
769 {
770 	struct lan966x_phc *phc = container_of(ptp, struct lan966x_phc, info);
771 	struct lan966x *lan966x = phc->lan966x;
772 	struct ptp_clock_info *info;
773 	int i;
774 
775 	/* Currently support only 1 channel */
776 	if (chan != 0)
777 		return -1;
778 
779 	switch (func) {
780 	case PTP_PF_NONE:
781 	case PTP_PF_PEROUT:
782 	case PTP_PF_EXTTS:
783 		break;
784 	default:
785 		return -1;
786 	}
787 
788 	/* The PTP pins are shared by all the PHC. So it is required to see if
789 	 * the pin is connected to another PHC. The pin is connected to another
790 	 * PHC if that pin already has a function on that PHC.
791 	 */
792 	for (i = 0; i < LAN966X_PHC_COUNT; ++i) {
793 		info = &lan966x->phc[i].info;
794 
795 		/* Ignore the check with ourself */
796 		if (ptp == info)
797 			continue;
798 
799 		if (info->pin_config[pin].func == PTP_PF_PEROUT ||
800 		    info->pin_config[pin].func == PTP_PF_EXTTS)
801 			return -1;
802 	}
803 
804 	return 0;
805 }
806 
807 static int lan966x_ptp_perout(struct ptp_clock_info *ptp,
808 			      struct ptp_clock_request *rq, int on)
809 {
810 	struct lan966x_phc *phc = container_of(ptp, struct lan966x_phc, info);
811 	struct lan966x *lan966x = phc->lan966x;
812 	struct timespec64 ts_phase, ts_period;
813 	unsigned long flags;
814 	s64 wf_high, wf_low;
815 	bool pps = false;
816 	int pin;
817 
818 	if (rq->perout.flags & ~(PTP_PEROUT_DUTY_CYCLE |
819 				 PTP_PEROUT_PHASE))
820 		return -EOPNOTSUPP;
821 
822 	pin = ptp_find_pin(phc->clock, PTP_PF_PEROUT, rq->perout.index);
823 	if (pin == -1 || pin >= LAN966X_PHC_PINS_NUM)
824 		return -EINVAL;
825 
826 	if (!on) {
827 		spin_lock_irqsave(&lan966x->ptp_clock_lock, flags);
828 		lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_IDLE) |
829 			PTP_PIN_CFG_PIN_DOM_SET(phc->index) |
830 			PTP_PIN_CFG_PIN_SYNC_SET(0),
831 			PTP_PIN_CFG_PIN_ACTION |
832 			PTP_PIN_CFG_PIN_DOM |
833 			PTP_PIN_CFG_PIN_SYNC,
834 			lan966x, PTP_PIN_CFG(pin));
835 		spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags);
836 		return 0;
837 	}
838 
839 	if (rq->perout.period.sec == 1 &&
840 	    rq->perout.period.nsec == 0)
841 		pps = true;
842 
843 	if (rq->perout.flags & PTP_PEROUT_PHASE) {
844 		ts_phase.tv_sec = rq->perout.phase.sec;
845 		ts_phase.tv_nsec = rq->perout.phase.nsec;
846 	} else {
847 		ts_phase.tv_sec = rq->perout.start.sec;
848 		ts_phase.tv_nsec = rq->perout.start.nsec;
849 	}
850 
851 	if (ts_phase.tv_sec || (ts_phase.tv_nsec && !pps)) {
852 		dev_warn(lan966x->dev,
853 			 "Absolute time not supported!\n");
854 		return -EINVAL;
855 	}
856 
857 	if (rq->perout.flags & PTP_PEROUT_DUTY_CYCLE) {
858 		struct timespec64 ts_on;
859 
860 		ts_on.tv_sec = rq->perout.on.sec;
861 		ts_on.tv_nsec = rq->perout.on.nsec;
862 
863 		wf_high = timespec64_to_ns(&ts_on);
864 	} else {
865 		wf_high = 5000;
866 	}
867 
868 	if (pps) {
869 		spin_lock_irqsave(&lan966x->ptp_clock_lock, flags);
870 		lan_wr(PTP_WF_LOW_PERIOD_PIN_WFL(ts_phase.tv_nsec),
871 		       lan966x, PTP_WF_LOW_PERIOD(pin));
872 		lan_wr(PTP_WF_HIGH_PERIOD_PIN_WFH(wf_high),
873 		       lan966x, PTP_WF_HIGH_PERIOD(pin));
874 		lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_CLOCK) |
875 			PTP_PIN_CFG_PIN_DOM_SET(phc->index) |
876 			PTP_PIN_CFG_PIN_SYNC_SET(3),
877 			PTP_PIN_CFG_PIN_ACTION |
878 			PTP_PIN_CFG_PIN_DOM |
879 			PTP_PIN_CFG_PIN_SYNC,
880 			lan966x, PTP_PIN_CFG(pin));
881 		spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags);
882 		return 0;
883 	}
884 
885 	ts_period.tv_sec = rq->perout.period.sec;
886 	ts_period.tv_nsec = rq->perout.period.nsec;
887 
888 	wf_low = timespec64_to_ns(&ts_period);
889 	wf_low -= wf_high;
890 
891 	spin_lock_irqsave(&lan966x->ptp_clock_lock, flags);
892 	lan_wr(PTP_WF_LOW_PERIOD_PIN_WFL(wf_low),
893 	       lan966x, PTP_WF_LOW_PERIOD(pin));
894 	lan_wr(PTP_WF_HIGH_PERIOD_PIN_WFH(wf_high),
895 	       lan966x, PTP_WF_HIGH_PERIOD(pin));
896 	lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_CLOCK) |
897 		PTP_PIN_CFG_PIN_DOM_SET(phc->index) |
898 		PTP_PIN_CFG_PIN_SYNC_SET(0),
899 		PTP_PIN_CFG_PIN_ACTION |
900 		PTP_PIN_CFG_PIN_DOM |
901 		PTP_PIN_CFG_PIN_SYNC,
902 		lan966x, PTP_PIN_CFG(pin));
903 	spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags);
904 
905 	return 0;
906 }
907 
908 static int lan966x_ptp_extts(struct ptp_clock_info *ptp,
909 			     struct ptp_clock_request *rq, int on)
910 {
911 	struct lan966x_phc *phc = container_of(ptp, struct lan966x_phc, info);
912 	struct lan966x *lan966x = phc->lan966x;
913 	unsigned long flags;
914 	int pin;
915 	u32 val;
916 
917 	if (lan966x->ptp_ext_irq <= 0)
918 		return -EOPNOTSUPP;
919 
920 	/* Reject requests with unsupported flags */
921 	if (rq->extts.flags & ~(PTP_ENABLE_FEATURE |
922 				PTP_RISING_EDGE |
923 				PTP_STRICT_FLAGS))
924 		return -EOPNOTSUPP;
925 
926 	pin = ptp_find_pin(phc->clock, PTP_PF_EXTTS, rq->extts.index);
927 	if (pin == -1 || pin >= LAN966X_PHC_PINS_NUM)
928 		return -EINVAL;
929 
930 	spin_lock_irqsave(&lan966x->ptp_clock_lock, flags);
931 	lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_SAVE) |
932 		PTP_PIN_CFG_PIN_SYNC_SET(on ? 3 : 0) |
933 		PTP_PIN_CFG_PIN_DOM_SET(phc->index) |
934 		PTP_PIN_CFG_PIN_SELECT_SET(pin),
935 		PTP_PIN_CFG_PIN_ACTION |
936 		PTP_PIN_CFG_PIN_SYNC |
937 		PTP_PIN_CFG_PIN_DOM |
938 		PTP_PIN_CFG_PIN_SELECT,
939 		lan966x, PTP_PIN_CFG(pin));
940 
941 	val = lan_rd(lan966x, PTP_PIN_INTR_ENA);
942 	if (on)
943 		val |= BIT(pin);
944 	else
945 		val &= ~BIT(pin);
946 	lan_wr(val, lan966x, PTP_PIN_INTR_ENA);
947 
948 	spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags);
949 
950 	return 0;
951 }
952 
953 static int lan966x_ptp_enable(struct ptp_clock_info *ptp,
954 			      struct ptp_clock_request *rq, int on)
955 {
956 	switch (rq->type) {
957 	case PTP_CLK_REQ_PEROUT:
958 		return lan966x_ptp_perout(ptp, rq, on);
959 	case PTP_CLK_REQ_EXTTS:
960 		return lan966x_ptp_extts(ptp, rq, on);
961 	default:
962 		return -EOPNOTSUPP;
963 	}
964 
965 	return 0;
966 }
967 
968 static struct ptp_clock_info lan966x_ptp_clock_info = {
969 	.owner		= THIS_MODULE,
970 	.name		= "lan966x ptp",
971 	.max_adj	= 200000,
972 	.gettime64	= lan966x_ptp_gettime64,
973 	.settime64	= lan966x_ptp_settime64,
974 	.adjtime	= lan966x_ptp_adjtime,
975 	.adjfine	= lan966x_ptp_adjfine,
976 	.verify		= lan966x_ptp_verify,
977 	.enable		= lan966x_ptp_enable,
978 	.n_per_out	= LAN966X_PHC_PINS_NUM,
979 	.n_ext_ts	= LAN966X_PHC_PINS_NUM,
980 	.n_pins		= LAN966X_PHC_PINS_NUM,
981 };
982 
983 static int lan966x_ptp_phc_init(struct lan966x *lan966x,
984 				int index,
985 				struct ptp_clock_info *clock_info)
986 {
987 	struct lan966x_phc *phc = &lan966x->phc[index];
988 	struct ptp_pin_desc *p;
989 	int i;
990 
991 	for (i = 0; i < LAN966X_PHC_PINS_NUM; i++) {
992 		p = &phc->pins[i];
993 
994 		snprintf(p->name, sizeof(p->name), "pin%d", i);
995 		p->index = i;
996 		p->func = PTP_PF_NONE;
997 	}
998 
999 	phc->info = *clock_info;
1000 	phc->info.pin_config = &phc->pins[0];
1001 	phc->clock = ptp_clock_register(&phc->info, lan966x->dev);
1002 	if (IS_ERR(phc->clock))
1003 		return PTR_ERR(phc->clock);
1004 
1005 	phc->index = index;
1006 	phc->lan966x = lan966x;
1007 
1008 	return 0;
1009 }
1010 
1011 int lan966x_ptp_init(struct lan966x *lan966x)
1012 {
1013 	u64 tod_adj = lan966x_ptp_get_nominal_value();
1014 	struct lan966x_port *port;
1015 	int err, i;
1016 
1017 	if (!lan966x->ptp)
1018 		return 0;
1019 
1020 	for (i = 0; i < LAN966X_PHC_COUNT; ++i) {
1021 		err = lan966x_ptp_phc_init(lan966x, i, &lan966x_ptp_clock_info);
1022 		if (err)
1023 			return err;
1024 	}
1025 
1026 	spin_lock_init(&lan966x->ptp_clock_lock);
1027 	spin_lock_init(&lan966x->ptp_ts_id_lock);
1028 	mutex_init(&lan966x->ptp_lock);
1029 
1030 	/* Disable master counters */
1031 	lan_wr(PTP_DOM_CFG_ENA_SET(0), lan966x, PTP_DOM_CFG);
1032 
1033 	/* Configure the nominal TOD increment per clock cycle */
1034 	lan_rmw(PTP_DOM_CFG_CLKCFG_DIS_SET(0x7),
1035 		PTP_DOM_CFG_CLKCFG_DIS,
1036 		lan966x, PTP_DOM_CFG);
1037 
1038 	for (i = 0; i < LAN966X_PHC_COUNT; ++i) {
1039 		lan_wr((u32)tod_adj & 0xFFFFFFFF, lan966x,
1040 		       PTP_CLK_PER_CFG(i, 0));
1041 		lan_wr((u32)(tod_adj >> 32), lan966x,
1042 		       PTP_CLK_PER_CFG(i, 1));
1043 	}
1044 
1045 	lan_rmw(PTP_DOM_CFG_CLKCFG_DIS_SET(0),
1046 		PTP_DOM_CFG_CLKCFG_DIS,
1047 		lan966x, PTP_DOM_CFG);
1048 
1049 	/* Enable master counters */
1050 	lan_wr(PTP_DOM_CFG_ENA_SET(0x7), lan966x, PTP_DOM_CFG);
1051 
1052 	for (i = 0; i < lan966x->num_phys_ports; i++) {
1053 		port = lan966x->ports[i];
1054 		if (!port)
1055 			continue;
1056 
1057 		skb_queue_head_init(&port->tx_skbs);
1058 	}
1059 
1060 	return 0;
1061 }
1062 
1063 void lan966x_ptp_deinit(struct lan966x *lan966x)
1064 {
1065 	struct lan966x_port *port;
1066 	int i;
1067 
1068 	if (!lan966x->ptp)
1069 		return;
1070 
1071 	for (i = 0; i < lan966x->num_phys_ports; i++) {
1072 		port = lan966x->ports[i];
1073 		if (!port)
1074 			continue;
1075 
1076 		skb_queue_purge(&port->tx_skbs);
1077 	}
1078 
1079 	for (i = 0; i < LAN966X_PHC_COUNT; ++i)
1080 		ptp_clock_unregister(lan966x->phc[i].clock);
1081 }
1082 
1083 void lan966x_ptp_rxtstamp(struct lan966x *lan966x, struct sk_buff *skb,
1084 			  u64 src_port, u64 timestamp)
1085 {
1086 	struct skb_shared_hwtstamps *shhwtstamps;
1087 	struct lan966x_phc *phc;
1088 	struct timespec64 ts;
1089 	u64 full_ts_in_ns;
1090 
1091 	if (!lan966x->ptp ||
1092 	    !lan966x->ports[src_port]->ptp_rx_cmd)
1093 		return;
1094 
1095 	phc = &lan966x->phc[LAN966X_PHC_PORT];
1096 	lan966x_ptp_gettime64(&phc->info, &ts);
1097 
1098 	/* Drop the sub-ns precision */
1099 	timestamp = timestamp >> 2;
1100 	if (ts.tv_nsec < timestamp)
1101 		ts.tv_sec--;
1102 	ts.tv_nsec = timestamp;
1103 	full_ts_in_ns = ktime_set(ts.tv_sec, ts.tv_nsec);
1104 
1105 	shhwtstamps = skb_hwtstamps(skb);
1106 	shhwtstamps->hwtstamp = full_ts_in_ns;
1107 }
1108 
1109 u32 lan966x_ptp_get_period_ps(void)
1110 {
1111 	/* This represents the system clock period in picoseconds */
1112 	return 15125;
1113 }
1114