1 // SPDX-License-Identifier: BSD-3-Clause OR GPL-2.0
2 /* Copyright (c) 2019 Mellanox Technologies. All rights reserved */
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>
16 #include "spectrum_ptp.h"
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
23 #define MLXSW_SP1_PTP_HT_GC_INTERVAL 500 /* ms */
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.
28 #define MLXSW_SP1_PTP_HT_GC_TIMEOUT 1000 /* ms */
30 struct mlxsw_sp_ptp_state {
31 struct mlxsw_sp *mlxsw_sp;
32 struct rhashtable unmatched_ht;
33 spinlock_t unmatched_lock; /* protects the HT */
34 struct delayed_work ht_gc_dw;
38 struct mlxsw_sp1_ptp_key {
46 struct mlxsw_sp1_ptp_unmatched {
47 struct mlxsw_sp1_ptp_key key;
48 struct rhash_head ht_node;
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),
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;
67 struct ptp_clock *ptp;
68 struct ptp_clock_info ptp_info;
69 unsigned long overflow_period;
70 struct delayed_work overflow_work;
73 static u64 __mlxsw_sp1_ptp_read_frc(struct mlxsw_sp_ptp_clock *clock,
74 struct ptp_system_timestamp *sts)
76 struct mlxsw_core *mlxsw_core = clock->core;
77 u32 frc_h1, frc_h2, frc_l;
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);
85 if (frc_h1 != frc_h2) {
87 ptp_read_system_prets(sts);
88 frc_l = mlxsw_core_read_frc_l(mlxsw_core);
89 ptp_read_system_postts(sts);
92 return (u64) frc_l | (u64) frc_h2 << 32;
95 static u64 mlxsw_sp1_ptp_read_frc(const struct cyclecounter *cc)
97 struct mlxsw_sp_ptp_clock *clock =
98 container_of(cc, struct mlxsw_sp_ptp_clock, cycles);
100 return __mlxsw_sp1_ptp_read_frc(clock, NULL) & cc->mask;
104 mlxsw_sp1_ptp_phc_adjfreq(struct mlxsw_sp_ptp_clock *clock, int freq_adj)
106 struct mlxsw_core *mlxsw_core = clock->core;
107 char mtutc_pl[MLXSW_REG_MTUTC_LEN];
109 mlxsw_reg_mtutc_pack(mtutc_pl, MLXSW_REG_MTUTC_OPERATION_ADJUST_FREQ,
111 return mlxsw_reg_write(mlxsw_core, MLXSW_REG(mtutc), mtutc_pl);
114 static u64 mlxsw_sp1_ptp_ns2cycles(const struct timecounter *tc, u64 nsec)
116 u64 cycles = (u64) nsec;
118 cycles <<= tc->cc->shift;
119 cycles = div_u64(cycles, tc->cc->mult);
125 mlxsw_sp1_ptp_phc_settime(struct mlxsw_sp_ptp_clock *clock, u64 nsec)
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];
133 next_sec = div_u64(nsec, NSEC_PER_SEC) + 1;
134 next_sec_in_nsec = next_sec * NSEC_PER_SEC;
136 spin_lock_bh(&clock->lock);
137 cycles = mlxsw_sp1_ptp_ns2cycles(&clock->tc, next_sec_in_nsec);
138 spin_unlock_bh(&clock->lock);
140 mlxsw_reg_mtpps_vpin_pack(mtpps_pl, cycles);
141 err = mlxsw_reg_write(mlxsw_core, MLXSW_REG(mtpps), mtpps_pl);
145 mlxsw_reg_mtutc_pack(mtutc_pl,
146 MLXSW_REG_MTUTC_OPERATION_SET_TIME_AT_NEXT_SEC,
148 return mlxsw_reg_write(mlxsw_core, MLXSW_REG(mtutc), mtutc_pl);
151 static int mlxsw_sp1_ptp_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)
153 struct mlxsw_sp_ptp_clock *clock =
154 container_of(ptp, struct mlxsw_sp_ptp_clock, ptp_info);
160 ppb = scaled_ppm_to_ppb(scaled_ppm);
167 adj = clock->nominal_c_mult;
169 diff = div_u64(adj, NSEC_PER_SEC);
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);
177 return mlxsw_sp1_ptp_phc_adjfreq(clock, neg_adj ? -ppb : ppb);
180 static int mlxsw_sp1_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
182 struct mlxsw_sp_ptp_clock *clock =
183 container_of(ptp, struct mlxsw_sp_ptp_clock, ptp_info);
186 spin_lock_bh(&clock->lock);
187 timecounter_adjtime(&clock->tc, delta);
188 nsec = timecounter_read(&clock->tc);
189 spin_unlock_bh(&clock->lock);
191 return mlxsw_sp1_ptp_phc_settime(clock, nsec);
194 static int mlxsw_sp1_ptp_gettimex(struct ptp_clock_info *ptp,
195 struct timespec64 *ts,
196 struct ptp_system_timestamp *sts)
198 struct mlxsw_sp_ptp_clock *clock =
199 container_of(ptp, struct mlxsw_sp_ptp_clock, ptp_info);
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);
207 *ts = ns_to_timespec64(nsec);
212 static int mlxsw_sp1_ptp_settime(struct ptp_clock_info *ptp,
213 const struct timespec64 *ts)
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);
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);
224 return mlxsw_sp1_ptp_phc_settime(clock, nsec);
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,
237 static void mlxsw_sp1_ptp_clock_overflow(struct work_struct *work)
239 struct delayed_work *dwork = to_delayed_work(work);
240 struct mlxsw_sp_ptp_clock *clock;
242 clock = container_of(dwork, struct mlxsw_sp_ptp_clock, overflow_work);
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);
250 struct mlxsw_sp_ptp_clock *
251 mlxsw_sp1_ptp_clock_init(struct mlxsw_sp *mlxsw_sp, struct device *dev)
253 u64 overflow_cycles, nsec, frac = 0;
254 struct mlxsw_sp_ptp_clock *clock;
257 clock = kzalloc(sizeof(*clock), GFP_KERNEL);
259 return ERR_PTR(-ENOMEM);
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;
270 timecounter_init(&clock->tc, &clock->cycles,
271 ktime_to_ns(ktime_get_real()));
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
280 overflow_cycles = div64_u64(~0ULL >> 1, clock->cycles.mult);
281 overflow_cycles = min(overflow_cycles, div_u64(clock->cycles.mask, 3));
283 nsec = cyclecounter_cyc2ns(&clock->cycles, overflow_cycles, 0, &frac);
284 clock->overflow_period = nsecs_to_jiffies(nsec);
286 INIT_DELAYED_WORK(&clock->overflow_work, mlxsw_sp1_ptp_clock_overflow);
287 mlxsw_core_schedule_dw(&clock->overflow_work, 0);
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;
299 err_ptp_clock_register:
300 cancel_delayed_work_sync(&clock->overflow_work);
305 void mlxsw_sp1_ptp_clock_fini(struct mlxsw_sp_ptp_clock *clock)
307 ptp_clock_unregister(clock->ptp);
308 cancel_delayed_work_sync(&clock->overflow_work);
312 static int mlxsw_sp_ptp_parse(struct sk_buff *skb,
317 unsigned int offset = 0;
318 unsigned int ptp_class;
321 data = skb_mac_header(skb);
322 ptp_class = ptp_classify_raw(skb);
324 switch (ptp_class & PTP_CLASS_VMASK) {
332 if (ptp_class & PTP_CLASS_VLAN)
335 switch (ptp_class & PTP_CLASS_PMASK) {
337 offset += ETH_HLEN + IPV4_HLEN(data + offset) + UDP_HLEN;
340 offset += ETH_HLEN + IP6_HLEN + UDP_HLEN;
349 /* PTP header is 34 bytes. */
350 if (skb->len < offset + 34)
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];
359 /* Returns NULL on successful insertion, a pointer on conflict, or an ERR_PTR on
362 static struct mlxsw_sp1_ptp_unmatched *
363 mlxsw_sp1_ptp_unmatched_save(struct mlxsw_sp *mlxsw_sp,
364 struct mlxsw_sp1_ptp_key key,
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 struct mlxsw_sp1_ptp_unmatched *conflict;
373 unmatched = kzalloc(sizeof(*unmatched), GFP_ATOMIC);
375 return ERR_PTR(-ENOMEM);
377 unmatched->key = key;
378 unmatched->skb = skb;
379 unmatched->timestamp = timestamp;
380 unmatched->gc_cycle = mlxsw_sp->ptp_state->gc_cycle + cycles;
382 conflict = rhashtable_lookup_get_insert_fast(&ptp_state->unmatched_ht,
384 mlxsw_sp1_ptp_unmatched_ht_params);
391 static struct mlxsw_sp1_ptp_unmatched *
392 mlxsw_sp1_ptp_unmatched_lookup(struct mlxsw_sp *mlxsw_sp,
393 struct mlxsw_sp1_ptp_key key)
395 return rhashtable_lookup(&mlxsw_sp->ptp_state->unmatched_ht, &key,
396 mlxsw_sp1_ptp_unmatched_ht_params);
400 mlxsw_sp1_ptp_unmatched_remove(struct mlxsw_sp *mlxsw_sp,
401 struct mlxsw_sp1_ptp_unmatched *unmatched)
403 return rhashtable_remove_fast(&mlxsw_sp->ptp_state->unmatched_ht,
405 mlxsw_sp1_ptp_unmatched_ht_params);
408 /* This function is called in the following scenarios:
410 * 1) When a packet is matched with its timestamp.
411 * 2) In several situation when it is necessary to immediately pass on
412 * an SKB without a timestamp.
413 * 3) From GC indirectly through mlxsw_sp1_ptp_unmatched_finish().
414 * This case is similar to 2) above.
416 static void mlxsw_sp1_ptp_packet_finish(struct mlxsw_sp *mlxsw_sp,
417 struct sk_buff *skb, u8 local_port,
419 struct skb_shared_hwtstamps *hwtstamps)
421 struct mlxsw_sp_port *mlxsw_sp_port;
423 /* Between capturing the packet and finishing it, there is a window of
424 * opportunity for the originating port to go away (e.g. due to a
425 * split). Also make sure the SKB device reference is still valid.
427 mlxsw_sp_port = mlxsw_sp->ports[local_port];
428 if (!(mlxsw_sp_port && (!skb->dev || skb->dev == mlxsw_sp_port->dev))) {
429 dev_kfree_skb_any(skb);
435 *skb_hwtstamps(skb) = *hwtstamps;
436 mlxsw_sp_rx_listener_no_mark_func(skb, local_port, mlxsw_sp);
438 /* skb_tstamp_tx() allows hwtstamps to be NULL. */
439 skb_tstamp_tx(skb, hwtstamps);
440 dev_kfree_skb_any(skb);
444 static void mlxsw_sp1_packet_timestamp(struct mlxsw_sp *mlxsw_sp,
445 struct mlxsw_sp1_ptp_key key,
449 struct skb_shared_hwtstamps hwtstamps;
452 spin_lock_bh(&mlxsw_sp->clock->lock);
453 nsec = timecounter_cyc2time(&mlxsw_sp->clock->tc, timestamp);
454 spin_unlock_bh(&mlxsw_sp->clock->lock);
456 hwtstamps.hwtstamp = ns_to_ktime(nsec);
457 mlxsw_sp1_ptp_packet_finish(mlxsw_sp, skb,
458 key.local_port, key.ingress, &hwtstamps);
462 mlxsw_sp1_ptp_unmatched_finish(struct mlxsw_sp *mlxsw_sp,
463 struct mlxsw_sp1_ptp_unmatched *unmatched)
465 if (unmatched->skb && unmatched->timestamp)
466 mlxsw_sp1_packet_timestamp(mlxsw_sp, unmatched->key,
468 unmatched->timestamp);
469 else if (unmatched->skb)
470 mlxsw_sp1_ptp_packet_finish(mlxsw_sp, unmatched->skb,
471 unmatched->key.local_port,
472 unmatched->key.ingress, NULL);
473 kfree_rcu(unmatched, rcu);
476 static void mlxsw_sp1_ptp_unmatched_free_fn(void *ptr, void *arg)
478 struct mlxsw_sp1_ptp_unmatched *unmatched = ptr;
480 /* This is invoked at a point where the ports are gone already. Nothing
481 * to do with whatever is left in the HT but to free it.
484 dev_kfree_skb_any(unmatched->skb);
485 kfree_rcu(unmatched, rcu);
488 static void mlxsw_sp1_ptp_got_piece(struct mlxsw_sp *mlxsw_sp,
489 struct mlxsw_sp1_ptp_key key,
490 struct sk_buff *skb, u64 timestamp)
492 struct mlxsw_sp1_ptp_unmatched *unmatched, *conflict;
497 unmatched = mlxsw_sp1_ptp_unmatched_lookup(mlxsw_sp, key);
499 spin_lock(&mlxsw_sp->ptp_state->unmatched_lock);
502 /* There was an unmatched entry when we looked, but it may have
503 * been removed before we took the lock.
505 err = mlxsw_sp1_ptp_unmatched_remove(mlxsw_sp, unmatched);
511 /* We have no unmatched entry, but one may have been added after
512 * we looked, but before we took the lock.
514 unmatched = mlxsw_sp1_ptp_unmatched_save(mlxsw_sp, key,
516 if (IS_ERR(unmatched)) {
518 mlxsw_sp1_ptp_packet_finish(mlxsw_sp, skb,
522 } else if (unmatched) {
523 /* Save just told us, under lock, that the entry is
524 * there, so this has to work.
526 err = mlxsw_sp1_ptp_unmatched_remove(mlxsw_sp,
532 /* If unmatched is non-NULL here, it comes either from the lookup, or
533 * from the save attempt above. In either case the entry was removed
534 * from the hash table. If unmatched is NULL, a new unmatched entry was
535 * added to the hash table, and there was no conflict.
538 if (skb && unmatched && unmatched->timestamp) {
539 unmatched->skb = skb;
540 } else if (timestamp && unmatched && unmatched->skb) {
541 unmatched->timestamp = timestamp;
542 } else if (unmatched) {
543 /* unmatched holds an older entry of the same type: either an
544 * skb if we are handling skb, or a timestamp if we are handling
545 * timestamp. We can't match that up, so save what we have.
547 conflict = mlxsw_sp1_ptp_unmatched_save(mlxsw_sp, key,
549 if (IS_ERR(conflict)) {
551 mlxsw_sp1_ptp_packet_finish(mlxsw_sp, skb,
555 /* Above, we removed an object with this key from the
556 * hash table, under lock, so conflict can not be a
559 WARN_ON_ONCE(conflict);
563 spin_unlock(&mlxsw_sp->ptp_state->unmatched_lock);
566 mlxsw_sp1_ptp_unmatched_finish(mlxsw_sp, unmatched);
571 static void mlxsw_sp1_ptp_got_packet(struct mlxsw_sp *mlxsw_sp,
572 struct sk_buff *skb, u8 local_port,
575 struct mlxsw_sp_port *mlxsw_sp_port;
576 struct mlxsw_sp1_ptp_key key;
580 mlxsw_sp_port = mlxsw_sp->ports[local_port];
584 types = ingress ? mlxsw_sp_port->ptp.ing_types :
585 mlxsw_sp_port->ptp.egr_types;
589 memset(&key, 0, sizeof(key));
590 key.local_port = local_port;
591 key.ingress = ingress;
593 err = mlxsw_sp_ptp_parse(skb, &key.domain_number, &key.message_type,
598 /* For packets whose timestamping was not enabled on this port, don't
599 * bother trying to match the timestamp.
601 if (!((1 << key.message_type) & types))
604 mlxsw_sp1_ptp_got_piece(mlxsw_sp, key, skb, 0);
608 mlxsw_sp1_ptp_packet_finish(mlxsw_sp, skb, local_port, ingress, NULL);
611 void mlxsw_sp1_ptp_got_timestamp(struct mlxsw_sp *mlxsw_sp, bool ingress,
612 u8 local_port, u8 message_type,
613 u8 domain_number, u16 sequence_id,
616 struct mlxsw_sp_port *mlxsw_sp_port;
617 struct mlxsw_sp1_ptp_key key;
620 mlxsw_sp_port = mlxsw_sp->ports[local_port];
624 types = ingress ? mlxsw_sp_port->ptp.ing_types :
625 mlxsw_sp_port->ptp.egr_types;
627 /* For message types whose timestamping was not enabled on this port,
628 * don't bother with the timestamp.
630 if (!((1 << message_type) & types))
633 memset(&key, 0, sizeof(key));
634 key.local_port = local_port;
635 key.domain_number = domain_number;
636 key.message_type = message_type;
637 key.sequence_id = sequence_id;
638 key.ingress = ingress;
640 mlxsw_sp1_ptp_got_piece(mlxsw_sp, key, NULL, timestamp);
643 void mlxsw_sp1_ptp_receive(struct mlxsw_sp *mlxsw_sp, struct sk_buff *skb,
646 skb_reset_mac_header(skb);
647 mlxsw_sp1_ptp_got_packet(mlxsw_sp, skb, local_port, true);
650 void mlxsw_sp1_ptp_transmitted(struct mlxsw_sp *mlxsw_sp,
651 struct sk_buff *skb, u8 local_port)
653 mlxsw_sp1_ptp_got_packet(mlxsw_sp, skb, local_port, false);
657 mlxsw_sp1_ptp_ht_gc_collect(struct mlxsw_sp_ptp_state *ptp_state,
658 struct mlxsw_sp1_ptp_unmatched *unmatched)
662 /* If an unmatched entry has an SKB, it has to be handed over to the
663 * networking stack. This is usually done from a trap handler, which is
664 * invoked in a softirq context. Here we are going to do it in process
665 * context. If that were to be interrupted by a softirq, it could cause
666 * a deadlock when an attempt is made to take an already-taken lock
667 * somewhere along the sending path. Disable softirqs to prevent this.
671 spin_lock(&ptp_state->unmatched_lock);
672 err = rhashtable_remove_fast(&ptp_state->unmatched_ht,
674 mlxsw_sp1_ptp_unmatched_ht_params);
675 spin_unlock(&ptp_state->unmatched_lock);
678 /* The packet was matched with timestamp during the walk. */
681 /* mlxsw_sp1_ptp_unmatched_finish() invokes netif_receive_skb(). While
682 * the comment at that function states that it can only be called in
683 * soft IRQ context, this pattern of local_bh_disable() +
684 * netif_receive_skb(), in process context, is seen elsewhere in the
685 * kernel, notably in pktgen.
687 mlxsw_sp1_ptp_unmatched_finish(ptp_state->mlxsw_sp, unmatched);
693 static void mlxsw_sp1_ptp_ht_gc(struct work_struct *work)
695 struct delayed_work *dwork = to_delayed_work(work);
696 struct mlxsw_sp1_ptp_unmatched *unmatched;
697 struct mlxsw_sp_ptp_state *ptp_state;
698 struct rhashtable_iter iter;
702 ptp_state = container_of(dwork, struct mlxsw_sp_ptp_state, ht_gc_dw);
703 gc_cycle = ptp_state->gc_cycle++;
705 rhashtable_walk_enter(&ptp_state->unmatched_ht, &iter);
706 rhashtable_walk_start(&iter);
707 while ((obj = rhashtable_walk_next(&iter))) {
712 if (unmatched->gc_cycle <= gc_cycle)
713 mlxsw_sp1_ptp_ht_gc_collect(ptp_state, unmatched);
715 rhashtable_walk_stop(&iter);
716 rhashtable_walk_exit(&iter);
718 mlxsw_core_schedule_dw(&ptp_state->ht_gc_dw,
719 MLXSW_SP1_PTP_HT_GC_INTERVAL);
722 static int mlxsw_sp_ptp_mtptpt_set(struct mlxsw_sp *mlxsw_sp,
723 enum mlxsw_reg_mtptpt_trap_id trap_id,
726 char mtptpt_pl[MLXSW_REG_MTPTPT_LEN];
728 mlxsw_reg_mtptptp_pack(mtptpt_pl, trap_id, message_type);
729 return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(mtptpt), mtptpt_pl);
732 static int mlxsw_sp1_ptp_set_fifo_clr_on_trap(struct mlxsw_sp *mlxsw_sp,
735 char mogcr_pl[MLXSW_REG_MOGCR_LEN] = {0};
738 err = mlxsw_reg_query(mlxsw_sp->core, MLXSW_REG(mogcr), mogcr_pl);
742 mlxsw_reg_mogcr_ptp_iftc_set(mogcr_pl, clr);
743 mlxsw_reg_mogcr_ptp_eftc_set(mogcr_pl, clr);
744 return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(mogcr), mogcr_pl);
747 static int mlxsw_sp1_ptp_mtpppc_set(struct mlxsw_sp *mlxsw_sp,
748 u16 ing_types, u16 egr_types)
750 char mtpppc_pl[MLXSW_REG_MTPPPC_LEN];
752 mlxsw_reg_mtpppc_pack(mtpppc_pl, ing_types, egr_types);
753 return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(mtpppc), mtpppc_pl);
756 struct mlxsw_sp1_ptp_shaper_params {
758 enum mlxsw_reg_qpsc_port_speed port_speed;
760 u8 shaper_time_mantissa;
763 u8 port_to_shaper_credits;
764 int ing_timestamp_inc;
765 int egr_timestamp_inc;
768 static const struct mlxsw_sp1_ptp_shaper_params
769 mlxsw_sp1_ptp_shaper_params[] = {
771 .ethtool_speed = SPEED_100,
772 .port_speed = MLXSW_REG_QPSC_PORT_SPEED_100M,
773 .shaper_time_exp = 4,
774 .shaper_time_mantissa = 12,
777 .port_to_shaper_credits = 1,
778 .ing_timestamp_inc = -313,
779 .egr_timestamp_inc = 313,
782 .ethtool_speed = SPEED_1000,
783 .port_speed = MLXSW_REG_QPSC_PORT_SPEED_1G,
784 .shaper_time_exp = 0,
785 .shaper_time_mantissa = 12,
788 .port_to_shaper_credits = 1,
789 .ing_timestamp_inc = -35,
790 .egr_timestamp_inc = 35,
793 .ethtool_speed = SPEED_10000,
794 .port_speed = MLXSW_REG_QPSC_PORT_SPEED_10G,
795 .shaper_time_exp = 0,
796 .shaper_time_mantissa = 2,
799 .port_to_shaper_credits = 1,
800 .ing_timestamp_inc = -11,
801 .egr_timestamp_inc = 11,
804 .ethtool_speed = SPEED_25000,
805 .port_speed = MLXSW_REG_QPSC_PORT_SPEED_25G,
806 .shaper_time_exp = 0,
807 .shaper_time_mantissa = 0,
810 .port_to_shaper_credits = 1,
811 .ing_timestamp_inc = -14,
812 .egr_timestamp_inc = 14,
816 #define MLXSW_SP1_PTP_SHAPER_PARAMS_LEN ARRAY_SIZE(mlxsw_sp1_ptp_shaper_params)
818 static int mlxsw_sp1_ptp_shaper_params_set(struct mlxsw_sp *mlxsw_sp)
820 const struct mlxsw_sp1_ptp_shaper_params *params;
821 char qpsc_pl[MLXSW_REG_QPSC_LEN];
824 for (i = 0; i < MLXSW_SP1_PTP_SHAPER_PARAMS_LEN; i++) {
825 params = &mlxsw_sp1_ptp_shaper_params[i];
826 mlxsw_reg_qpsc_pack(qpsc_pl, params->port_speed,
827 params->shaper_time_exp,
828 params->shaper_time_mantissa,
829 params->shaper_inc, params->shaper_bs,
830 params->port_to_shaper_credits,
831 params->ing_timestamp_inc,
832 params->egr_timestamp_inc);
833 err = mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(qpsc), qpsc_pl);
841 struct mlxsw_sp_ptp_state *mlxsw_sp1_ptp_init(struct mlxsw_sp *mlxsw_sp)
843 struct mlxsw_sp_ptp_state *ptp_state;
847 err = mlxsw_sp1_ptp_shaper_params_set(mlxsw_sp);
851 ptp_state = kzalloc(sizeof(*ptp_state), GFP_KERNEL);
853 return ERR_PTR(-ENOMEM);
854 ptp_state->mlxsw_sp = mlxsw_sp;
856 spin_lock_init(&ptp_state->unmatched_lock);
858 err = rhashtable_init(&ptp_state->unmatched_ht,
859 &mlxsw_sp1_ptp_unmatched_ht_params);
861 goto err_hashtable_init;
863 /* Delive these message types as PTP0. */
864 message_type = BIT(MLXSW_SP_PTP_MESSAGE_TYPE_SYNC) |
865 BIT(MLXSW_SP_PTP_MESSAGE_TYPE_DELAY_REQ) |
866 BIT(MLXSW_SP_PTP_MESSAGE_TYPE_PDELAY_REQ) |
867 BIT(MLXSW_SP_PTP_MESSAGE_TYPE_PDELAY_RESP);
868 err = mlxsw_sp_ptp_mtptpt_set(mlxsw_sp, MLXSW_REG_MTPTPT_TRAP_ID_PTP0,
873 /* Everything else is PTP1. */
874 message_type = ~message_type;
875 err = mlxsw_sp_ptp_mtptpt_set(mlxsw_sp, MLXSW_REG_MTPTPT_TRAP_ID_PTP1,
878 goto err_mtptpt1_set;
880 err = mlxsw_sp1_ptp_set_fifo_clr_on_trap(mlxsw_sp, true);
884 INIT_DELAYED_WORK(&ptp_state->ht_gc_dw, mlxsw_sp1_ptp_ht_gc);
885 mlxsw_core_schedule_dw(&ptp_state->ht_gc_dw,
886 MLXSW_SP1_PTP_HT_GC_INTERVAL);
890 mlxsw_sp_ptp_mtptpt_set(mlxsw_sp, MLXSW_REG_MTPTPT_TRAP_ID_PTP1, 0);
892 mlxsw_sp_ptp_mtptpt_set(mlxsw_sp, MLXSW_REG_MTPTPT_TRAP_ID_PTP0, 0);
894 rhashtable_destroy(&ptp_state->unmatched_ht);
900 void mlxsw_sp1_ptp_fini(struct mlxsw_sp_ptp_state *ptp_state)
902 struct mlxsw_sp *mlxsw_sp = ptp_state->mlxsw_sp;
904 cancel_delayed_work_sync(&ptp_state->ht_gc_dw);
905 mlxsw_sp1_ptp_mtpppc_set(mlxsw_sp, 0, 0);
906 mlxsw_sp1_ptp_set_fifo_clr_on_trap(mlxsw_sp, false);
907 mlxsw_sp_ptp_mtptpt_set(mlxsw_sp, MLXSW_REG_MTPTPT_TRAP_ID_PTP1, 0);
908 mlxsw_sp_ptp_mtptpt_set(mlxsw_sp, MLXSW_REG_MTPTPT_TRAP_ID_PTP0, 0);
909 rhashtable_free_and_destroy(&ptp_state->unmatched_ht,
910 &mlxsw_sp1_ptp_unmatched_free_fn, NULL);
914 int mlxsw_sp1_ptp_hwtstamp_get(struct mlxsw_sp_port *mlxsw_sp_port,
915 struct hwtstamp_config *config)
917 *config = mlxsw_sp_port->ptp.hwtstamp_config;
921 static int mlxsw_sp_ptp_get_message_types(const struct hwtstamp_config *config,
922 u16 *p_ing_types, u16 *p_egr_types,
923 enum hwtstamp_rx_filters *p_rx_filter)
925 enum hwtstamp_rx_filters rx_filter = config->rx_filter;
926 enum hwtstamp_tx_types tx_type = config->tx_type;
927 u16 ing_types = 0x00;
928 u16 egr_types = 0x00;
931 case HWTSTAMP_TX_OFF:
937 case HWTSTAMP_TX_ONESTEP_SYNC:
942 case HWTSTAMP_FILTER_NONE:
945 case HWTSTAMP_FILTER_PTP_V1_L4_SYNC:
946 case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
947 case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
948 case HWTSTAMP_FILTER_PTP_V2_SYNC:
951 case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ:
952 case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
953 case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
954 case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
957 case HWTSTAMP_FILTER_PTP_V1_L4_EVENT:
958 case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
959 case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
960 case HWTSTAMP_FILTER_PTP_V2_EVENT:
963 case HWTSTAMP_FILTER_ALL:
966 case HWTSTAMP_FILTER_SOME:
967 case HWTSTAMP_FILTER_NTP_ALL:
971 *p_ing_types = ing_types;
972 *p_egr_types = egr_types;
973 *p_rx_filter = rx_filter;
977 static int mlxsw_sp1_ptp_mtpppc_update(struct mlxsw_sp_port *mlxsw_sp_port,
978 u16 ing_types, u16 egr_types)
980 struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
981 struct mlxsw_sp_port *tmp;
982 u16 orig_ing_types = 0;
983 u16 orig_egr_types = 0;
987 /* MTPPPC configures timestamping globally, not per port. Find the
988 * configuration that contains all configured timestamping requests.
990 for (i = 1; i < mlxsw_core_max_ports(mlxsw_sp->core); i++) {
991 tmp = mlxsw_sp->ports[i];
993 orig_ing_types |= tmp->ptp.ing_types;
994 orig_egr_types |= tmp->ptp.egr_types;
996 if (tmp && tmp != mlxsw_sp_port) {
997 ing_types |= tmp->ptp.ing_types;
998 egr_types |= tmp->ptp.egr_types;
1002 if ((ing_types || egr_types) && !(orig_ing_types || orig_egr_types)) {
1003 err = mlxsw_sp_nve_inc_parsing_depth_get(mlxsw_sp);
1005 netdev_err(mlxsw_sp_port->dev, "Failed to increase parsing depth");
1009 if (!(ing_types || egr_types) && (orig_ing_types || orig_egr_types))
1010 mlxsw_sp_nve_inc_parsing_depth_put(mlxsw_sp);
1012 return mlxsw_sp1_ptp_mtpppc_set(mlxsw_sp_port->mlxsw_sp,
1013 ing_types, egr_types);
1016 static bool mlxsw_sp1_ptp_hwtstamp_enabled(struct mlxsw_sp_port *mlxsw_sp_port)
1018 return mlxsw_sp_port->ptp.ing_types || mlxsw_sp_port->ptp.egr_types;
1022 mlxsw_sp1_ptp_port_shaper_set(struct mlxsw_sp_port *mlxsw_sp_port, bool enable)
1024 struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
1025 char qeec_pl[MLXSW_REG_QEEC_LEN];
1027 mlxsw_reg_qeec_ptps_pack(qeec_pl, mlxsw_sp_port->local_port, enable);
1028 return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(qeec), qeec_pl);
1031 static int mlxsw_sp1_ptp_port_shaper_check(struct mlxsw_sp_port *mlxsw_sp_port)
1033 const struct mlxsw_sp_port_type_speed_ops *port_type_speed_ops;
1034 struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
1035 char ptys_pl[MLXSW_REG_PTYS_LEN];
1036 u32 eth_proto_oper, speed;
1040 if (!mlxsw_sp1_ptp_hwtstamp_enabled(mlxsw_sp_port))
1041 return mlxsw_sp1_ptp_port_shaper_set(mlxsw_sp_port, false);
1043 port_type_speed_ops = mlxsw_sp->port_type_speed_ops;
1044 port_type_speed_ops->reg_ptys_eth_pack(mlxsw_sp, ptys_pl,
1045 mlxsw_sp_port->local_port, 0,
1047 err = mlxsw_reg_query(mlxsw_sp->core, MLXSW_REG(ptys), ptys_pl);
1050 port_type_speed_ops->reg_ptys_eth_unpack(mlxsw_sp, ptys_pl, NULL, NULL,
1053 speed = port_type_speed_ops->from_ptys_speed(mlxsw_sp, eth_proto_oper);
1054 for (i = 0; i < MLXSW_SP1_PTP_SHAPER_PARAMS_LEN; i++) {
1055 if (mlxsw_sp1_ptp_shaper_params[i].ethtool_speed == speed) {
1061 return mlxsw_sp1_ptp_port_shaper_set(mlxsw_sp_port, ptps);
1064 void mlxsw_sp1_ptp_shaper_work(struct work_struct *work)
1066 struct delayed_work *dwork = to_delayed_work(work);
1067 struct mlxsw_sp_port *mlxsw_sp_port;
1070 mlxsw_sp_port = container_of(dwork, struct mlxsw_sp_port,
1073 if (!mlxsw_sp1_ptp_hwtstamp_enabled(mlxsw_sp_port))
1076 err = mlxsw_sp1_ptp_port_shaper_check(mlxsw_sp_port);
1078 netdev_err(mlxsw_sp_port->dev, "Failed to set up PTP shaper\n");
1081 int mlxsw_sp1_ptp_hwtstamp_set(struct mlxsw_sp_port *mlxsw_sp_port,
1082 struct hwtstamp_config *config)
1084 enum hwtstamp_rx_filters rx_filter;
1089 err = mlxsw_sp_ptp_get_message_types(config, &ing_types, &egr_types,
1094 err = mlxsw_sp1_ptp_mtpppc_update(mlxsw_sp_port, ing_types, egr_types);
1098 mlxsw_sp_port->ptp.hwtstamp_config = *config;
1099 mlxsw_sp_port->ptp.ing_types = ing_types;
1100 mlxsw_sp_port->ptp.egr_types = egr_types;
1102 err = mlxsw_sp1_ptp_port_shaper_check(mlxsw_sp_port);
1106 /* Notify the ioctl caller what we are actually timestamping. */
1107 config->rx_filter = rx_filter;
1112 int mlxsw_sp1_ptp_get_ts_info(struct mlxsw_sp *mlxsw_sp,
1113 struct ethtool_ts_info *info)
1115 info->phc_index = ptp_clock_index(mlxsw_sp->clock->ptp);
1117 info->so_timestamping = SOF_TIMESTAMPING_TX_HARDWARE |
1118 SOF_TIMESTAMPING_RX_HARDWARE |
1119 SOF_TIMESTAMPING_RAW_HARDWARE;
1121 info->tx_types = BIT(HWTSTAMP_TX_OFF) |
1122 BIT(HWTSTAMP_TX_ON);
1124 info->rx_filters = BIT(HWTSTAMP_FILTER_NONE) |
1125 BIT(HWTSTAMP_FILTER_ALL);