]>
Commit | Line | Data |
---|---|---|
992aa864 ST |
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> | |
d92e4e6e PM |
9 | #include <linux/rhashtable.h> |
10 | #include <linux/ptp_classify.h> | |
11 | #include <linux/if_ether.h> | |
12 | #include <linux/if_vlan.h> | |
87486427 | 13 | #include <linux/net_tstamp.h> |
992aa864 | 14 | |
810256ce | 15 | #include "spectrum.h" |
992aa864 ST |
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 | ||
5d23e415 PM |
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 | ||
810256ce | 30 | struct mlxsw_sp_ptp_state { |
5d23e415 | 31 | struct mlxsw_sp *mlxsw_sp; |
810256ce PM |
32 | struct rhashtable unmatched_ht; |
33 | spinlock_t unmatched_lock; /* protects the HT */ | |
5d23e415 PM |
34 | struct delayed_work ht_gc_dw; |
35 | u32 gc_cycle; | |
810256ce PM |
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 rhash_head ht_node; | |
49 | struct rcu_head rcu; | |
50 | struct sk_buff *skb; | |
51 | u64 timestamp; | |
5d23e415 | 52 | u32 gc_cycle; |
810256ce PM |
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 | ||
992aa864 ST |
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; | |
cd4bb2a3 | 128 | u64 next_sec, next_sec_in_nsec, cycles; |
992aa864 ST |
129 | char mtutc_pl[MLXSW_REG_MTUTC_LEN]; |
130 | char mtpps_pl[MLXSW_REG_MTPPS_LEN]; | |
992aa864 ST |
131 | int err; |
132 | ||
cd4bb2a3 | 133 | next_sec = div_u64(nsec, NSEC_PER_SEC) + 1; |
992aa864 ST |
134 | next_sec_in_nsec = next_sec * NSEC_PER_SEC; |
135 | ||
89e602ee | 136 | spin_lock_bh(&clock->lock); |
992aa864 | 137 | cycles = mlxsw_sp1_ptp_ns2cycles(&clock->tc, next_sec_in_nsec); |
89e602ee | 138 | spin_unlock_bh(&clock->lock); |
992aa864 ST |
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 | ||
89e602ee | 171 | spin_lock_bh(&clock->lock); |
992aa864 ST |
172 | timecounter_read(&clock->tc); |
173 | clock->cycles.mult = neg_adj ? clock->nominal_c_mult - diff : | |
174 | clock->nominal_c_mult + diff; | |
89e602ee | 175 | spin_unlock_bh(&clock->lock); |
992aa864 ST |
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 | ||
89e602ee | 186 | spin_lock_bh(&clock->lock); |
992aa864 ST |
187 | timecounter_adjtime(&clock->tc, delta); |
188 | nsec = timecounter_read(&clock->tc); | |
89e602ee | 189 | spin_unlock_bh(&clock->lock); |
992aa864 ST |
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 | ||
89e602ee | 202 | spin_lock_bh(&clock->lock); |
992aa864 ST |
203 | cycles = __mlxsw_sp1_ptp_read_frc(clock, sts); |
204 | nsec = timecounter_cyc2time(&clock->tc, cycles); | |
89e602ee | 205 | spin_unlock_bh(&clock->lock); |
992aa864 ST |
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 | ||
89e602ee | 219 | spin_lock_bh(&clock->lock); |
992aa864 ST |
220 | timecounter_init(&clock->tc, &clock->cycles, nsec); |
221 | nsec = timecounter_read(&clock->tc); | |
89e602ee | 222 | spin_unlock_bh(&clock->lock); |
992aa864 ST |
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 | ||
89e602ee | 244 | spin_lock_bh(&clock->lock); |
992aa864 | 245 | timecounter_read(&clock->tc); |
89e602ee | 246 | spin_unlock_bh(&clock->lock); |
992aa864 ST |
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 | } | |
aed4b572 | 311 | |
d92e4e6e PM |
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 struct mlxsw_sp1_ptp_unmatched * | |
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 | { | |
5d23e415 | 368 | int cycles = MLXSW_SP1_PTP_HT_GC_TIMEOUT / MLXSW_SP1_PTP_HT_GC_INTERVAL; |
d92e4e6e PM |
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; | |
372 | ||
373 | unmatched = kzalloc(sizeof(*unmatched), GFP_ATOMIC); | |
374 | if (!unmatched) | |
375 | return ERR_PTR(-ENOMEM); | |
376 | ||
377 | unmatched->key = key; | |
378 | unmatched->skb = skb; | |
379 | unmatched->timestamp = timestamp; | |
5d23e415 | 380 | unmatched->gc_cycle = mlxsw_sp->ptp_state->gc_cycle + cycles; |
d92e4e6e PM |
381 | |
382 | conflict = rhashtable_lookup_get_insert_fast(&ptp_state->unmatched_ht, | |
383 | &unmatched->ht_node, | |
384 | mlxsw_sp1_ptp_unmatched_ht_params); | |
385 | if (conflict) | |
386 | kfree(unmatched); | |
387 | ||
388 | return conflict; | |
389 | } | |
390 | ||
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) | |
394 | { | |
395 | return rhashtable_lookup(&mlxsw_sp->ptp_state->unmatched_ht, &key, | |
396 | mlxsw_sp1_ptp_unmatched_ht_params); | |
397 | } | |
398 | ||
399 | static int | |
400 | mlxsw_sp1_ptp_unmatched_remove(struct mlxsw_sp *mlxsw_sp, | |
401 | struct mlxsw_sp1_ptp_unmatched *unmatched) | |
402 | { | |
403 | return rhashtable_remove_fast(&mlxsw_sp->ptp_state->unmatched_ht, | |
404 | &unmatched->ht_node, | |
405 | mlxsw_sp1_ptp_unmatched_ht_params); | |
406 | } | |
407 | ||
408 | /* This function is called in the following scenarios: | |
409 | * | |
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. | |
5d23e415 PM |
413 | * 3) From GC indirectly through mlxsw_sp1_ptp_unmatched_finish(). |
414 | * This case is similar to 2) above. | |
d92e4e6e PM |
415 | */ |
416 | static void mlxsw_sp1_ptp_packet_finish(struct mlxsw_sp *mlxsw_sp, | |
417 | struct sk_buff *skb, u8 local_port, | |
418 | bool ingress, | |
419 | struct skb_shared_hwtstamps *hwtstamps) | |
420 | { | |
421 | struct mlxsw_sp_port *mlxsw_sp_port; | |
422 | ||
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. | |
426 | */ | |
427 | mlxsw_sp_port = mlxsw_sp->ports[local_port]; | |
dbcdb61a | 428 | if (!(mlxsw_sp_port && (!skb->dev || skb->dev == mlxsw_sp_port->dev))) { |
d92e4e6e PM |
429 | dev_kfree_skb_any(skb); |
430 | return; | |
431 | } | |
432 | ||
433 | if (ingress) { | |
434 | if (hwtstamps) | |
435 | *skb_hwtstamps(skb) = *hwtstamps; | |
436 | mlxsw_sp_rx_listener_no_mark_func(skb, local_port, mlxsw_sp); | |
437 | } else { | |
438 | /* skb_tstamp_tx() allows hwtstamps to be NULL. */ | |
439 | skb_tstamp_tx(skb, hwtstamps); | |
440 | dev_kfree_skb_any(skb); | |
441 | } | |
442 | } | |
443 | ||
444 | static void mlxsw_sp1_packet_timestamp(struct mlxsw_sp *mlxsw_sp, | |
445 | struct mlxsw_sp1_ptp_key key, | |
446 | struct sk_buff *skb, | |
447 | u64 timestamp) | |
448 | { | |
449 | struct skb_shared_hwtstamps hwtstamps; | |
450 | u64 nsec; | |
451 | ||
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); | |
455 | ||
456 | hwtstamps.hwtstamp = ns_to_ktime(nsec); | |
457 | mlxsw_sp1_ptp_packet_finish(mlxsw_sp, skb, | |
458 | key.local_port, key.ingress, &hwtstamps); | |
459 | } | |
460 | ||
461 | static void | |
462 | mlxsw_sp1_ptp_unmatched_finish(struct mlxsw_sp *mlxsw_sp, | |
463 | struct mlxsw_sp1_ptp_unmatched *unmatched) | |
464 | { | |
465 | if (unmatched->skb && unmatched->timestamp) | |
466 | mlxsw_sp1_packet_timestamp(mlxsw_sp, unmatched->key, | |
467 | unmatched->skb, | |
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); | |
474 | } | |
475 | ||
810256ce PM |
476 | static void mlxsw_sp1_ptp_unmatched_free_fn(void *ptr, void *arg) |
477 | { | |
478 | struct mlxsw_sp1_ptp_unmatched *unmatched = ptr; | |
479 | ||
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. | |
482 | */ | |
483 | if (unmatched->skb) | |
484 | dev_kfree_skb_any(unmatched->skb); | |
485 | kfree_rcu(unmatched, rcu); | |
486 | } | |
487 | ||
d92e4e6e PM |
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) | |
491 | { | |
492 | struct mlxsw_sp1_ptp_unmatched *unmatched, *conflict; | |
493 | int err; | |
494 | ||
495 | rcu_read_lock(); | |
496 | ||
497 | unmatched = mlxsw_sp1_ptp_unmatched_lookup(mlxsw_sp, key); | |
498 | ||
499 | spin_lock(&mlxsw_sp->ptp_state->unmatched_lock); | |
500 | ||
501 | if (unmatched) { | |
502 | /* There was an unmatched entry when we looked, but it may have | |
503 | * been removed before we took the lock. | |
504 | */ | |
505 | err = mlxsw_sp1_ptp_unmatched_remove(mlxsw_sp, unmatched); | |
506 | if (err) | |
507 | unmatched = NULL; | |
508 | } | |
509 | ||
510 | if (!unmatched) { | |
511 | /* We have no unmatched entry, but one may have been added after | |
512 | * we looked, but before we took the lock. | |
513 | */ | |
514 | unmatched = mlxsw_sp1_ptp_unmatched_save(mlxsw_sp, key, | |
515 | skb, timestamp); | |
516 | if (IS_ERR(unmatched)) { | |
517 | if (skb) | |
518 | mlxsw_sp1_ptp_packet_finish(mlxsw_sp, skb, | |
519 | key.local_port, | |
520 | key.ingress, NULL); | |
521 | unmatched = NULL; | |
522 | } else if (unmatched) { | |
523 | /* Save just told us, under lock, that the entry is | |
524 | * there, so this has to work. | |
525 | */ | |
526 | err = mlxsw_sp1_ptp_unmatched_remove(mlxsw_sp, | |
527 | unmatched); | |
528 | WARN_ON_ONCE(err); | |
529 | } | |
530 | } | |
531 | ||
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. | |
536 | */ | |
537 | ||
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. | |
546 | */ | |
547 | conflict = mlxsw_sp1_ptp_unmatched_save(mlxsw_sp, key, | |
548 | skb, timestamp); | |
549 | if (IS_ERR(conflict)) { | |
550 | if (skb) | |
551 | mlxsw_sp1_ptp_packet_finish(mlxsw_sp, skb, | |
552 | key.local_port, | |
553 | key.ingress, NULL); | |
554 | } else { | |
555 | /* Above, we removed an object with this key from the | |
556 | * hash table, under lock, so conflict can not be a | |
557 | * valid pointer. | |
558 | */ | |
559 | WARN_ON_ONCE(conflict); | |
560 | } | |
561 | } | |
562 | ||
563 | spin_unlock(&mlxsw_sp->ptp_state->unmatched_lock); | |
564 | ||
565 | if (unmatched) | |
566 | mlxsw_sp1_ptp_unmatched_finish(mlxsw_sp, unmatched); | |
567 | ||
568 | rcu_read_unlock(); | |
569 | } | |
570 | ||
571 | static void mlxsw_sp1_ptp_got_packet(struct mlxsw_sp *mlxsw_sp, | |
572 | struct sk_buff *skb, u8 local_port, | |
573 | bool ingress) | |
574 | { | |
575 | struct mlxsw_sp_port *mlxsw_sp_port; | |
576 | struct mlxsw_sp1_ptp_key key; | |
577 | u8 types; | |
578 | int err; | |
579 | ||
580 | mlxsw_sp_port = mlxsw_sp->ports[local_port]; | |
581 | if (!mlxsw_sp_port) | |
582 | goto immediate; | |
583 | ||
584 | types = ingress ? mlxsw_sp_port->ptp.ing_types : | |
585 | mlxsw_sp_port->ptp.egr_types; | |
586 | if (!types) | |
587 | goto immediate; | |
588 | ||
589 | memset(&key, 0, sizeof(key)); | |
590 | key.local_port = local_port; | |
591 | key.ingress = ingress; | |
592 | ||
593 | err = mlxsw_sp_ptp_parse(skb, &key.domain_number, &key.message_type, | |
594 | &key.sequence_id); | |
595 | if (err) | |
596 | goto immediate; | |
597 | ||
598 | /* For packets whose timestamping was not enabled on this port, don't | |
599 | * bother trying to match the timestamp. | |
600 | */ | |
601 | if (!((1 << key.message_type) & types)) | |
602 | goto immediate; | |
603 | ||
604 | mlxsw_sp1_ptp_got_piece(mlxsw_sp, key, skb, 0); | |
605 | return; | |
606 | ||
607 | immediate: | |
608 | mlxsw_sp1_ptp_packet_finish(mlxsw_sp, skb, local_port, ingress, NULL); | |
609 | } | |
610 | ||
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, | |
614 | u64 timestamp) | |
615 | { | |
616 | struct mlxsw_sp_port *mlxsw_sp_port; | |
617 | struct mlxsw_sp1_ptp_key key; | |
618 | u8 types; | |
619 | ||
620 | mlxsw_sp_port = mlxsw_sp->ports[local_port]; | |
621 | if (!mlxsw_sp_port) | |
622 | return; | |
623 | ||
624 | types = ingress ? mlxsw_sp_port->ptp.ing_types : | |
625 | mlxsw_sp_port->ptp.egr_types; | |
626 | ||
627 | /* For message types whose timestamping was not enabled on this port, | |
628 | * don't bother with the timestamp. | |
629 | */ | |
630 | if (!((1 << message_type) & types)) | |
631 | return; | |
632 | ||
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; | |
639 | ||
640 | mlxsw_sp1_ptp_got_piece(mlxsw_sp, key, NULL, timestamp); | |
641 | } | |
642 | ||
aed4b572 PM |
643 | void mlxsw_sp1_ptp_receive(struct mlxsw_sp *mlxsw_sp, struct sk_buff *skb, |
644 | u8 local_port) | |
645 | { | |
d92e4e6e PM |
646 | skb_reset_mac_header(skb); |
647 | mlxsw_sp1_ptp_got_packet(mlxsw_sp, skb, local_port, true); | |
aed4b572 | 648 | } |
0714256c PM |
649 | |
650 | void mlxsw_sp1_ptp_transmitted(struct mlxsw_sp *mlxsw_sp, | |
651 | struct sk_buff *skb, u8 local_port) | |
652 | { | |
d92e4e6e | 653 | mlxsw_sp1_ptp_got_packet(mlxsw_sp, skb, local_port, false); |
0714256c | 654 | } |
810256ce | 655 | |
5d23e415 PM |
656 | static void |
657 | mlxsw_sp1_ptp_ht_gc_collect(struct mlxsw_sp_ptp_state *ptp_state, | |
658 | struct mlxsw_sp1_ptp_unmatched *unmatched) | |
659 | { | |
660 | int err; | |
661 | ||
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. | |
668 | */ | |
669 | local_bh_disable(); | |
670 | ||
671 | spin_lock(&ptp_state->unmatched_lock); | |
672 | err = rhashtable_remove_fast(&ptp_state->unmatched_ht, | |
673 | &unmatched->ht_node, | |
674 | mlxsw_sp1_ptp_unmatched_ht_params); | |
675 | spin_unlock(&ptp_state->unmatched_lock); | |
676 | ||
677 | if (err) | |
678 | /* The packet was matched with timestamp during the walk. */ | |
679 | goto out; | |
680 | ||
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. | |
686 | */ | |
687 | mlxsw_sp1_ptp_unmatched_finish(ptp_state->mlxsw_sp, unmatched); | |
688 | ||
689 | out: | |
690 | local_bh_enable(); | |
691 | } | |
692 | ||
693 | static void mlxsw_sp1_ptp_ht_gc(struct work_struct *work) | |
694 | { | |
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; | |
699 | u32 gc_cycle; | |
700 | void *obj; | |
701 | ||
702 | ptp_state = container_of(dwork, struct mlxsw_sp_ptp_state, ht_gc_dw); | |
703 | gc_cycle = ptp_state->gc_cycle++; | |
704 | ||
705 | rhashtable_walk_enter(&ptp_state->unmatched_ht, &iter); | |
706 | rhashtable_walk_start(&iter); | |
707 | while ((obj = rhashtable_walk_next(&iter))) { | |
708 | if (IS_ERR(obj)) | |
709 | continue; | |
710 | ||
711 | unmatched = obj; | |
712 | if (unmatched->gc_cycle <= gc_cycle) | |
713 | mlxsw_sp1_ptp_ht_gc_collect(ptp_state, unmatched); | |
714 | } | |
715 | rhashtable_walk_stop(&iter); | |
716 | rhashtable_walk_exit(&iter); | |
717 | ||
718 | mlxsw_core_schedule_dw(&ptp_state->ht_gc_dw, | |
719 | MLXSW_SP1_PTP_HT_GC_INTERVAL); | |
720 | } | |
721 | ||
a773c76c PM |
722 | static int mlxsw_sp_ptp_mtptpt_set(struct mlxsw_sp *mlxsw_sp, |
723 | enum mlxsw_reg_mtptpt_trap_id trap_id, | |
724 | u16 message_type) | |
725 | { | |
726 | char mtptpt_pl[MLXSW_REG_MTPTPT_LEN]; | |
727 | ||
728 | mlxsw_reg_mtptptp_pack(mtptpt_pl, trap_id, message_type); | |
729 | return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(mtptpt), mtptpt_pl); | |
730 | } | |
731 | ||
732 | static int mlxsw_sp1_ptp_set_fifo_clr_on_trap(struct mlxsw_sp *mlxsw_sp, | |
733 | bool clr) | |
734 | { | |
735 | char mogcr_pl[MLXSW_REG_MOGCR_LEN] = {0}; | |
736 | int err; | |
737 | ||
738 | err = mlxsw_reg_query(mlxsw_sp->core, MLXSW_REG(mogcr), mogcr_pl); | |
739 | if (err) | |
740 | return err; | |
741 | ||
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); | |
745 | } | |
746 | ||
87486427 PM |
747 | static int mlxsw_sp1_ptp_mtpppc_set(struct mlxsw_sp *mlxsw_sp, |
748 | u16 ing_types, u16 egr_types) | |
749 | { | |
750 | char mtpppc_pl[MLXSW_REG_MTPPPC_LEN]; | |
751 | ||
752 | mlxsw_reg_mtpppc_pack(mtpppc_pl, ing_types, egr_types); | |
753 | return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(mtpppc), mtpppc_pl); | |
754 | } | |
755 | ||
399569cb ST |
756 | struct mlxsw_sp1_ptp_shaper_params { |
757 | u32 ethtool_speed; | |
758 | enum mlxsw_reg_qpsc_port_speed port_speed; | |
759 | u8 shaper_time_exp; | |
760 | u8 shaper_time_mantissa; | |
761 | u8 shaper_inc; | |
762 | u8 shaper_bs; | |
763 | u8 port_to_shaper_credits; | |
764 | int ing_timestamp_inc; | |
765 | int egr_timestamp_inc; | |
766 | }; | |
767 | ||
768 | static const struct mlxsw_sp1_ptp_shaper_params | |
769 | mlxsw_sp1_ptp_shaper_params[] = { | |
770 | }; | |
771 | ||
772 | #define MLXSW_SP1_PTP_SHAPER_PARAMS_LEN ARRAY_SIZE(mlxsw_sp1_ptp_shaper_params) | |
773 | ||
774 | static int mlxsw_sp1_ptp_shaper_params_set(struct mlxsw_sp *mlxsw_sp) | |
775 | { | |
776 | const struct mlxsw_sp1_ptp_shaper_params *params; | |
777 | char qpsc_pl[MLXSW_REG_QPSC_LEN]; | |
778 | int i, err; | |
779 | ||
780 | for (i = 0; i < MLXSW_SP1_PTP_SHAPER_PARAMS_LEN; i++) { | |
781 | params = &mlxsw_sp1_ptp_shaper_params[i]; | |
782 | mlxsw_reg_qpsc_pack(qpsc_pl, params->port_speed, | |
783 | params->shaper_time_exp, | |
784 | params->shaper_time_mantissa, | |
785 | params->shaper_inc, params->shaper_bs, | |
786 | params->port_to_shaper_credits, | |
787 | params->ing_timestamp_inc, | |
788 | params->egr_timestamp_inc); | |
789 | err = mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(qpsc), qpsc_pl); | |
790 | if (err) | |
791 | return err; | |
792 | } | |
793 | ||
794 | return 0; | |
795 | } | |
796 | ||
810256ce PM |
797 | struct mlxsw_sp_ptp_state *mlxsw_sp1_ptp_init(struct mlxsw_sp *mlxsw_sp) |
798 | { | |
799 | struct mlxsw_sp_ptp_state *ptp_state; | |
a773c76c | 800 | u16 message_type; |
810256ce PM |
801 | int err; |
802 | ||
399569cb ST |
803 | err = mlxsw_sp1_ptp_shaper_params_set(mlxsw_sp); |
804 | if (err) | |
805 | return ERR_PTR(err); | |
806 | ||
810256ce PM |
807 | ptp_state = kzalloc(sizeof(*ptp_state), GFP_KERNEL); |
808 | if (!ptp_state) | |
809 | return ERR_PTR(-ENOMEM); | |
5d23e415 | 810 | ptp_state->mlxsw_sp = mlxsw_sp; |
810256ce PM |
811 | |
812 | spin_lock_init(&ptp_state->unmatched_lock); | |
813 | ||
814 | err = rhashtable_init(&ptp_state->unmatched_ht, | |
815 | &mlxsw_sp1_ptp_unmatched_ht_params); | |
816 | if (err) | |
817 | goto err_hashtable_init; | |
818 | ||
a773c76c PM |
819 | /* Delive these message types as PTP0. */ |
820 | message_type = BIT(MLXSW_SP_PTP_MESSAGE_TYPE_SYNC) | | |
821 | BIT(MLXSW_SP_PTP_MESSAGE_TYPE_DELAY_REQ) | | |
822 | BIT(MLXSW_SP_PTP_MESSAGE_TYPE_PDELAY_REQ) | | |
823 | BIT(MLXSW_SP_PTP_MESSAGE_TYPE_PDELAY_RESP); | |
824 | err = mlxsw_sp_ptp_mtptpt_set(mlxsw_sp, MLXSW_REG_MTPTPT_TRAP_ID_PTP0, | |
825 | message_type); | |
826 | if (err) | |
827 | goto err_mtptpt_set; | |
828 | ||
829 | /* Everything else is PTP1. */ | |
830 | message_type = ~message_type; | |
831 | err = mlxsw_sp_ptp_mtptpt_set(mlxsw_sp, MLXSW_REG_MTPTPT_TRAP_ID_PTP1, | |
832 | message_type); | |
833 | if (err) | |
834 | goto err_mtptpt1_set; | |
835 | ||
836 | err = mlxsw_sp1_ptp_set_fifo_clr_on_trap(mlxsw_sp, true); | |
837 | if (err) | |
838 | goto err_fifo_clr; | |
839 | ||
5d23e415 PM |
840 | INIT_DELAYED_WORK(&ptp_state->ht_gc_dw, mlxsw_sp1_ptp_ht_gc); |
841 | mlxsw_core_schedule_dw(&ptp_state->ht_gc_dw, | |
842 | MLXSW_SP1_PTP_HT_GC_INTERVAL); | |
810256ce PM |
843 | return ptp_state; |
844 | ||
a773c76c PM |
845 | err_fifo_clr: |
846 | mlxsw_sp_ptp_mtptpt_set(mlxsw_sp, MLXSW_REG_MTPTPT_TRAP_ID_PTP1, 0); | |
847 | err_mtptpt1_set: | |
848 | mlxsw_sp_ptp_mtptpt_set(mlxsw_sp, MLXSW_REG_MTPTPT_TRAP_ID_PTP0, 0); | |
849 | err_mtptpt_set: | |
850 | rhashtable_destroy(&ptp_state->unmatched_ht); | |
810256ce PM |
851 | err_hashtable_init: |
852 | kfree(ptp_state); | |
853 | return ERR_PTR(err); | |
854 | } | |
855 | ||
856 | void mlxsw_sp1_ptp_fini(struct mlxsw_sp_ptp_state *ptp_state) | |
857 | { | |
a773c76c PM |
858 | struct mlxsw_sp *mlxsw_sp = ptp_state->mlxsw_sp; |
859 | ||
5d23e415 | 860 | cancel_delayed_work_sync(&ptp_state->ht_gc_dw); |
87486427 | 861 | mlxsw_sp1_ptp_mtpppc_set(mlxsw_sp, 0, 0); |
a773c76c PM |
862 | mlxsw_sp1_ptp_set_fifo_clr_on_trap(mlxsw_sp, false); |
863 | 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); | |
810256ce PM |
865 | rhashtable_free_and_destroy(&ptp_state->unmatched_ht, |
866 | &mlxsw_sp1_ptp_unmatched_free_fn, NULL); | |
867 | kfree(ptp_state); | |
868 | } | |
87486427 PM |
869 | |
870 | int mlxsw_sp1_ptp_hwtstamp_get(struct mlxsw_sp_port *mlxsw_sp_port, | |
871 | struct hwtstamp_config *config) | |
872 | { | |
873 | *config = mlxsw_sp_port->ptp.hwtstamp_config; | |
874 | return 0; | |
875 | } | |
876 | ||
877 | static int mlxsw_sp_ptp_get_message_types(const struct hwtstamp_config *config, | |
878 | u16 *p_ing_types, u16 *p_egr_types, | |
879 | enum hwtstamp_rx_filters *p_rx_filter) | |
880 | { | |
881 | enum hwtstamp_rx_filters rx_filter = config->rx_filter; | |
882 | enum hwtstamp_tx_types tx_type = config->tx_type; | |
883 | u16 ing_types = 0x00; | |
884 | u16 egr_types = 0x00; | |
885 | ||
886 | switch (tx_type) { | |
887 | case HWTSTAMP_TX_OFF: | |
888 | egr_types = 0x00; | |
889 | break; | |
890 | case HWTSTAMP_TX_ON: | |
891 | egr_types = 0xff; | |
892 | break; | |
893 | case HWTSTAMP_TX_ONESTEP_SYNC: | |
894 | return -ERANGE; | |
895 | } | |
896 | ||
897 | switch (rx_filter) { | |
898 | case HWTSTAMP_FILTER_NONE: | |
899 | ing_types = 0x00; | |
900 | break; | |
901 | case HWTSTAMP_FILTER_PTP_V1_L4_SYNC: | |
902 | case HWTSTAMP_FILTER_PTP_V2_L4_SYNC: | |
903 | case HWTSTAMP_FILTER_PTP_V2_L2_SYNC: | |
904 | case HWTSTAMP_FILTER_PTP_V2_SYNC: | |
905 | ing_types = 0x01; | |
906 | break; | |
907 | case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ: | |
908 | case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ: | |
909 | case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ: | |
910 | case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ: | |
911 | ing_types = 0x02; | |
912 | break; | |
913 | case HWTSTAMP_FILTER_PTP_V1_L4_EVENT: | |
914 | case HWTSTAMP_FILTER_PTP_V2_L4_EVENT: | |
915 | case HWTSTAMP_FILTER_PTP_V2_L2_EVENT: | |
916 | case HWTSTAMP_FILTER_PTP_V2_EVENT: | |
917 | ing_types = 0x0f; | |
918 | break; | |
919 | case HWTSTAMP_FILTER_ALL: | |
920 | ing_types = 0xff; | |
921 | break; | |
922 | case HWTSTAMP_FILTER_SOME: | |
923 | case HWTSTAMP_FILTER_NTP_ALL: | |
924 | return -ERANGE; | |
925 | } | |
926 | ||
927 | *p_ing_types = ing_types; | |
928 | *p_egr_types = egr_types; | |
929 | *p_rx_filter = rx_filter; | |
930 | return 0; | |
931 | } | |
932 | ||
933 | static int mlxsw_sp1_ptp_mtpppc_update(struct mlxsw_sp_port *mlxsw_sp_port, | |
934 | u16 ing_types, u16 egr_types) | |
935 | { | |
936 | struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp; | |
937 | struct mlxsw_sp_port *tmp; | |
938 | int i; | |
939 | ||
940 | /* MTPPPC configures timestamping globally, not per port. Find the | |
941 | * configuration that contains all configured timestamping requests. | |
942 | */ | |
943 | for (i = 1; i < mlxsw_core_max_ports(mlxsw_sp->core); i++) { | |
944 | tmp = mlxsw_sp->ports[i]; | |
945 | if (tmp && tmp != mlxsw_sp_port) { | |
946 | ing_types |= tmp->ptp.ing_types; | |
947 | egr_types |= tmp->ptp.egr_types; | |
948 | } | |
949 | } | |
950 | ||
951 | return mlxsw_sp1_ptp_mtpppc_set(mlxsw_sp_port->mlxsw_sp, | |
952 | ing_types, egr_types); | |
953 | } | |
954 | ||
eceed3b1 ST |
955 | static bool mlxsw_sp1_ptp_hwtstamp_enabled(struct mlxsw_sp_port *mlxsw_sp_port) |
956 | { | |
957 | return mlxsw_sp_port->ptp.ing_types || mlxsw_sp_port->ptp.egr_types; | |
958 | } | |
959 | ||
960 | static int | |
961 | mlxsw_sp1_ptp_port_shaper_set(struct mlxsw_sp_port *mlxsw_sp_port, bool enable) | |
962 | { | |
963 | struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp; | |
964 | char qeec_pl[MLXSW_REG_QEEC_LEN]; | |
965 | ||
966 | mlxsw_reg_qeec_ptps_pack(qeec_pl, mlxsw_sp_port->local_port, enable); | |
967 | return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(qeec), qeec_pl); | |
968 | } | |
969 | ||
970 | static int mlxsw_sp1_ptp_port_shaper_check(struct mlxsw_sp_port *mlxsw_sp_port) | |
971 | { | |
972 | const struct mlxsw_sp_port_type_speed_ops *port_type_speed_ops; | |
973 | struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp; | |
974 | char ptys_pl[MLXSW_REG_PTYS_LEN]; | |
975 | u32 eth_proto_oper, speed; | |
976 | bool ptps = false; | |
977 | int err, i; | |
978 | ||
979 | if (!mlxsw_sp1_ptp_hwtstamp_enabled(mlxsw_sp_port)) | |
980 | return mlxsw_sp1_ptp_port_shaper_set(mlxsw_sp_port, false); | |
981 | ||
982 | port_type_speed_ops = mlxsw_sp->port_type_speed_ops; | |
983 | port_type_speed_ops->reg_ptys_eth_pack(mlxsw_sp, ptys_pl, | |
984 | mlxsw_sp_port->local_port, 0, | |
985 | false); | |
986 | err = mlxsw_reg_query(mlxsw_sp->core, MLXSW_REG(ptys), ptys_pl); | |
987 | if (err) | |
988 | return err; | |
989 | port_type_speed_ops->reg_ptys_eth_unpack(mlxsw_sp, ptys_pl, NULL, NULL, | |
990 | ð_proto_oper); | |
991 | ||
992 | speed = port_type_speed_ops->from_ptys_speed(mlxsw_sp, eth_proto_oper); | |
993 | for (i = 0; i < MLXSW_SP1_PTP_SHAPER_PARAMS_LEN; i++) { | |
994 | if (mlxsw_sp1_ptp_shaper_params[i].ethtool_speed == speed) { | |
995 | ptps = true; | |
996 | break; | |
997 | } | |
998 | } | |
999 | ||
1000 | return mlxsw_sp1_ptp_port_shaper_set(mlxsw_sp_port, ptps); | |
1001 | } | |
1002 | ||
5fc17338 ST |
1003 | void mlxsw_sp1_ptp_shaper_work(struct work_struct *work) |
1004 | { | |
1005 | struct delayed_work *dwork = to_delayed_work(work); | |
1006 | struct mlxsw_sp_port *mlxsw_sp_port; | |
1007 | int err; | |
1008 | ||
1009 | mlxsw_sp_port = container_of(dwork, struct mlxsw_sp_port, | |
1010 | ptp.shaper_dw); | |
1011 | ||
1012 | if (!mlxsw_sp1_ptp_hwtstamp_enabled(mlxsw_sp_port)) | |
1013 | return; | |
1014 | ||
1015 | err = mlxsw_sp1_ptp_port_shaper_check(mlxsw_sp_port); | |
1016 | if (err) | |
1017 | netdev_err(mlxsw_sp_port->dev, "Failed to set up PTP shaper\n"); | |
1018 | } | |
1019 | ||
87486427 PM |
1020 | int mlxsw_sp1_ptp_hwtstamp_set(struct mlxsw_sp_port *mlxsw_sp_port, |
1021 | struct hwtstamp_config *config) | |
1022 | { | |
1023 | enum hwtstamp_rx_filters rx_filter; | |
1024 | u16 ing_types; | |
1025 | u16 egr_types; | |
1026 | int err; | |
1027 | ||
1028 | err = mlxsw_sp_ptp_get_message_types(config, &ing_types, &egr_types, | |
1029 | &rx_filter); | |
1030 | if (err) | |
1031 | return err; | |
1032 | ||
1033 | err = mlxsw_sp1_ptp_mtpppc_update(mlxsw_sp_port, ing_types, egr_types); | |
1034 | if (err) | |
1035 | return err; | |
1036 | ||
1037 | mlxsw_sp_port->ptp.hwtstamp_config = *config; | |
1038 | mlxsw_sp_port->ptp.ing_types = ing_types; | |
1039 | mlxsw_sp_port->ptp.egr_types = egr_types; | |
1040 | ||
eceed3b1 ST |
1041 | err = mlxsw_sp1_ptp_port_shaper_check(mlxsw_sp_port); |
1042 | if (err) | |
1043 | return err; | |
1044 | ||
87486427 PM |
1045 | /* Notify the ioctl caller what we are actually timestamping. */ |
1046 | config->rx_filter = rx_filter; | |
1047 | ||
1048 | return 0; | |
1049 | } | |
87ee07f8 PM |
1050 | |
1051 | int mlxsw_sp1_ptp_get_ts_info(struct mlxsw_sp *mlxsw_sp, | |
1052 | struct ethtool_ts_info *info) | |
1053 | { | |
1054 | info->phc_index = ptp_clock_index(mlxsw_sp->clock->ptp); | |
1055 | ||
1056 | info->so_timestamping = SOF_TIMESTAMPING_TX_HARDWARE | | |
1057 | SOF_TIMESTAMPING_RX_HARDWARE | | |
1058 | SOF_TIMESTAMPING_RAW_HARDWARE; | |
1059 | ||
1060 | info->tx_types = BIT(HWTSTAMP_TX_OFF) | | |
1061 | BIT(HWTSTAMP_TX_ON); | |
1062 | ||
1063 | info->rx_filters = BIT(HWTSTAMP_FILTER_NONE) | | |
1064 | BIT(HWTSTAMP_FILTER_ALL); | |
1065 | ||
1066 | return 0; | |
1067 | } |