This source file includes following definitions.
- __mlxsw_sp1_ptp_read_frc
- mlxsw_sp1_ptp_read_frc
- mlxsw_sp1_ptp_phc_adjfreq
- mlxsw_sp1_ptp_ns2cycles
- mlxsw_sp1_ptp_phc_settime
- mlxsw_sp1_ptp_adjfine
- mlxsw_sp1_ptp_adjtime
- mlxsw_sp1_ptp_gettimex
- mlxsw_sp1_ptp_settime
- mlxsw_sp1_ptp_clock_overflow
- mlxsw_sp1_ptp_clock_init
- mlxsw_sp1_ptp_clock_fini
- mlxsw_sp_ptp_parse
- mlxsw_sp1_ptp_unmatched_save
- mlxsw_sp1_ptp_unmatched_lookup
- mlxsw_sp1_ptp_unmatched_remove
- mlxsw_sp1_ptp_packet_finish
- mlxsw_sp1_packet_timestamp
- mlxsw_sp1_ptp_unmatched_finish
- mlxsw_sp1_ptp_unmatched_free_fn
- mlxsw_sp1_ptp_got_piece
- mlxsw_sp1_ptp_got_packet
- mlxsw_sp1_ptp_got_timestamp
- mlxsw_sp1_ptp_receive
- mlxsw_sp1_ptp_transmitted
- mlxsw_sp1_ptp_ht_gc_collect
- mlxsw_sp1_ptp_ht_gc
- mlxsw_sp_ptp_mtptpt_set
- mlxsw_sp1_ptp_set_fifo_clr_on_trap
- mlxsw_sp1_ptp_mtpppc_set
- mlxsw_sp1_ptp_shaper_params_set
- mlxsw_sp1_ptp_init
- mlxsw_sp1_ptp_fini
- mlxsw_sp1_ptp_hwtstamp_get
- mlxsw_sp_ptp_get_message_types
- mlxsw_sp1_ptp_mtpppc_update
- mlxsw_sp1_ptp_hwtstamp_enabled
- mlxsw_sp1_ptp_port_shaper_set
- mlxsw_sp1_ptp_port_shaper_check
- mlxsw_sp1_ptp_shaper_work
- mlxsw_sp1_ptp_hwtstamp_set
- mlxsw_sp1_ptp_get_ts_info
- mlxsw_sp1_get_stats_count
- mlxsw_sp1_get_stats_strings
- mlxsw_sp1_get_stats
1
2
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
21 #define MLXSW_SP1_PTP_CLOCK_MASK 64
22
23 #define MLXSW_SP1_PTP_HT_GC_INTERVAL 500
24
25
26
27
28 #define MLXSW_SP1_PTP_HT_GC_TIMEOUT 1000
29
30 struct mlxsw_sp_ptp_state {
31 struct mlxsw_sp *mlxsw_sp;
32 struct rhltable unmatched_ht;
33 spinlock_t unmatched_lock;
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;
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
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
274
275
276
277
278
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
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
360
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
419
420
421
422
423
424
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
434
435
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
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
491
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
517
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
572
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
601
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
638
639
640
641
642
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
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
667
668
669
670
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
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
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 return -ERANGE;
924 }
925
926 switch (rx_filter) {
927 case HWTSTAMP_FILTER_NONE:
928 ing_types = 0x00;
929 break;
930 case HWTSTAMP_FILTER_PTP_V1_L4_SYNC:
931 case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
932 case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
933 case HWTSTAMP_FILTER_PTP_V2_SYNC:
934 ing_types = 0x01;
935 break;
936 case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ:
937 case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
938 case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
939 case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
940 ing_types = 0x02;
941 break;
942 case HWTSTAMP_FILTER_PTP_V1_L4_EVENT:
943 case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
944 case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
945 case HWTSTAMP_FILTER_PTP_V2_EVENT:
946 ing_types = 0x0f;
947 break;
948 case HWTSTAMP_FILTER_ALL:
949 ing_types = 0xff;
950 break;
951 case HWTSTAMP_FILTER_SOME:
952 case HWTSTAMP_FILTER_NTP_ALL:
953 return -ERANGE;
954 }
955
956 *p_ing_types = ing_types;
957 *p_egr_types = egr_types;
958 *p_rx_filter = rx_filter;
959 return 0;
960 }
961
962 static int mlxsw_sp1_ptp_mtpppc_update(struct mlxsw_sp_port *mlxsw_sp_port,
963 u16 ing_types, u16 egr_types)
964 {
965 struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
966 struct mlxsw_sp_port *tmp;
967 u16 orig_ing_types = 0;
968 u16 orig_egr_types = 0;
969 int err;
970 int i;
971
972
973
974
975 for (i = 1; i < mlxsw_core_max_ports(mlxsw_sp->core); i++) {
976 tmp = mlxsw_sp->ports[i];
977 if (tmp) {
978 orig_ing_types |= tmp->ptp.ing_types;
979 orig_egr_types |= tmp->ptp.egr_types;
980 }
981 if (tmp && tmp != mlxsw_sp_port) {
982 ing_types |= tmp->ptp.ing_types;
983 egr_types |= tmp->ptp.egr_types;
984 }
985 }
986
987 if ((ing_types || egr_types) && !(orig_ing_types || orig_egr_types)) {
988 err = mlxsw_sp_nve_inc_parsing_depth_get(mlxsw_sp);
989 if (err) {
990 netdev_err(mlxsw_sp_port->dev, "Failed to increase parsing depth");
991 return err;
992 }
993 }
994 if (!(ing_types || egr_types) && (orig_ing_types || orig_egr_types))
995 mlxsw_sp_nve_inc_parsing_depth_put(mlxsw_sp);
996
997 return mlxsw_sp1_ptp_mtpppc_set(mlxsw_sp_port->mlxsw_sp,
998 ing_types, egr_types);
999 }
1000
1001 static bool mlxsw_sp1_ptp_hwtstamp_enabled(struct mlxsw_sp_port *mlxsw_sp_port)
1002 {
1003 return mlxsw_sp_port->ptp.ing_types || mlxsw_sp_port->ptp.egr_types;
1004 }
1005
1006 static int
1007 mlxsw_sp1_ptp_port_shaper_set(struct mlxsw_sp_port *mlxsw_sp_port, bool enable)
1008 {
1009 struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
1010 char qeec_pl[MLXSW_REG_QEEC_LEN];
1011
1012 mlxsw_reg_qeec_ptps_pack(qeec_pl, mlxsw_sp_port->local_port, enable);
1013 return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(qeec), qeec_pl);
1014 }
1015
1016 static int mlxsw_sp1_ptp_port_shaper_check(struct mlxsw_sp_port *mlxsw_sp_port)
1017 {
1018 const struct mlxsw_sp_port_type_speed_ops *port_type_speed_ops;
1019 struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
1020 char ptys_pl[MLXSW_REG_PTYS_LEN];
1021 u32 eth_proto_oper, speed;
1022 bool ptps = false;
1023 int err, i;
1024
1025 if (!mlxsw_sp1_ptp_hwtstamp_enabled(mlxsw_sp_port))
1026 return mlxsw_sp1_ptp_port_shaper_set(mlxsw_sp_port, false);
1027
1028 port_type_speed_ops = mlxsw_sp->port_type_speed_ops;
1029 port_type_speed_ops->reg_ptys_eth_pack(mlxsw_sp, ptys_pl,
1030 mlxsw_sp_port->local_port, 0,
1031 false);
1032 err = mlxsw_reg_query(mlxsw_sp->core, MLXSW_REG(ptys), ptys_pl);
1033 if (err)
1034 return err;
1035 port_type_speed_ops->reg_ptys_eth_unpack(mlxsw_sp, ptys_pl, NULL, NULL,
1036 ð_proto_oper);
1037
1038 speed = port_type_speed_ops->from_ptys_speed(mlxsw_sp, eth_proto_oper);
1039 for (i = 0; i < MLXSW_SP1_PTP_SHAPER_PARAMS_LEN; i++) {
1040 if (mlxsw_sp1_ptp_shaper_params[i].ethtool_speed == speed) {
1041 ptps = true;
1042 break;
1043 }
1044 }
1045
1046 return mlxsw_sp1_ptp_port_shaper_set(mlxsw_sp_port, ptps);
1047 }
1048
1049 void mlxsw_sp1_ptp_shaper_work(struct work_struct *work)
1050 {
1051 struct delayed_work *dwork = to_delayed_work(work);
1052 struct mlxsw_sp_port *mlxsw_sp_port;
1053 int err;
1054
1055 mlxsw_sp_port = container_of(dwork, struct mlxsw_sp_port,
1056 ptp.shaper_dw);
1057
1058 if (!mlxsw_sp1_ptp_hwtstamp_enabled(mlxsw_sp_port))
1059 return;
1060
1061 err = mlxsw_sp1_ptp_port_shaper_check(mlxsw_sp_port);
1062 if (err)
1063 netdev_err(mlxsw_sp_port->dev, "Failed to set up PTP shaper\n");
1064 }
1065
1066 int mlxsw_sp1_ptp_hwtstamp_set(struct mlxsw_sp_port *mlxsw_sp_port,
1067 struct hwtstamp_config *config)
1068 {
1069 enum hwtstamp_rx_filters rx_filter;
1070 u16 ing_types;
1071 u16 egr_types;
1072 int err;
1073
1074 err = mlxsw_sp_ptp_get_message_types(config, &ing_types, &egr_types,
1075 &rx_filter);
1076 if (err)
1077 return err;
1078
1079 err = mlxsw_sp1_ptp_mtpppc_update(mlxsw_sp_port, ing_types, egr_types);
1080 if (err)
1081 return err;
1082
1083 mlxsw_sp_port->ptp.hwtstamp_config = *config;
1084 mlxsw_sp_port->ptp.ing_types = ing_types;
1085 mlxsw_sp_port->ptp.egr_types = egr_types;
1086
1087 err = mlxsw_sp1_ptp_port_shaper_check(mlxsw_sp_port);
1088 if (err)
1089 return err;
1090
1091
1092 config->rx_filter = rx_filter;
1093
1094 return 0;
1095 }
1096
1097 int mlxsw_sp1_ptp_get_ts_info(struct mlxsw_sp *mlxsw_sp,
1098 struct ethtool_ts_info *info)
1099 {
1100 info->phc_index = ptp_clock_index(mlxsw_sp->clock->ptp);
1101
1102 info->so_timestamping = SOF_TIMESTAMPING_TX_HARDWARE |
1103 SOF_TIMESTAMPING_RX_HARDWARE |
1104 SOF_TIMESTAMPING_RAW_HARDWARE;
1105
1106 info->tx_types = BIT(HWTSTAMP_TX_OFF) |
1107 BIT(HWTSTAMP_TX_ON);
1108
1109 info->rx_filters = BIT(HWTSTAMP_FILTER_NONE) |
1110 BIT(HWTSTAMP_FILTER_ALL);
1111
1112 return 0;
1113 }
1114
1115 struct mlxsw_sp_ptp_port_stat {
1116 char str[ETH_GSTRING_LEN];
1117 ptrdiff_t offset;
1118 };
1119
1120 #define MLXSW_SP_PTP_PORT_STAT(NAME, FIELD) \
1121 { \
1122 .str = NAME, \
1123 .offset = offsetof(struct mlxsw_sp_ptp_port_stats, \
1124 FIELD), \
1125 }
1126
1127 static const struct mlxsw_sp_ptp_port_stat mlxsw_sp_ptp_port_stats[] = {
1128 MLXSW_SP_PTP_PORT_STAT("ptp_rx_gcd_packets", rx_gcd.packets),
1129 MLXSW_SP_PTP_PORT_STAT("ptp_rx_gcd_timestamps", rx_gcd.timestamps),
1130 MLXSW_SP_PTP_PORT_STAT("ptp_tx_gcd_packets", tx_gcd.packets),
1131 MLXSW_SP_PTP_PORT_STAT("ptp_tx_gcd_timestamps", tx_gcd.timestamps),
1132 };
1133
1134 #undef MLXSW_SP_PTP_PORT_STAT
1135
1136 #define MLXSW_SP_PTP_PORT_STATS_LEN \
1137 ARRAY_SIZE(mlxsw_sp_ptp_port_stats)
1138
1139 int mlxsw_sp1_get_stats_count(void)
1140 {
1141 return MLXSW_SP_PTP_PORT_STATS_LEN;
1142 }
1143
1144 void mlxsw_sp1_get_stats_strings(u8 **p)
1145 {
1146 int i;
1147
1148 for (i = 0; i < MLXSW_SP_PTP_PORT_STATS_LEN; i++) {
1149 memcpy(*p, mlxsw_sp_ptp_port_stats[i].str,
1150 ETH_GSTRING_LEN);
1151 *p += ETH_GSTRING_LEN;
1152 }
1153 }
1154
1155 void mlxsw_sp1_get_stats(struct mlxsw_sp_port *mlxsw_sp_port,
1156 u64 *data, int data_index)
1157 {
1158 void *stats = &mlxsw_sp_port->ptp.stats;
1159 ptrdiff_t offset;
1160 int i;
1161
1162 data += data_index;
1163 for (i = 0; i < MLXSW_SP_PTP_PORT_STATS_LEN; i++) {
1164 offset = mlxsw_sp_ptp_port_stats[i].offset;
1165 *data++ = *(u64 *)(stats + offset);
1166 }
1167 }