1 // SPDX-License-Identifier: BSD-3-Clause OR GPL-2.0
2 /* Copyright (c) 2019 Mellanox Technologies. All rights reserved */
3 
4 #include <linux/ptp_clock_kernel.h>
5 #include <linux/clocksource.h>
6 #include <linux/timecounter.h>
7 #include <linux/spinlock.h>
8 #include <linux/device.h>
9 #include <linux/rhashtable.h>
10 #include <linux/ptp_classify.h>
11 #include <linux/if_ether.h>
12 #include <linux/if_vlan.h>
13 #include <linux/net_tstamp.h>
14 
15 #include "spectrum.h"
16 #include "spectrum_ptp.h"
17 #include "core.h"
18 
19 #define MLXSW_SP1_PTP_CLOCK_CYCLES_SHIFT	29
20 #define MLXSW_SP1_PTP_CLOCK_FREQ_KHZ		156257 /* 6.4nSec */
21 #define MLXSW_SP1_PTP_CLOCK_MASK		64
22 
23 #define MLXSW_SP1_PTP_HT_GC_INTERVAL		500 /* ms */
24 
25 /* How long, approximately, should the unmatched entries stay in the hash table
26  * before they are collected. Should be evenly divisible by the GC interval.
27  */
28 #define MLXSW_SP1_PTP_HT_GC_TIMEOUT		1000 /* ms */
29 
30 struct mlxsw_sp_ptp_state {
31 	struct mlxsw_sp *mlxsw_sp;
32 	struct rhltable unmatched_ht;
33 	spinlock_t unmatched_lock; /* protects the HT */
34 	struct delayed_work ht_gc_dw;
35 	u32 gc_cycle;
36 };
37 
38 struct mlxsw_sp1_ptp_key {
39 	u8 local_port;
40 	u8 message_type;
41 	u16 sequence_id;
42 	u8 domain_number;
43 	bool ingress;
44 };
45 
46 struct mlxsw_sp1_ptp_unmatched {
47 	struct mlxsw_sp1_ptp_key key;
48 	struct rhlist_head ht_node;
49 	struct rcu_head rcu;
50 	struct sk_buff *skb;
51 	u64 timestamp;
52 	u32 gc_cycle;
53 };
54 
55 static const struct rhashtable_params mlxsw_sp1_ptp_unmatched_ht_params = {
56 	.key_len = sizeof_field(struct mlxsw_sp1_ptp_unmatched, key),
57 	.key_offset = offsetof(struct mlxsw_sp1_ptp_unmatched, key),
58 	.head_offset = offsetof(struct mlxsw_sp1_ptp_unmatched, ht_node),
59 };
60 
61 struct mlxsw_sp_ptp_clock {
62 	struct mlxsw_core *core;
63 	spinlock_t lock; /* protect this structure */
64 	struct cyclecounter cycles;
65 	struct timecounter tc;
66 	u32 nominal_c_mult;
67 	struct ptp_clock *ptp;
68 	struct ptp_clock_info ptp_info;
69 	unsigned long overflow_period;
70 	struct delayed_work overflow_work;
71 };
72 
73 static u64 __mlxsw_sp1_ptp_read_frc(struct mlxsw_sp_ptp_clock *clock,
74 				    struct ptp_system_timestamp *sts)
75 {
76 	struct mlxsw_core *mlxsw_core = clock->core;
77 	u32 frc_h1, frc_h2, frc_l;
78 
79 	frc_h1 = mlxsw_core_read_frc_h(mlxsw_core);
80 	ptp_read_system_prets(sts);
81 	frc_l = mlxsw_core_read_frc_l(mlxsw_core);
82 	ptp_read_system_postts(sts);
83 	frc_h2 = mlxsw_core_read_frc_h(mlxsw_core);
84 
85 	if (frc_h1 != frc_h2) {
86 		/* wrap around */
87 		ptp_read_system_prets(sts);
88 		frc_l = mlxsw_core_read_frc_l(mlxsw_core);
89 		ptp_read_system_postts(sts);
90 	}
91 
92 	return (u64) frc_l | (u64) frc_h2 << 32;
93 }
94 
95 static u64 mlxsw_sp1_ptp_read_frc(const struct cyclecounter *cc)
96 {
97 	struct mlxsw_sp_ptp_clock *clock =
98 		container_of(cc, struct mlxsw_sp_ptp_clock, cycles);
99 
100 	return __mlxsw_sp1_ptp_read_frc(clock, NULL) & cc->mask;
101 }
102 
103 static int
104 mlxsw_sp1_ptp_phc_adjfreq(struct mlxsw_sp_ptp_clock *clock, int freq_adj)
105 {
106 	struct mlxsw_core *mlxsw_core = clock->core;
107 	char mtutc_pl[MLXSW_REG_MTUTC_LEN];
108 
109 	mlxsw_reg_mtutc_pack(mtutc_pl, MLXSW_REG_MTUTC_OPERATION_ADJUST_FREQ,
110 			     freq_adj, 0);
111 	return mlxsw_reg_write(mlxsw_core, MLXSW_REG(mtutc), mtutc_pl);
112 }
113 
114 static u64 mlxsw_sp1_ptp_ns2cycles(const struct timecounter *tc, u64 nsec)
115 {
116 	u64 cycles = (u64) nsec;
117 
118 	cycles <<= tc->cc->shift;
119 	cycles = div_u64(cycles, tc->cc->mult);
120 
121 	return cycles;
122 }
123 
124 static int
125 mlxsw_sp1_ptp_phc_settime(struct mlxsw_sp_ptp_clock *clock, u64 nsec)
126 {
127 	struct mlxsw_core *mlxsw_core = clock->core;
128 	u64 next_sec, next_sec_in_nsec, cycles;
129 	char mtutc_pl[MLXSW_REG_MTUTC_LEN];
130 	char mtpps_pl[MLXSW_REG_MTPPS_LEN];
131 	int err;
132 
133 	next_sec = div_u64(nsec, NSEC_PER_SEC) + 1;
134 	next_sec_in_nsec = next_sec * NSEC_PER_SEC;
135 
136 	spin_lock_bh(&clock->lock);
137 	cycles = mlxsw_sp1_ptp_ns2cycles(&clock->tc, next_sec_in_nsec);
138 	spin_unlock_bh(&clock->lock);
139 
140 	mlxsw_reg_mtpps_vpin_pack(mtpps_pl, cycles);
141 	err = mlxsw_reg_write(mlxsw_core, MLXSW_REG(mtpps), mtpps_pl);
142 	if (err)
143 		return err;
144 
145 	mlxsw_reg_mtutc_pack(mtutc_pl,
146 			     MLXSW_REG_MTUTC_OPERATION_SET_TIME_AT_NEXT_SEC,
147 			     0, next_sec);
148 	return mlxsw_reg_write(mlxsw_core, MLXSW_REG(mtutc), mtutc_pl);
149 }
150 
151 static int mlxsw_sp1_ptp_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)
152 {
153 	struct mlxsw_sp_ptp_clock *clock =
154 		container_of(ptp, struct mlxsw_sp_ptp_clock, ptp_info);
155 	int neg_adj = 0;
156 	u32 diff;
157 	u64 adj;
158 	s32 ppb;
159 
160 	ppb = scaled_ppm_to_ppb(scaled_ppm);
161 
162 	if (ppb < 0) {
163 		neg_adj = 1;
164 		ppb = -ppb;
165 	}
166 
167 	adj = clock->nominal_c_mult;
168 	adj *= ppb;
169 	diff = div_u64(adj, NSEC_PER_SEC);
170 
171 	spin_lock_bh(&clock->lock);
172 	timecounter_read(&clock->tc);
173 	clock->cycles.mult = neg_adj ? clock->nominal_c_mult - diff :
174 				       clock->nominal_c_mult + diff;
175 	spin_unlock_bh(&clock->lock);
176 
177 	return mlxsw_sp1_ptp_phc_adjfreq(clock, neg_adj ? -ppb : ppb);
178 }
179 
180 static int mlxsw_sp1_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
181 {
182 	struct mlxsw_sp_ptp_clock *clock =
183 		container_of(ptp, struct mlxsw_sp_ptp_clock, ptp_info);
184 	u64 nsec;
185 
186 	spin_lock_bh(&clock->lock);
187 	timecounter_adjtime(&clock->tc, delta);
188 	nsec = timecounter_read(&clock->tc);
189 	spin_unlock_bh(&clock->lock);
190 
191 	return mlxsw_sp1_ptp_phc_settime(clock, nsec);
192 }
193 
194 static int mlxsw_sp1_ptp_gettimex(struct ptp_clock_info *ptp,
195 				  struct timespec64 *ts,
196 				  struct ptp_system_timestamp *sts)
197 {
198 	struct mlxsw_sp_ptp_clock *clock =
199 		container_of(ptp, struct mlxsw_sp_ptp_clock, ptp_info);
200 	u64 cycles, nsec;
201 
202 	spin_lock_bh(&clock->lock);
203 	cycles = __mlxsw_sp1_ptp_read_frc(clock, sts);
204 	nsec = timecounter_cyc2time(&clock->tc, cycles);
205 	spin_unlock_bh(&clock->lock);
206 
207 	*ts = ns_to_timespec64(nsec);
208 
209 	return 0;
210 }
211 
212 static int mlxsw_sp1_ptp_settime(struct ptp_clock_info *ptp,
213 				 const struct timespec64 *ts)
214 {
215 	struct mlxsw_sp_ptp_clock *clock =
216 		container_of(ptp, struct mlxsw_sp_ptp_clock, ptp_info);
217 	u64 nsec = timespec64_to_ns(ts);
218 
219 	spin_lock_bh(&clock->lock);
220 	timecounter_init(&clock->tc, &clock->cycles, nsec);
221 	nsec = timecounter_read(&clock->tc);
222 	spin_unlock_bh(&clock->lock);
223 
224 	return mlxsw_sp1_ptp_phc_settime(clock, nsec);
225 }
226 
227 static const struct ptp_clock_info mlxsw_sp1_ptp_clock_info = {
228 	.owner		= THIS_MODULE,
229 	.name		= "mlxsw_sp_clock",
230 	.max_adj	= 100000000,
231 	.adjfine	= mlxsw_sp1_ptp_adjfine,
232 	.adjtime	= mlxsw_sp1_ptp_adjtime,
233 	.gettimex64	= mlxsw_sp1_ptp_gettimex,
234 	.settime64	= mlxsw_sp1_ptp_settime,
235 };
236 
237 static void mlxsw_sp1_ptp_clock_overflow(struct work_struct *work)
238 {
239 	struct delayed_work *dwork = to_delayed_work(work);
240 	struct mlxsw_sp_ptp_clock *clock;
241 
242 	clock = container_of(dwork, struct mlxsw_sp_ptp_clock, overflow_work);
243 
244 	spin_lock_bh(&clock->lock);
245 	timecounter_read(&clock->tc);
246 	spin_unlock_bh(&clock->lock);
247 	mlxsw_core_schedule_dw(&clock->overflow_work, clock->overflow_period);
248 }
249 
250 struct mlxsw_sp_ptp_clock *
251 mlxsw_sp1_ptp_clock_init(struct mlxsw_sp *mlxsw_sp, struct device *dev)
252 {
253 	u64 overflow_cycles, nsec, frac = 0;
254 	struct mlxsw_sp_ptp_clock *clock;
255 	int err;
256 
257 	clock = kzalloc(sizeof(*clock), GFP_KERNEL);
258 	if (!clock)
259 		return ERR_PTR(-ENOMEM);
260 
261 	spin_lock_init(&clock->lock);
262 	clock->cycles.read = mlxsw_sp1_ptp_read_frc;
263 	clock->cycles.shift = MLXSW_SP1_PTP_CLOCK_CYCLES_SHIFT;
264 	clock->cycles.mult = clocksource_khz2mult(MLXSW_SP1_PTP_CLOCK_FREQ_KHZ,
265 						  clock->cycles.shift);
266 	clock->nominal_c_mult = clock->cycles.mult;
267 	clock->cycles.mask = CLOCKSOURCE_MASK(MLXSW_SP1_PTP_CLOCK_MASK);
268 	clock->core = mlxsw_sp->core;
269 
270 	timecounter_init(&clock->tc, &clock->cycles,
271 			 ktime_to_ns(ktime_get_real()));
272 
273 	/* Calculate period in seconds to call the overflow watchdog - to make
274 	 * sure counter is checked at least twice every wrap around.
275 	 * The period is calculated as the minimum between max HW cycles count
276 	 * (The clock source mask) and max amount of cycles that can be
277 	 * multiplied by clock multiplier where the result doesn't exceed
278 	 * 64bits.
279 	 */
280 	overflow_cycles = div64_u64(~0ULL >> 1, clock->cycles.mult);
281 	overflow_cycles = min(overflow_cycles, div_u64(clock->cycles.mask, 3));
282 
283 	nsec = cyclecounter_cyc2ns(&clock->cycles, overflow_cycles, 0, &frac);
284 	clock->overflow_period = nsecs_to_jiffies(nsec);
285 
286 	INIT_DELAYED_WORK(&clock->overflow_work, mlxsw_sp1_ptp_clock_overflow);
287 	mlxsw_core_schedule_dw(&clock->overflow_work, 0);
288 
289 	clock->ptp_info = mlxsw_sp1_ptp_clock_info;
290 	clock->ptp = ptp_clock_register(&clock->ptp_info, dev);
291 	if (IS_ERR(clock->ptp)) {
292 		err = PTR_ERR(clock->ptp);
293 		dev_err(dev, "ptp_clock_register failed %d\n", err);
294 		goto err_ptp_clock_register;
295 	}
296 
297 	return clock;
298 
299 err_ptp_clock_register:
300 	cancel_delayed_work_sync(&clock->overflow_work);
301 	kfree(clock);
302 	return ERR_PTR(err);
303 }
304 
305 void mlxsw_sp1_ptp_clock_fini(struct mlxsw_sp_ptp_clock *clock)
306 {
307 	ptp_clock_unregister(clock->ptp);
308 	cancel_delayed_work_sync(&clock->overflow_work);
309 	kfree(clock);
310 }
311 
312 static int mlxsw_sp_ptp_parse(struct sk_buff *skb,
313 			      u8 *p_domain_number,
314 			      u8 *p_message_type,
315 			      u16 *p_sequence_id)
316 {
317 	unsigned int offset = 0;
318 	unsigned int ptp_class;
319 	u8 *data;
320 
321 	data = skb_mac_header(skb);
322 	ptp_class = ptp_classify_raw(skb);
323 
324 	switch (ptp_class & PTP_CLASS_VMASK) {
325 	case PTP_CLASS_V1:
326 	case PTP_CLASS_V2:
327 		break;
328 	default:
329 		return -ERANGE;
330 	}
331 
332 	if (ptp_class & PTP_CLASS_VLAN)
333 		offset += VLAN_HLEN;
334 
335 	switch (ptp_class & PTP_CLASS_PMASK) {
336 	case PTP_CLASS_IPV4:
337 		offset += ETH_HLEN + IPV4_HLEN(data + offset) + UDP_HLEN;
338 		break;
339 	case PTP_CLASS_IPV6:
340 		offset += ETH_HLEN + IP6_HLEN + UDP_HLEN;
341 		break;
342 	case PTP_CLASS_L2:
343 		offset += ETH_HLEN;
344 		break;
345 	default:
346 		return -ERANGE;
347 	}
348 
349 	/* PTP header is 34 bytes. */
350 	if (skb->len < offset + 34)
351 		return -EINVAL;
352 
353 	*p_message_type = data[offset] & 0x0f;
354 	*p_domain_number = data[offset + 4];
355 	*p_sequence_id = (u16)(data[offset + 30]) << 8 | data[offset + 31];
356 	return 0;
357 }
358 
359 /* Returns NULL on successful insertion, a pointer on conflict, or an ERR_PTR on
360  * error.
361  */
362 static int
363 mlxsw_sp1_ptp_unmatched_save(struct mlxsw_sp *mlxsw_sp,
364 			     struct mlxsw_sp1_ptp_key key,
365 			     struct sk_buff *skb,
366 			     u64 timestamp)
367 {
368 	int cycles = MLXSW_SP1_PTP_HT_GC_TIMEOUT / MLXSW_SP1_PTP_HT_GC_INTERVAL;
369 	struct mlxsw_sp_ptp_state *ptp_state = mlxsw_sp->ptp_state;
370 	struct mlxsw_sp1_ptp_unmatched *unmatched;
371 	int err;
372 
373 	unmatched = kzalloc(sizeof(*unmatched), GFP_ATOMIC);
374 	if (!unmatched)
375 		return -ENOMEM;
376 
377 	unmatched->key = key;
378 	unmatched->skb = skb;
379 	unmatched->timestamp = timestamp;
380 	unmatched->gc_cycle = mlxsw_sp->ptp_state->gc_cycle + cycles;
381 
382 	err = rhltable_insert(&ptp_state->unmatched_ht, &unmatched->ht_node,
383 			      mlxsw_sp1_ptp_unmatched_ht_params);
384 	if (err)
385 		kfree(unmatched);
386 
387 	return err;
388 }
389 
390 static struct mlxsw_sp1_ptp_unmatched *
391 mlxsw_sp1_ptp_unmatched_lookup(struct mlxsw_sp *mlxsw_sp,
392 			       struct mlxsw_sp1_ptp_key key, int *p_length)
393 {
394 	struct mlxsw_sp1_ptp_unmatched *unmatched, *last = NULL;
395 	struct rhlist_head *tmp, *list;
396 	int length = 0;
397 
398 	list = rhltable_lookup(&mlxsw_sp->ptp_state->unmatched_ht, &key,
399 			       mlxsw_sp1_ptp_unmatched_ht_params);
400 	rhl_for_each_entry_rcu(unmatched, tmp, list, ht_node) {
401 		last = unmatched;
402 		length++;
403 	}
404 
405 	*p_length = length;
406 	return last;
407 }
408 
409 static int
410 mlxsw_sp1_ptp_unmatched_remove(struct mlxsw_sp *mlxsw_sp,
411 			       struct mlxsw_sp1_ptp_unmatched *unmatched)
412 {
413 	return rhltable_remove(&mlxsw_sp->ptp_state->unmatched_ht,
414 			       &unmatched->ht_node,
415 			       mlxsw_sp1_ptp_unmatched_ht_params);
416 }
417 
418 /* This function is called in the following scenarios:
419  *
420  * 1) When a packet is matched with its timestamp.
421  * 2) In several situation when it is necessary to immediately pass on
422  *    an SKB without a timestamp.
423  * 3) From GC indirectly through mlxsw_sp1_ptp_unmatched_finish().
424  *    This case is similar to 2) above.
425  */
426 static void mlxsw_sp1_ptp_packet_finish(struct mlxsw_sp *mlxsw_sp,
427 					struct sk_buff *skb, u8 local_port,
428 					bool ingress,
429 					struct skb_shared_hwtstamps *hwtstamps)
430 {
431 	struct mlxsw_sp_port *mlxsw_sp_port;
432 
433 	/* Between capturing the packet and finishing it, there is a window of
434 	 * opportunity for the originating port to go away (e.g. due to a
435 	 * split). Also make sure the SKB device reference is still valid.
436 	 */
437 	mlxsw_sp_port = mlxsw_sp->ports[local_port];
438 	if (!(mlxsw_sp_port && (!skb->dev || skb->dev == mlxsw_sp_port->dev))) {
439 		dev_kfree_skb_any(skb);
440 		return;
441 	}
442 
443 	if (ingress) {
444 		if (hwtstamps)
445 			*skb_hwtstamps(skb) = *hwtstamps;
446 		mlxsw_sp_rx_listener_no_mark_func(skb, local_port, mlxsw_sp);
447 	} else {
448 		/* skb_tstamp_tx() allows hwtstamps to be NULL. */
449 		skb_tstamp_tx(skb, hwtstamps);
450 		dev_kfree_skb_any(skb);
451 	}
452 }
453 
454 static void mlxsw_sp1_packet_timestamp(struct mlxsw_sp *mlxsw_sp,
455 				       struct mlxsw_sp1_ptp_key key,
456 				       struct sk_buff *skb,
457 				       u64 timestamp)
458 {
459 	struct skb_shared_hwtstamps hwtstamps;
460 	u64 nsec;
461 
462 	spin_lock_bh(&mlxsw_sp->clock->lock);
463 	nsec = timecounter_cyc2time(&mlxsw_sp->clock->tc, timestamp);
464 	spin_unlock_bh(&mlxsw_sp->clock->lock);
465 
466 	hwtstamps.hwtstamp = ns_to_ktime(nsec);
467 	mlxsw_sp1_ptp_packet_finish(mlxsw_sp, skb,
468 				    key.local_port, key.ingress, &hwtstamps);
469 }
470 
471 static void
472 mlxsw_sp1_ptp_unmatched_finish(struct mlxsw_sp *mlxsw_sp,
473 			       struct mlxsw_sp1_ptp_unmatched *unmatched)
474 {
475 	if (unmatched->skb && unmatched->timestamp)
476 		mlxsw_sp1_packet_timestamp(mlxsw_sp, unmatched->key,
477 					   unmatched->skb,
478 					   unmatched->timestamp);
479 	else if (unmatched->skb)
480 		mlxsw_sp1_ptp_packet_finish(mlxsw_sp, unmatched->skb,
481 					    unmatched->key.local_port,
482 					    unmatched->key.ingress, NULL);
483 	kfree_rcu(unmatched, rcu);
484 }
485 
486 static void mlxsw_sp1_ptp_unmatched_free_fn(void *ptr, void *arg)
487 {
488 	struct mlxsw_sp1_ptp_unmatched *unmatched = ptr;
489 
490 	/* This is invoked at a point where the ports are gone already. Nothing
491 	 * to do with whatever is left in the HT but to free it.
492 	 */
493 	if (unmatched->skb)
494 		dev_kfree_skb_any(unmatched->skb);
495 	kfree_rcu(unmatched, rcu);
496 }
497 
498 static void mlxsw_sp1_ptp_got_piece(struct mlxsw_sp *mlxsw_sp,
499 				    struct mlxsw_sp1_ptp_key key,
500 				    struct sk_buff *skb, u64 timestamp)
501 {
502 	struct mlxsw_sp1_ptp_unmatched *unmatched;
503 	int length;
504 	int err;
505 
506 	rcu_read_lock();
507 
508 	spin_lock(&mlxsw_sp->ptp_state->unmatched_lock);
509 
510 	unmatched = mlxsw_sp1_ptp_unmatched_lookup(mlxsw_sp, key, &length);
511 	if (skb && unmatched && unmatched->timestamp) {
512 		unmatched->skb = skb;
513 	} else if (timestamp && unmatched && unmatched->skb) {
514 		unmatched->timestamp = timestamp;
515 	} else {
516 		/* Either there is no entry to match, or one that is there is
517 		 * incompatible.
518 		 */
519 		if (length < 100)
520 			err = mlxsw_sp1_ptp_unmatched_save(mlxsw_sp, key,
521 							   skb, timestamp);
522 		else
523 			err = -E2BIG;
524 		if (err && skb)
525 			mlxsw_sp1_ptp_packet_finish(mlxsw_sp, skb,
526 						    key.local_port,
527 						    key.ingress, NULL);
528 		unmatched = NULL;
529 	}
530 
531 	if (unmatched) {
532 		err = mlxsw_sp1_ptp_unmatched_remove(mlxsw_sp, unmatched);
533 		WARN_ON_ONCE(err);
534 	}
535 
536 	spin_unlock(&mlxsw_sp->ptp_state->unmatched_lock);
537 
538 	if (unmatched)
539 		mlxsw_sp1_ptp_unmatched_finish(mlxsw_sp, unmatched);
540 
541 	rcu_read_unlock();
542 }
543 
544 static void mlxsw_sp1_ptp_got_packet(struct mlxsw_sp *mlxsw_sp,
545 				     struct sk_buff *skb, u8 local_port,
546 				     bool ingress)
547 {
548 	struct mlxsw_sp_port *mlxsw_sp_port;
549 	struct mlxsw_sp1_ptp_key key;
550 	u8 types;
551 	int err;
552 
553 	mlxsw_sp_port = mlxsw_sp->ports[local_port];
554 	if (!mlxsw_sp_port)
555 		goto immediate;
556 
557 	types = ingress ? mlxsw_sp_port->ptp.ing_types :
558 			  mlxsw_sp_port->ptp.egr_types;
559 	if (!types)
560 		goto immediate;
561 
562 	memset(&key, 0, sizeof(key));
563 	key.local_port = local_port;
564 	key.ingress = ingress;
565 
566 	err = mlxsw_sp_ptp_parse(skb, &key.domain_number, &key.message_type,
567 				 &key.sequence_id);
568 	if (err)
569 		goto immediate;
570 
571 	/* For packets whose timestamping was not enabled on this port, don't
572 	 * bother trying to match the timestamp.
573 	 */
574 	if (!((1 << key.message_type) & types))
575 		goto immediate;
576 
577 	mlxsw_sp1_ptp_got_piece(mlxsw_sp, key, skb, 0);
578 	return;
579 
580 immediate:
581 	mlxsw_sp1_ptp_packet_finish(mlxsw_sp, skb, local_port, ingress, NULL);
582 }
583 
584 void mlxsw_sp1_ptp_got_timestamp(struct mlxsw_sp *mlxsw_sp, bool ingress,
585 				 u8 local_port, u8 message_type,
586 				 u8 domain_number, u16 sequence_id,
587 				 u64 timestamp)
588 {
589 	struct mlxsw_sp_port *mlxsw_sp_port;
590 	struct mlxsw_sp1_ptp_key key;
591 	u8 types;
592 
593 	mlxsw_sp_port = mlxsw_sp->ports[local_port];
594 	if (!mlxsw_sp_port)
595 		return;
596 
597 	types = ingress ? mlxsw_sp_port->ptp.ing_types :
598 			  mlxsw_sp_port->ptp.egr_types;
599 
600 	/* For message types whose timestamping was not enabled on this port,
601 	 * don't bother with the timestamp.
602 	 */
603 	if (!((1 << message_type) & types))
604 		return;
605 
606 	memset(&key, 0, sizeof(key));
607 	key.local_port = local_port;
608 	key.domain_number = domain_number;
609 	key.message_type = message_type;
610 	key.sequence_id = sequence_id;
611 	key.ingress = ingress;
612 
613 	mlxsw_sp1_ptp_got_piece(mlxsw_sp, key, NULL, timestamp);
614 }
615 
616 void mlxsw_sp1_ptp_receive(struct mlxsw_sp *mlxsw_sp, struct sk_buff *skb,
617 			   u8 local_port)
618 {
619 	skb_reset_mac_header(skb);
620 	mlxsw_sp1_ptp_got_packet(mlxsw_sp, skb, local_port, true);
621 }
622 
623 void mlxsw_sp1_ptp_transmitted(struct mlxsw_sp *mlxsw_sp,
624 			       struct sk_buff *skb, u8 local_port)
625 {
626 	mlxsw_sp1_ptp_got_packet(mlxsw_sp, skb, local_port, false);
627 }
628 
629 static void
630 mlxsw_sp1_ptp_ht_gc_collect(struct mlxsw_sp_ptp_state *ptp_state,
631 			    struct mlxsw_sp1_ptp_unmatched *unmatched)
632 {
633 	struct mlxsw_sp_ptp_port_dir_stats *stats;
634 	struct mlxsw_sp_port *mlxsw_sp_port;
635 	int err;
636 
637 	/* If an unmatched entry has an SKB, it has to be handed over to the
638 	 * networking stack. This is usually done from a trap handler, which is
639 	 * invoked in a softirq context. Here we are going to do it in process
640 	 * context. If that were to be interrupted by a softirq, it could cause
641 	 * a deadlock when an attempt is made to take an already-taken lock
642 	 * somewhere along the sending path. Disable softirqs to prevent this.
643 	 */
644 	local_bh_disable();
645 
646 	spin_lock(&ptp_state->unmatched_lock);
647 	err = rhltable_remove(&ptp_state->unmatched_ht, &unmatched->ht_node,
648 			      mlxsw_sp1_ptp_unmatched_ht_params);
649 	spin_unlock(&ptp_state->unmatched_lock);
650 
651 	if (err)
652 		/* The packet was matched with timestamp during the walk. */
653 		goto out;
654 
655 	mlxsw_sp_port = ptp_state->mlxsw_sp->ports[unmatched->key.local_port];
656 	if (mlxsw_sp_port) {
657 		stats = unmatched->key.ingress ?
658 			&mlxsw_sp_port->ptp.stats.rx_gcd :
659 			&mlxsw_sp_port->ptp.stats.tx_gcd;
660 		if (unmatched->skb)
661 			stats->packets++;
662 		else
663 			stats->timestamps++;
664 	}
665 
666 	/* mlxsw_sp1_ptp_unmatched_finish() invokes netif_receive_skb(). While
667 	 * the comment at that function states that it can only be called in
668 	 * soft IRQ context, this pattern of local_bh_disable() +
669 	 * netif_receive_skb(), in process context, is seen elsewhere in the
670 	 * kernel, notably in pktgen.
671 	 */
672 	mlxsw_sp1_ptp_unmatched_finish(ptp_state->mlxsw_sp, unmatched);
673 
674 out:
675 	local_bh_enable();
676 }
677 
678 static void mlxsw_sp1_ptp_ht_gc(struct work_struct *work)
679 {
680 	struct delayed_work *dwork = to_delayed_work(work);
681 	struct mlxsw_sp1_ptp_unmatched *unmatched;
682 	struct mlxsw_sp_ptp_state *ptp_state;
683 	struct rhashtable_iter iter;
684 	u32 gc_cycle;
685 	void *obj;
686 
687 	ptp_state = container_of(dwork, struct mlxsw_sp_ptp_state, ht_gc_dw);
688 	gc_cycle = ptp_state->gc_cycle++;
689 
690 	rhltable_walk_enter(&ptp_state->unmatched_ht, &iter);
691 	rhashtable_walk_start(&iter);
692 	while ((obj = rhashtable_walk_next(&iter))) {
693 		if (IS_ERR(obj))
694 			continue;
695 
696 		unmatched = obj;
697 		if (unmatched->gc_cycle <= gc_cycle)
698 			mlxsw_sp1_ptp_ht_gc_collect(ptp_state, unmatched);
699 	}
700 	rhashtable_walk_stop(&iter);
701 	rhashtable_walk_exit(&iter);
702 
703 	mlxsw_core_schedule_dw(&ptp_state->ht_gc_dw,
704 			       MLXSW_SP1_PTP_HT_GC_INTERVAL);
705 }
706 
707 static int mlxsw_sp_ptp_mtptpt_set(struct mlxsw_sp *mlxsw_sp,
708 				   enum mlxsw_reg_mtptpt_trap_id trap_id,
709 				   u16 message_type)
710 {
711 	char mtptpt_pl[MLXSW_REG_MTPTPT_LEN];
712 
713 	mlxsw_reg_mtptptp_pack(mtptpt_pl, trap_id, message_type);
714 	return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(mtptpt), mtptpt_pl);
715 }
716 
717 static int mlxsw_sp1_ptp_set_fifo_clr_on_trap(struct mlxsw_sp *mlxsw_sp,
718 					      bool clr)
719 {
720 	char mogcr_pl[MLXSW_REG_MOGCR_LEN] = {0};
721 	int err;
722 
723 	err = mlxsw_reg_query(mlxsw_sp->core, MLXSW_REG(mogcr), mogcr_pl);
724 	if (err)
725 		return err;
726 
727 	mlxsw_reg_mogcr_ptp_iftc_set(mogcr_pl, clr);
728 	mlxsw_reg_mogcr_ptp_eftc_set(mogcr_pl, clr);
729 	return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(mogcr), mogcr_pl);
730 }
731 
732 static int mlxsw_sp1_ptp_mtpppc_set(struct mlxsw_sp *mlxsw_sp,
733 				    u16 ing_types, u16 egr_types)
734 {
735 	char mtpppc_pl[MLXSW_REG_MTPPPC_LEN];
736 
737 	mlxsw_reg_mtpppc_pack(mtpppc_pl, ing_types, egr_types);
738 	return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(mtpppc), mtpppc_pl);
739 }
740 
741 struct mlxsw_sp1_ptp_shaper_params {
742 	u32 ethtool_speed;
743 	enum mlxsw_reg_qpsc_port_speed port_speed;
744 	u8 shaper_time_exp;
745 	u8 shaper_time_mantissa;
746 	u8 shaper_inc;
747 	u8 shaper_bs;
748 	u8 port_to_shaper_credits;
749 	int ing_timestamp_inc;
750 	int egr_timestamp_inc;
751 };
752 
753 static const struct mlxsw_sp1_ptp_shaper_params
754 mlxsw_sp1_ptp_shaper_params[] = {
755 	{
756 		.ethtool_speed		= SPEED_100,
757 		.port_speed		= MLXSW_REG_QPSC_PORT_SPEED_100M,
758 		.shaper_time_exp	= 4,
759 		.shaper_time_mantissa	= 12,
760 		.shaper_inc		= 9,
761 		.shaper_bs		= 1,
762 		.port_to_shaper_credits	= 1,
763 		.ing_timestamp_inc	= -313,
764 		.egr_timestamp_inc	= 313,
765 	},
766 	{
767 		.ethtool_speed		= SPEED_1000,
768 		.port_speed		= MLXSW_REG_QPSC_PORT_SPEED_1G,
769 		.shaper_time_exp	= 0,
770 		.shaper_time_mantissa	= 12,
771 		.shaper_inc		= 6,
772 		.shaper_bs		= 0,
773 		.port_to_shaper_credits	= 1,
774 		.ing_timestamp_inc	= -35,
775 		.egr_timestamp_inc	= 35,
776 	},
777 	{
778 		.ethtool_speed		= SPEED_10000,
779 		.port_speed		= MLXSW_REG_QPSC_PORT_SPEED_10G,
780 		.shaper_time_exp	= 0,
781 		.shaper_time_mantissa	= 2,
782 		.shaper_inc		= 14,
783 		.shaper_bs		= 1,
784 		.port_to_shaper_credits	= 1,
785 		.ing_timestamp_inc	= -11,
786 		.egr_timestamp_inc	= 11,
787 	},
788 	{
789 		.ethtool_speed		= SPEED_25000,
790 		.port_speed		= MLXSW_REG_QPSC_PORT_SPEED_25G,
791 		.shaper_time_exp	= 0,
792 		.shaper_time_mantissa	= 0,
793 		.shaper_inc		= 11,
794 		.shaper_bs		= 1,
795 		.port_to_shaper_credits	= 1,
796 		.ing_timestamp_inc	= -14,
797 		.egr_timestamp_inc	= 14,
798 	},
799 };
800 
801 #define MLXSW_SP1_PTP_SHAPER_PARAMS_LEN ARRAY_SIZE(mlxsw_sp1_ptp_shaper_params)
802 
803 static int mlxsw_sp1_ptp_shaper_params_set(struct mlxsw_sp *mlxsw_sp)
804 {
805 	const struct mlxsw_sp1_ptp_shaper_params *params;
806 	char qpsc_pl[MLXSW_REG_QPSC_LEN];
807 	int i, err;
808 
809 	for (i = 0; i < MLXSW_SP1_PTP_SHAPER_PARAMS_LEN; i++) {
810 		params = &mlxsw_sp1_ptp_shaper_params[i];
811 		mlxsw_reg_qpsc_pack(qpsc_pl, params->port_speed,
812 				    params->shaper_time_exp,
813 				    params->shaper_time_mantissa,
814 				    params->shaper_inc, params->shaper_bs,
815 				    params->port_to_shaper_credits,
816 				    params->ing_timestamp_inc,
817 				    params->egr_timestamp_inc);
818 		err = mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(qpsc), qpsc_pl);
819 		if (err)
820 			return err;
821 	}
822 
823 	return 0;
824 }
825 
826 struct mlxsw_sp_ptp_state *mlxsw_sp1_ptp_init(struct mlxsw_sp *mlxsw_sp)
827 {
828 	struct mlxsw_sp_ptp_state *ptp_state;
829 	u16 message_type;
830 	int err;
831 
832 	err = mlxsw_sp1_ptp_shaper_params_set(mlxsw_sp);
833 	if (err)
834 		return ERR_PTR(err);
835 
836 	ptp_state = kzalloc(sizeof(*ptp_state), GFP_KERNEL);
837 	if (!ptp_state)
838 		return ERR_PTR(-ENOMEM);
839 	ptp_state->mlxsw_sp = mlxsw_sp;
840 
841 	spin_lock_init(&ptp_state->unmatched_lock);
842 
843 	err = rhltable_init(&ptp_state->unmatched_ht,
844 			    &mlxsw_sp1_ptp_unmatched_ht_params);
845 	if (err)
846 		goto err_hashtable_init;
847 
848 	/* Delive these message types as PTP0. */
849 	message_type = BIT(MLXSW_SP_PTP_MESSAGE_TYPE_SYNC) |
850 		       BIT(MLXSW_SP_PTP_MESSAGE_TYPE_DELAY_REQ) |
851 		       BIT(MLXSW_SP_PTP_MESSAGE_TYPE_PDELAY_REQ) |
852 		       BIT(MLXSW_SP_PTP_MESSAGE_TYPE_PDELAY_RESP);
853 	err = mlxsw_sp_ptp_mtptpt_set(mlxsw_sp, MLXSW_REG_MTPTPT_TRAP_ID_PTP0,
854 				      message_type);
855 	if (err)
856 		goto err_mtptpt_set;
857 
858 	/* Everything else is PTP1. */
859 	message_type = ~message_type;
860 	err = mlxsw_sp_ptp_mtptpt_set(mlxsw_sp, MLXSW_REG_MTPTPT_TRAP_ID_PTP1,
861 				      message_type);
862 	if (err)
863 		goto err_mtptpt1_set;
864 
865 	err = mlxsw_sp1_ptp_set_fifo_clr_on_trap(mlxsw_sp, true);
866 	if (err)
867 		goto err_fifo_clr;
868 
869 	INIT_DELAYED_WORK(&ptp_state->ht_gc_dw, mlxsw_sp1_ptp_ht_gc);
870 	mlxsw_core_schedule_dw(&ptp_state->ht_gc_dw,
871 			       MLXSW_SP1_PTP_HT_GC_INTERVAL);
872 	return ptp_state;
873 
874 err_fifo_clr:
875 	mlxsw_sp_ptp_mtptpt_set(mlxsw_sp, MLXSW_REG_MTPTPT_TRAP_ID_PTP1, 0);
876 err_mtptpt1_set:
877 	mlxsw_sp_ptp_mtptpt_set(mlxsw_sp, MLXSW_REG_MTPTPT_TRAP_ID_PTP0, 0);
878 err_mtptpt_set:
879 	rhltable_destroy(&ptp_state->unmatched_ht);
880 err_hashtable_init:
881 	kfree(ptp_state);
882 	return ERR_PTR(err);
883 }
884 
885 void mlxsw_sp1_ptp_fini(struct mlxsw_sp_ptp_state *ptp_state)
886 {
887 	struct mlxsw_sp *mlxsw_sp = ptp_state->mlxsw_sp;
888 
889 	cancel_delayed_work_sync(&ptp_state->ht_gc_dw);
890 	mlxsw_sp1_ptp_mtpppc_set(mlxsw_sp, 0, 0);
891 	mlxsw_sp1_ptp_set_fifo_clr_on_trap(mlxsw_sp, false);
892 	mlxsw_sp_ptp_mtptpt_set(mlxsw_sp, MLXSW_REG_MTPTPT_TRAP_ID_PTP1, 0);
893 	mlxsw_sp_ptp_mtptpt_set(mlxsw_sp, MLXSW_REG_MTPTPT_TRAP_ID_PTP0, 0);
894 	rhltable_free_and_destroy(&ptp_state->unmatched_ht,
895 				  &mlxsw_sp1_ptp_unmatched_free_fn, NULL);
896 	kfree(ptp_state);
897 }
898 
899 int mlxsw_sp1_ptp_hwtstamp_get(struct mlxsw_sp_port *mlxsw_sp_port,
900 			       struct hwtstamp_config *config)
901 {
902 	*config = mlxsw_sp_port->ptp.hwtstamp_config;
903 	return 0;
904 }
905 
906 static int mlxsw_sp_ptp_get_message_types(const struct hwtstamp_config *config,
907 					  u16 *p_ing_types, u16 *p_egr_types,
908 					  enum hwtstamp_rx_filters *p_rx_filter)
909 {
910 	enum hwtstamp_rx_filters rx_filter = config->rx_filter;
911 	enum hwtstamp_tx_types tx_type = config->tx_type;
912 	u16 ing_types = 0x00;
913 	u16 egr_types = 0x00;
914 
915 	switch (tx_type) {
916 	case HWTSTAMP_TX_OFF:
917 		egr_types = 0x00;
918 		break;
919 	case HWTSTAMP_TX_ON:
920 		egr_types = 0xff;
921 		break;
922 	case HWTSTAMP_TX_ONESTEP_SYNC:
923 	case HWTSTAMP_TX_ONESTEP_P2P:
924 		return -ERANGE;
925 	}
926 
927 	switch (rx_filter) {
928 	case HWTSTAMP_FILTER_NONE:
929 		ing_types = 0x00;
930 		break;
931 	case HWTSTAMP_FILTER_PTP_V1_L4_SYNC:
932 	case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
933 	case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
934 	case HWTSTAMP_FILTER_PTP_V2_SYNC:
935 		ing_types = 0x01;
936 		break;
937 	case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ:
938 	case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
939 	case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
940 	case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
941 		ing_types = 0x02;
942 		break;
943 	case HWTSTAMP_FILTER_PTP_V1_L4_EVENT:
944 	case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
945 	case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
946 	case HWTSTAMP_FILTER_PTP_V2_EVENT:
947 		ing_types = 0x0f;
948 		break;
949 	case HWTSTAMP_FILTER_ALL:
950 		ing_types = 0xff;
951 		break;
952 	case HWTSTAMP_FILTER_SOME:
953 	case HWTSTAMP_FILTER_NTP_ALL:
954 		return -ERANGE;
955 	}
956 
957 	*p_ing_types = ing_types;
958 	*p_egr_types = egr_types;
959 	*p_rx_filter = rx_filter;
960 	return 0;
961 }
962 
963 static int mlxsw_sp1_ptp_mtpppc_update(struct mlxsw_sp_port *mlxsw_sp_port,
964 				       u16 ing_types, u16 egr_types)
965 {
966 	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
967 	struct mlxsw_sp_port *tmp;
968 	u16 orig_ing_types = 0;
969 	u16 orig_egr_types = 0;
970 	int err;
971 	int i;
972 
973 	/* MTPPPC configures timestamping globally, not per port. Find the
974 	 * configuration that contains all configured timestamping requests.
975 	 */
976 	for (i = 1; i < mlxsw_core_max_ports(mlxsw_sp->core); i++) {
977 		tmp = mlxsw_sp->ports[i];
978 		if (tmp) {
979 			orig_ing_types |= tmp->ptp.ing_types;
980 			orig_egr_types |= tmp->ptp.egr_types;
981 		}
982 		if (tmp && tmp != mlxsw_sp_port) {
983 			ing_types |= tmp->ptp.ing_types;
984 			egr_types |= tmp->ptp.egr_types;
985 		}
986 	}
987 
988 	if ((ing_types || egr_types) && !(orig_ing_types || orig_egr_types)) {
989 		err = mlxsw_sp_nve_inc_parsing_depth_get(mlxsw_sp);
990 		if (err) {
991 			netdev_err(mlxsw_sp_port->dev, "Failed to increase parsing depth");
992 			return err;
993 		}
994 	}
995 	if (!(ing_types || egr_types) && (orig_ing_types || orig_egr_types))
996 		mlxsw_sp_nve_inc_parsing_depth_put(mlxsw_sp);
997 
998 	return mlxsw_sp1_ptp_mtpppc_set(mlxsw_sp_port->mlxsw_sp,
999 				       ing_types, egr_types);
1000 }
1001 
1002 static bool mlxsw_sp1_ptp_hwtstamp_enabled(struct mlxsw_sp_port *mlxsw_sp_port)
1003 {
1004 	return mlxsw_sp_port->ptp.ing_types || mlxsw_sp_port->ptp.egr_types;
1005 }
1006 
1007 static int
1008 mlxsw_sp1_ptp_port_shaper_set(struct mlxsw_sp_port *mlxsw_sp_port, bool enable)
1009 {
1010 	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
1011 	char qeec_pl[MLXSW_REG_QEEC_LEN];
1012 
1013 	mlxsw_reg_qeec_ptps_pack(qeec_pl, mlxsw_sp_port->local_port, enable);
1014 	return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(qeec), qeec_pl);
1015 }
1016 
1017 static int mlxsw_sp1_ptp_port_shaper_check(struct mlxsw_sp_port *mlxsw_sp_port)
1018 {
1019 	bool ptps = false;
1020 	int err, i;
1021 	u32 speed;
1022 
1023 	if (!mlxsw_sp1_ptp_hwtstamp_enabled(mlxsw_sp_port))
1024 		return mlxsw_sp1_ptp_port_shaper_set(mlxsw_sp_port, false);
1025 
1026 	err = mlxsw_sp_port_speed_get(mlxsw_sp_port, &speed);
1027 	if (err)
1028 		return err;
1029 
1030 	for (i = 0; i < MLXSW_SP1_PTP_SHAPER_PARAMS_LEN; i++) {
1031 		if (mlxsw_sp1_ptp_shaper_params[i].ethtool_speed == speed) {
1032 			ptps = true;
1033 			break;
1034 		}
1035 	}
1036 
1037 	return mlxsw_sp1_ptp_port_shaper_set(mlxsw_sp_port, ptps);
1038 }
1039 
1040 void mlxsw_sp1_ptp_shaper_work(struct work_struct *work)
1041 {
1042 	struct delayed_work *dwork = to_delayed_work(work);
1043 	struct mlxsw_sp_port *mlxsw_sp_port;
1044 	int err;
1045 
1046 	mlxsw_sp_port = container_of(dwork, struct mlxsw_sp_port,
1047 				     ptp.shaper_dw);
1048 
1049 	if (!mlxsw_sp1_ptp_hwtstamp_enabled(mlxsw_sp_port))
1050 		return;
1051 
1052 	err = mlxsw_sp1_ptp_port_shaper_check(mlxsw_sp_port);
1053 	if (err)
1054 		netdev_err(mlxsw_sp_port->dev, "Failed to set up PTP shaper\n");
1055 }
1056 
1057 int mlxsw_sp1_ptp_hwtstamp_set(struct mlxsw_sp_port *mlxsw_sp_port,
1058 			       struct hwtstamp_config *config)
1059 {
1060 	enum hwtstamp_rx_filters rx_filter;
1061 	u16 ing_types;
1062 	u16 egr_types;
1063 	int err;
1064 
1065 	err = mlxsw_sp_ptp_get_message_types(config, &ing_types, &egr_types,
1066 					     &rx_filter);
1067 	if (err)
1068 		return err;
1069 
1070 	err = mlxsw_sp1_ptp_mtpppc_update(mlxsw_sp_port, ing_types, egr_types);
1071 	if (err)
1072 		return err;
1073 
1074 	mlxsw_sp_port->ptp.hwtstamp_config = *config;
1075 	mlxsw_sp_port->ptp.ing_types = ing_types;
1076 	mlxsw_sp_port->ptp.egr_types = egr_types;
1077 
1078 	err = mlxsw_sp1_ptp_port_shaper_check(mlxsw_sp_port);
1079 	if (err)
1080 		return err;
1081 
1082 	/* Notify the ioctl caller what we are actually timestamping. */
1083 	config->rx_filter = rx_filter;
1084 
1085 	return 0;
1086 }
1087 
1088 int mlxsw_sp1_ptp_get_ts_info(struct mlxsw_sp *mlxsw_sp,
1089 			      struct ethtool_ts_info *info)
1090 {
1091 	info->phc_index = ptp_clock_index(mlxsw_sp->clock->ptp);
1092 
1093 	info->so_timestamping = SOF_TIMESTAMPING_TX_HARDWARE |
1094 				SOF_TIMESTAMPING_RX_HARDWARE |
1095 				SOF_TIMESTAMPING_RAW_HARDWARE;
1096 
1097 	info->tx_types = BIT(HWTSTAMP_TX_OFF) |
1098 			 BIT(HWTSTAMP_TX_ON);
1099 
1100 	info->rx_filters = BIT(HWTSTAMP_FILTER_NONE) |
1101 			   BIT(HWTSTAMP_FILTER_ALL);
1102 
1103 	return 0;
1104 }
1105 
1106 struct mlxsw_sp_ptp_port_stat {
1107 	char str[ETH_GSTRING_LEN];
1108 	ptrdiff_t offset;
1109 };
1110 
1111 #define MLXSW_SP_PTP_PORT_STAT(NAME, FIELD)				\
1112 	{								\
1113 		.str = NAME,						\
1114 		.offset = offsetof(struct mlxsw_sp_ptp_port_stats,	\
1115 				    FIELD),				\
1116 	}
1117 
1118 static const struct mlxsw_sp_ptp_port_stat mlxsw_sp_ptp_port_stats[] = {
1119 	MLXSW_SP_PTP_PORT_STAT("ptp_rx_gcd_packets",    rx_gcd.packets),
1120 	MLXSW_SP_PTP_PORT_STAT("ptp_rx_gcd_timestamps", rx_gcd.timestamps),
1121 	MLXSW_SP_PTP_PORT_STAT("ptp_tx_gcd_packets",    tx_gcd.packets),
1122 	MLXSW_SP_PTP_PORT_STAT("ptp_tx_gcd_timestamps", tx_gcd.timestamps),
1123 };
1124 
1125 #undef MLXSW_SP_PTP_PORT_STAT
1126 
1127 #define MLXSW_SP_PTP_PORT_STATS_LEN \
1128 	ARRAY_SIZE(mlxsw_sp_ptp_port_stats)
1129 
1130 int mlxsw_sp1_get_stats_count(void)
1131 {
1132 	return MLXSW_SP_PTP_PORT_STATS_LEN;
1133 }
1134 
1135 void mlxsw_sp1_get_stats_strings(u8 **p)
1136 {
1137 	int i;
1138 
1139 	for (i = 0; i < MLXSW_SP_PTP_PORT_STATS_LEN; i++) {
1140 		memcpy(*p, mlxsw_sp_ptp_port_stats[i].str,
1141 		       ETH_GSTRING_LEN);
1142 		*p += ETH_GSTRING_LEN;
1143 	}
1144 }
1145 
1146 void mlxsw_sp1_get_stats(struct mlxsw_sp_port *mlxsw_sp_port,
1147 			 u64 *data, int data_index)
1148 {
1149 	void *stats = &mlxsw_sp_port->ptp.stats;
1150 	ptrdiff_t offset;
1151 	int i;
1152 
1153 	data += data_index;
1154 	for (i = 0; i < MLXSW_SP_PTP_PORT_STATS_LEN; i++) {
1155 		offset = mlxsw_sp_ptp_port_stats[i].offset;
1156 		*data++ = *(u64 *)(stats + offset);
1157 	}
1158 }
1159