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 rhltable 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 rhlist_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
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
;
373 unmatched
= kzalloc(sizeof(*unmatched
), GFP_ATOMIC
);
377 unmatched
->key
= key
;
378 unmatched
->skb
= skb
;
379 unmatched
->timestamp
= timestamp
;
380 unmatched
->gc_cycle
= mlxsw_sp
->ptp_state
->gc_cycle
+ cycles
;
382 err
= rhltable_insert(&ptp_state
->unmatched_ht
, &unmatched
->ht_node
,
383 mlxsw_sp1_ptp_unmatched_ht_params
);
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
)
394 struct mlxsw_sp1_ptp_unmatched
*unmatched
, *last
= NULL
;
395 struct rhlist_head
*tmp
, *list
;
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
) {
410 mlxsw_sp1_ptp_unmatched_remove(struct mlxsw_sp
*mlxsw_sp
,
411 struct mlxsw_sp1_ptp_unmatched
*unmatched
)
413 return rhltable_remove(&mlxsw_sp
->ptp_state
->unmatched_ht
,
415 mlxsw_sp1_ptp_unmatched_ht_params
);
418 /* This function is called in the following scenarios:
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.
426 static void mlxsw_sp1_ptp_packet_finish(struct mlxsw_sp
*mlxsw_sp
,
427 struct sk_buff
*skb
, u8 local_port
,
429 struct skb_shared_hwtstamps
*hwtstamps
)
431 struct mlxsw_sp_port
*mlxsw_sp_port
;
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.
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
);
445 *skb_hwtstamps(skb
) = *hwtstamps
;
446 mlxsw_sp_rx_listener_no_mark_func(skb
, local_port
, mlxsw_sp
);
448 /* skb_tstamp_tx() allows hwtstamps to be NULL. */
449 skb_tstamp_tx(skb
, hwtstamps
);
450 dev_kfree_skb_any(skb
);
454 static void mlxsw_sp1_packet_timestamp(struct mlxsw_sp
*mlxsw_sp
,
455 struct mlxsw_sp1_ptp_key key
,
459 struct skb_shared_hwtstamps hwtstamps
;
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
);
466 hwtstamps
.hwtstamp
= ns_to_ktime(nsec
);
467 mlxsw_sp1_ptp_packet_finish(mlxsw_sp
, skb
,
468 key
.local_port
, key
.ingress
, &hwtstamps
);
472 mlxsw_sp1_ptp_unmatched_finish(struct mlxsw_sp
*mlxsw_sp
,
473 struct mlxsw_sp1_ptp_unmatched
*unmatched
)
475 if (unmatched
->skb
&& unmatched
->timestamp
)
476 mlxsw_sp1_packet_timestamp(mlxsw_sp
, unmatched
->key
,
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
);
486 static void mlxsw_sp1_ptp_unmatched_free_fn(void *ptr
, void *arg
)
488 struct mlxsw_sp1_ptp_unmatched
*unmatched
= ptr
;
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.
494 dev_kfree_skb_any(unmatched
->skb
);
495 kfree_rcu(unmatched
, rcu
);
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
)
502 struct mlxsw_sp1_ptp_unmatched
*unmatched
;
508 spin_lock(&mlxsw_sp
->ptp_state
->unmatched_lock
);
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
;
516 /* Either there is no entry to match, or one that is there is
520 err
= mlxsw_sp1_ptp_unmatched_save(mlxsw_sp
, key
,
525 mlxsw_sp1_ptp_packet_finish(mlxsw_sp
, skb
,
532 err
= mlxsw_sp1_ptp_unmatched_remove(mlxsw_sp
, unmatched
);
536 spin_unlock(&mlxsw_sp
->ptp_state
->unmatched_lock
);
539 mlxsw_sp1_ptp_unmatched_finish(mlxsw_sp
, unmatched
);
544 static void mlxsw_sp1_ptp_got_packet(struct mlxsw_sp
*mlxsw_sp
,
545 struct sk_buff
*skb
, u8 local_port
,
548 struct mlxsw_sp_port
*mlxsw_sp_port
;
549 struct mlxsw_sp1_ptp_key key
;
553 mlxsw_sp_port
= mlxsw_sp
->ports
[local_port
];
557 types
= ingress
? mlxsw_sp_port
->ptp
.ing_types
:
558 mlxsw_sp_port
->ptp
.egr_types
;
562 memset(&key
, 0, sizeof(key
));
563 key
.local_port
= local_port
;
564 key
.ingress
= ingress
;
566 err
= mlxsw_sp_ptp_parse(skb
, &key
.domain_number
, &key
.message_type
,
571 /* For packets whose timestamping was not enabled on this port, don't
572 * bother trying to match the timestamp.
574 if (!((1 << key
.message_type
) & types
))
577 mlxsw_sp1_ptp_got_piece(mlxsw_sp
, key
, skb
, 0);
581 mlxsw_sp1_ptp_packet_finish(mlxsw_sp
, skb
, local_port
, ingress
, NULL
);
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
,
589 struct mlxsw_sp_port
*mlxsw_sp_port
;
590 struct mlxsw_sp1_ptp_key key
;
593 mlxsw_sp_port
= mlxsw_sp
->ports
[local_port
];
597 types
= ingress
? mlxsw_sp_port
->ptp
.ing_types
:
598 mlxsw_sp_port
->ptp
.egr_types
;
600 /* For message types whose timestamping was not enabled on this port,
601 * don't bother with the timestamp.
603 if (!((1 << message_type
) & types
))
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
;
613 mlxsw_sp1_ptp_got_piece(mlxsw_sp
, key
, NULL
, timestamp
);
616 void mlxsw_sp1_ptp_receive(struct mlxsw_sp
*mlxsw_sp
, struct sk_buff
*skb
,
619 skb_reset_mac_header(skb
);
620 mlxsw_sp1_ptp_got_packet(mlxsw_sp
, skb
, local_port
, true);
623 void mlxsw_sp1_ptp_transmitted(struct mlxsw_sp
*mlxsw_sp
,
624 struct sk_buff
*skb
, u8 local_port
)
626 mlxsw_sp1_ptp_got_packet(mlxsw_sp
, skb
, local_port
, false);
630 mlxsw_sp1_ptp_ht_gc_collect(struct mlxsw_sp_ptp_state
*ptp_state
,
631 struct mlxsw_sp1_ptp_unmatched
*unmatched
)
635 /* If an unmatched entry has an SKB, it has to be handed over to the
636 * networking stack. This is usually done from a trap handler, which is
637 * invoked in a softirq context. Here we are going to do it in process
638 * context. If that were to be interrupted by a softirq, it could cause
639 * a deadlock when an attempt is made to take an already-taken lock
640 * somewhere along the sending path. Disable softirqs to prevent this.
644 spin_lock(&ptp_state
->unmatched_lock
);
645 err
= rhltable_remove(&ptp_state
->unmatched_ht
, &unmatched
->ht_node
,
646 mlxsw_sp1_ptp_unmatched_ht_params
);
647 spin_unlock(&ptp_state
->unmatched_lock
);
650 /* The packet was matched with timestamp during the walk. */
653 /* mlxsw_sp1_ptp_unmatched_finish() invokes netif_receive_skb(). While
654 * the comment at that function states that it can only be called in
655 * soft IRQ context, this pattern of local_bh_disable() +
656 * netif_receive_skb(), in process context, is seen elsewhere in the
657 * kernel, notably in pktgen.
659 mlxsw_sp1_ptp_unmatched_finish(ptp_state
->mlxsw_sp
, unmatched
);
665 static void mlxsw_sp1_ptp_ht_gc(struct work_struct
*work
)
667 struct delayed_work
*dwork
= to_delayed_work(work
);
668 struct mlxsw_sp1_ptp_unmatched
*unmatched
;
669 struct mlxsw_sp_ptp_state
*ptp_state
;
670 struct rhashtable_iter iter
;
674 ptp_state
= container_of(dwork
, struct mlxsw_sp_ptp_state
, ht_gc_dw
);
675 gc_cycle
= ptp_state
->gc_cycle
++;
677 rhltable_walk_enter(&ptp_state
->unmatched_ht
, &iter
);
678 rhashtable_walk_start(&iter
);
679 while ((obj
= rhashtable_walk_next(&iter
))) {
684 if (unmatched
->gc_cycle
<= gc_cycle
)
685 mlxsw_sp1_ptp_ht_gc_collect(ptp_state
, unmatched
);
687 rhashtable_walk_stop(&iter
);
688 rhashtable_walk_exit(&iter
);
690 mlxsw_core_schedule_dw(&ptp_state
->ht_gc_dw
,
691 MLXSW_SP1_PTP_HT_GC_INTERVAL
);
694 static int mlxsw_sp_ptp_mtptpt_set(struct mlxsw_sp
*mlxsw_sp
,
695 enum mlxsw_reg_mtptpt_trap_id trap_id
,
698 char mtptpt_pl
[MLXSW_REG_MTPTPT_LEN
];
700 mlxsw_reg_mtptptp_pack(mtptpt_pl
, trap_id
, message_type
);
701 return mlxsw_reg_write(mlxsw_sp
->core
, MLXSW_REG(mtptpt
), mtptpt_pl
);
704 static int mlxsw_sp1_ptp_set_fifo_clr_on_trap(struct mlxsw_sp
*mlxsw_sp
,
707 char mogcr_pl
[MLXSW_REG_MOGCR_LEN
] = {0};
710 err
= mlxsw_reg_query(mlxsw_sp
->core
, MLXSW_REG(mogcr
), mogcr_pl
);
714 mlxsw_reg_mogcr_ptp_iftc_set(mogcr_pl
, clr
);
715 mlxsw_reg_mogcr_ptp_eftc_set(mogcr_pl
, clr
);
716 return mlxsw_reg_write(mlxsw_sp
->core
, MLXSW_REG(mogcr
), mogcr_pl
);
719 static int mlxsw_sp1_ptp_mtpppc_set(struct mlxsw_sp
*mlxsw_sp
,
720 u16 ing_types
, u16 egr_types
)
722 char mtpppc_pl
[MLXSW_REG_MTPPPC_LEN
];
724 mlxsw_reg_mtpppc_pack(mtpppc_pl
, ing_types
, egr_types
);
725 return mlxsw_reg_write(mlxsw_sp
->core
, MLXSW_REG(mtpppc
), mtpppc_pl
);
728 struct mlxsw_sp1_ptp_shaper_params
{
730 enum mlxsw_reg_qpsc_port_speed port_speed
;
732 u8 shaper_time_mantissa
;
735 u8 port_to_shaper_credits
;
736 int ing_timestamp_inc
;
737 int egr_timestamp_inc
;
740 static const struct mlxsw_sp1_ptp_shaper_params
741 mlxsw_sp1_ptp_shaper_params
[] = {
743 .ethtool_speed
= SPEED_100
,
744 .port_speed
= MLXSW_REG_QPSC_PORT_SPEED_100M
,
745 .shaper_time_exp
= 4,
746 .shaper_time_mantissa
= 12,
749 .port_to_shaper_credits
= 1,
750 .ing_timestamp_inc
= -313,
751 .egr_timestamp_inc
= 313,
754 .ethtool_speed
= SPEED_1000
,
755 .port_speed
= MLXSW_REG_QPSC_PORT_SPEED_1G
,
756 .shaper_time_exp
= 0,
757 .shaper_time_mantissa
= 12,
760 .port_to_shaper_credits
= 1,
761 .ing_timestamp_inc
= -35,
762 .egr_timestamp_inc
= 35,
765 .ethtool_speed
= SPEED_10000
,
766 .port_speed
= MLXSW_REG_QPSC_PORT_SPEED_10G
,
767 .shaper_time_exp
= 0,
768 .shaper_time_mantissa
= 2,
771 .port_to_shaper_credits
= 1,
772 .ing_timestamp_inc
= -11,
773 .egr_timestamp_inc
= 11,
776 .ethtool_speed
= SPEED_25000
,
777 .port_speed
= MLXSW_REG_QPSC_PORT_SPEED_25G
,
778 .shaper_time_exp
= 0,
779 .shaper_time_mantissa
= 0,
782 .port_to_shaper_credits
= 1,
783 .ing_timestamp_inc
= -14,
784 .egr_timestamp_inc
= 14,
788 #define MLXSW_SP1_PTP_SHAPER_PARAMS_LEN ARRAY_SIZE(mlxsw_sp1_ptp_shaper_params)
790 static int mlxsw_sp1_ptp_shaper_params_set(struct mlxsw_sp
*mlxsw_sp
)
792 const struct mlxsw_sp1_ptp_shaper_params
*params
;
793 char qpsc_pl
[MLXSW_REG_QPSC_LEN
];
796 for (i
= 0; i
< MLXSW_SP1_PTP_SHAPER_PARAMS_LEN
; i
++) {
797 params
= &mlxsw_sp1_ptp_shaper_params
[i
];
798 mlxsw_reg_qpsc_pack(qpsc_pl
, params
->port_speed
,
799 params
->shaper_time_exp
,
800 params
->shaper_time_mantissa
,
801 params
->shaper_inc
, params
->shaper_bs
,
802 params
->port_to_shaper_credits
,
803 params
->ing_timestamp_inc
,
804 params
->egr_timestamp_inc
);
805 err
= mlxsw_reg_write(mlxsw_sp
->core
, MLXSW_REG(qpsc
), qpsc_pl
);
813 struct mlxsw_sp_ptp_state
*mlxsw_sp1_ptp_init(struct mlxsw_sp
*mlxsw_sp
)
815 struct mlxsw_sp_ptp_state
*ptp_state
;
819 err
= mlxsw_sp1_ptp_shaper_params_set(mlxsw_sp
);
823 ptp_state
= kzalloc(sizeof(*ptp_state
), GFP_KERNEL
);
825 return ERR_PTR(-ENOMEM
);
826 ptp_state
->mlxsw_sp
= mlxsw_sp
;
828 spin_lock_init(&ptp_state
->unmatched_lock
);
830 err
= rhltable_init(&ptp_state
->unmatched_ht
,
831 &mlxsw_sp1_ptp_unmatched_ht_params
);
833 goto err_hashtable_init
;
835 /* Delive these message types as PTP0. */
836 message_type
= BIT(MLXSW_SP_PTP_MESSAGE_TYPE_SYNC
) |
837 BIT(MLXSW_SP_PTP_MESSAGE_TYPE_DELAY_REQ
) |
838 BIT(MLXSW_SP_PTP_MESSAGE_TYPE_PDELAY_REQ
) |
839 BIT(MLXSW_SP_PTP_MESSAGE_TYPE_PDELAY_RESP
);
840 err
= mlxsw_sp_ptp_mtptpt_set(mlxsw_sp
, MLXSW_REG_MTPTPT_TRAP_ID_PTP0
,
845 /* Everything else is PTP1. */
846 message_type
= ~message_type
;
847 err
= mlxsw_sp_ptp_mtptpt_set(mlxsw_sp
, MLXSW_REG_MTPTPT_TRAP_ID_PTP1
,
850 goto err_mtptpt1_set
;
852 err
= mlxsw_sp1_ptp_set_fifo_clr_on_trap(mlxsw_sp
, true);
856 INIT_DELAYED_WORK(&ptp_state
->ht_gc_dw
, mlxsw_sp1_ptp_ht_gc
);
857 mlxsw_core_schedule_dw(&ptp_state
->ht_gc_dw
,
858 MLXSW_SP1_PTP_HT_GC_INTERVAL
);
862 mlxsw_sp_ptp_mtptpt_set(mlxsw_sp
, MLXSW_REG_MTPTPT_TRAP_ID_PTP1
, 0);
864 mlxsw_sp_ptp_mtptpt_set(mlxsw_sp
, MLXSW_REG_MTPTPT_TRAP_ID_PTP0
, 0);
866 rhltable_destroy(&ptp_state
->unmatched_ht
);
872 void mlxsw_sp1_ptp_fini(struct mlxsw_sp_ptp_state
*ptp_state
)
874 struct mlxsw_sp
*mlxsw_sp
= ptp_state
->mlxsw_sp
;
876 cancel_delayed_work_sync(&ptp_state
->ht_gc_dw
);
877 mlxsw_sp1_ptp_mtpppc_set(mlxsw_sp
, 0, 0);
878 mlxsw_sp1_ptp_set_fifo_clr_on_trap(mlxsw_sp
, false);
879 mlxsw_sp_ptp_mtptpt_set(mlxsw_sp
, MLXSW_REG_MTPTPT_TRAP_ID_PTP1
, 0);
880 mlxsw_sp_ptp_mtptpt_set(mlxsw_sp
, MLXSW_REG_MTPTPT_TRAP_ID_PTP0
, 0);
881 rhltable_free_and_destroy(&ptp_state
->unmatched_ht
,
882 &mlxsw_sp1_ptp_unmatched_free_fn
, NULL
);
886 int mlxsw_sp1_ptp_hwtstamp_get(struct mlxsw_sp_port
*mlxsw_sp_port
,
887 struct hwtstamp_config
*config
)
889 *config
= mlxsw_sp_port
->ptp
.hwtstamp_config
;
893 static int mlxsw_sp_ptp_get_message_types(const struct hwtstamp_config
*config
,
894 u16
*p_ing_types
, u16
*p_egr_types
,
895 enum hwtstamp_rx_filters
*p_rx_filter
)
897 enum hwtstamp_rx_filters rx_filter
= config
->rx_filter
;
898 enum hwtstamp_tx_types tx_type
= config
->tx_type
;
899 u16 ing_types
= 0x00;
900 u16 egr_types
= 0x00;
903 case HWTSTAMP_TX_OFF
:
909 case HWTSTAMP_TX_ONESTEP_SYNC
:
914 case HWTSTAMP_FILTER_NONE
:
917 case HWTSTAMP_FILTER_PTP_V1_L4_SYNC
:
918 case HWTSTAMP_FILTER_PTP_V2_L4_SYNC
:
919 case HWTSTAMP_FILTER_PTP_V2_L2_SYNC
:
920 case HWTSTAMP_FILTER_PTP_V2_SYNC
:
923 case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ
:
924 case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ
:
925 case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ
:
926 case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ
:
929 case HWTSTAMP_FILTER_PTP_V1_L4_EVENT
:
930 case HWTSTAMP_FILTER_PTP_V2_L4_EVENT
:
931 case HWTSTAMP_FILTER_PTP_V2_L2_EVENT
:
932 case HWTSTAMP_FILTER_PTP_V2_EVENT
:
935 case HWTSTAMP_FILTER_ALL
:
938 case HWTSTAMP_FILTER_SOME
:
939 case HWTSTAMP_FILTER_NTP_ALL
:
943 *p_ing_types
= ing_types
;
944 *p_egr_types
= egr_types
;
945 *p_rx_filter
= rx_filter
;
949 static int mlxsw_sp1_ptp_mtpppc_update(struct mlxsw_sp_port
*mlxsw_sp_port
,
950 u16 ing_types
, u16 egr_types
)
952 struct mlxsw_sp
*mlxsw_sp
= mlxsw_sp_port
->mlxsw_sp
;
953 struct mlxsw_sp_port
*tmp
;
954 u16 orig_ing_types
= 0;
955 u16 orig_egr_types
= 0;
959 /* MTPPPC configures timestamping globally, not per port. Find the
960 * configuration that contains all configured timestamping requests.
962 for (i
= 1; i
< mlxsw_core_max_ports(mlxsw_sp
->core
); i
++) {
963 tmp
= mlxsw_sp
->ports
[i
];
965 orig_ing_types
|= tmp
->ptp
.ing_types
;
966 orig_egr_types
|= tmp
->ptp
.egr_types
;
968 if (tmp
&& tmp
!= mlxsw_sp_port
) {
969 ing_types
|= tmp
->ptp
.ing_types
;
970 egr_types
|= tmp
->ptp
.egr_types
;
974 if ((ing_types
|| egr_types
) && !(orig_ing_types
|| orig_egr_types
)) {
975 err
= mlxsw_sp_nve_inc_parsing_depth_get(mlxsw_sp
);
977 netdev_err(mlxsw_sp_port
->dev
, "Failed to increase parsing depth");
981 if (!(ing_types
|| egr_types
) && (orig_ing_types
|| orig_egr_types
))
982 mlxsw_sp_nve_inc_parsing_depth_put(mlxsw_sp
);
984 return mlxsw_sp1_ptp_mtpppc_set(mlxsw_sp_port
->mlxsw_sp
,
985 ing_types
, egr_types
);
988 static bool mlxsw_sp1_ptp_hwtstamp_enabled(struct mlxsw_sp_port
*mlxsw_sp_port
)
990 return mlxsw_sp_port
->ptp
.ing_types
|| mlxsw_sp_port
->ptp
.egr_types
;
994 mlxsw_sp1_ptp_port_shaper_set(struct mlxsw_sp_port
*mlxsw_sp_port
, bool enable
)
996 struct mlxsw_sp
*mlxsw_sp
= mlxsw_sp_port
->mlxsw_sp
;
997 char qeec_pl
[MLXSW_REG_QEEC_LEN
];
999 mlxsw_reg_qeec_ptps_pack(qeec_pl
, mlxsw_sp_port
->local_port
, enable
);
1000 return mlxsw_reg_write(mlxsw_sp
->core
, MLXSW_REG(qeec
), qeec_pl
);
1003 static int mlxsw_sp1_ptp_port_shaper_check(struct mlxsw_sp_port
*mlxsw_sp_port
)
1005 const struct mlxsw_sp_port_type_speed_ops
*port_type_speed_ops
;
1006 struct mlxsw_sp
*mlxsw_sp
= mlxsw_sp_port
->mlxsw_sp
;
1007 char ptys_pl
[MLXSW_REG_PTYS_LEN
];
1008 u32 eth_proto_oper
, speed
;
1012 if (!mlxsw_sp1_ptp_hwtstamp_enabled(mlxsw_sp_port
))
1013 return mlxsw_sp1_ptp_port_shaper_set(mlxsw_sp_port
, false);
1015 port_type_speed_ops
= mlxsw_sp
->port_type_speed_ops
;
1016 port_type_speed_ops
->reg_ptys_eth_pack(mlxsw_sp
, ptys_pl
,
1017 mlxsw_sp_port
->local_port
, 0,
1019 err
= mlxsw_reg_query(mlxsw_sp
->core
, MLXSW_REG(ptys
), ptys_pl
);
1022 port_type_speed_ops
->reg_ptys_eth_unpack(mlxsw_sp
, ptys_pl
, NULL
, NULL
,
1025 speed
= port_type_speed_ops
->from_ptys_speed(mlxsw_sp
, eth_proto_oper
);
1026 for (i
= 0; i
< MLXSW_SP1_PTP_SHAPER_PARAMS_LEN
; i
++) {
1027 if (mlxsw_sp1_ptp_shaper_params
[i
].ethtool_speed
== speed
) {
1033 return mlxsw_sp1_ptp_port_shaper_set(mlxsw_sp_port
, ptps
);
1036 void mlxsw_sp1_ptp_shaper_work(struct work_struct
*work
)
1038 struct delayed_work
*dwork
= to_delayed_work(work
);
1039 struct mlxsw_sp_port
*mlxsw_sp_port
;
1042 mlxsw_sp_port
= container_of(dwork
, struct mlxsw_sp_port
,
1045 if (!mlxsw_sp1_ptp_hwtstamp_enabled(mlxsw_sp_port
))
1048 err
= mlxsw_sp1_ptp_port_shaper_check(mlxsw_sp_port
);
1050 netdev_err(mlxsw_sp_port
->dev
, "Failed to set up PTP shaper\n");
1053 int mlxsw_sp1_ptp_hwtstamp_set(struct mlxsw_sp_port
*mlxsw_sp_port
,
1054 struct hwtstamp_config
*config
)
1056 enum hwtstamp_rx_filters rx_filter
;
1061 err
= mlxsw_sp_ptp_get_message_types(config
, &ing_types
, &egr_types
,
1066 err
= mlxsw_sp1_ptp_mtpppc_update(mlxsw_sp_port
, ing_types
, egr_types
);
1070 mlxsw_sp_port
->ptp
.hwtstamp_config
= *config
;
1071 mlxsw_sp_port
->ptp
.ing_types
= ing_types
;
1072 mlxsw_sp_port
->ptp
.egr_types
= egr_types
;
1074 err
= mlxsw_sp1_ptp_port_shaper_check(mlxsw_sp_port
);
1078 /* Notify the ioctl caller what we are actually timestamping. */
1079 config
->rx_filter
= rx_filter
;
1084 int mlxsw_sp1_ptp_get_ts_info(struct mlxsw_sp
*mlxsw_sp
,
1085 struct ethtool_ts_info
*info
)
1087 info
->phc_index
= ptp_clock_index(mlxsw_sp
->clock
->ptp
);
1089 info
->so_timestamping
= SOF_TIMESTAMPING_TX_HARDWARE
|
1090 SOF_TIMESTAMPING_RX_HARDWARE
|
1091 SOF_TIMESTAMPING_RAW_HARDWARE
;
1093 info
->tx_types
= BIT(HWTSTAMP_TX_OFF
) |
1094 BIT(HWTSTAMP_TX_ON
);
1096 info
->rx_filters
= BIT(HWTSTAMP_FILTER_NONE
) |
1097 BIT(HWTSTAMP_FILTER_ALL
);