1// SPDX-License-Identifier: BSD-3-Clause OR GPL-2.0 2/* Copyright (c) 2019 Mellanox Technologies. All rights reserved */ 3 4#include <linux/ptp_clock_kernel.h> 5#include <linux/clocksource.h> 6#include <linux/timecounter.h> 7#include <linux/spinlock.h> 8#include <linux/device.h> 9#include <linux/rhashtable.h> 10#include <linux/ptp_classify.h> 11#include <linux/if_ether.h> 12#include <linux/if_vlan.h> 13#include <linux/net_tstamp.h> 14#include <linux/refcount.h> 15 16#include "spectrum.h" 17#include "spectrum_ptp.h" 18#include "core.h" 19 20#define MLXSW_SP1_PTP_CLOCK_CYCLES_SHIFT 29 21#define MLXSW_SP1_PTP_CLOCK_FREQ_KHZ 156257 /* 6.4nSec */ 22#define MLXSW_SP1_PTP_CLOCK_MASK 64 23 24#define MLXSW_SP1_PTP_HT_GC_INTERVAL 500 /* ms */ 25 26/* How long, approximately, should the unmatched entries stay in the hash table 27 * before they are collected. Should be evenly divisible by the GC interval. 28 */ 29#define MLXSW_SP1_PTP_HT_GC_TIMEOUT 1000 /* ms */ 30 31struct mlxsw_sp_ptp_state { 32 struct mlxsw_sp *mlxsw_sp; 33}; 34 35struct mlxsw_sp1_ptp_state { 36 struct mlxsw_sp_ptp_state common; 37 struct rhltable unmatched_ht; 38 spinlock_t unmatched_lock; /* protects the HT */ 39 struct delayed_work ht_gc_dw; 40 u32 gc_cycle; 41}; 42 43struct mlxsw_sp2_ptp_state { 44 struct mlxsw_sp_ptp_state common; 45 refcount_t ptp_port_enabled_ref; /* Number of ports with time stamping 46 * enabled. 47 */ 48 struct hwtstamp_config config; 49 struct mutex lock; /* Protects 'config' and HW configuration. */ 50}; 51 52struct mlxsw_sp1_ptp_key { 53 u16 local_port; 54 u8 message_type; 55 u16 sequence_id; 56 u8 domain_number; 57 bool ingress; 58}; 59 60struct mlxsw_sp1_ptp_unmatched { 61 struct mlxsw_sp1_ptp_key key; 62 struct rhlist_head ht_node; 63 struct rcu_head rcu; 64 struct sk_buff *skb; 65 u64 timestamp; 66 u32 gc_cycle; 67}; 68 69static const struct rhashtable_params mlxsw_sp1_ptp_unmatched_ht_params = { 70 .key_len = sizeof_field(struct mlxsw_sp1_ptp_unmatched, key), 71 .key_offset = offsetof(struct mlxsw_sp1_ptp_unmatched, key), 72 .head_offset = offsetof(struct mlxsw_sp1_ptp_unmatched, ht_node), 73}; 74 75struct mlxsw_sp_ptp_clock { 76 struct mlxsw_core *core; 77 struct ptp_clock *ptp; 78 struct ptp_clock_info ptp_info; 79}; 80 81struct mlxsw_sp1_ptp_clock { 82 struct mlxsw_sp_ptp_clock common; 83 spinlock_t lock; /* protect this structure */ 84 struct cyclecounter cycles; 85 struct timecounter tc; 86 u32 nominal_c_mult; 87 unsigned long overflow_period; 88 struct delayed_work overflow_work; 89}; 90 91static struct mlxsw_sp1_ptp_state * 92mlxsw_sp1_ptp_state(struct mlxsw_sp *mlxsw_sp) 93{ 94 return container_of(mlxsw_sp->ptp_state, struct mlxsw_sp1_ptp_state, 95 common); 96} 97 98static struct mlxsw_sp2_ptp_state * 99mlxsw_sp2_ptp_state(struct mlxsw_sp *mlxsw_sp) 100{ 101 return container_of(mlxsw_sp->ptp_state, struct mlxsw_sp2_ptp_state, 102 common); 103} 104 105static struct mlxsw_sp1_ptp_clock * 106mlxsw_sp1_ptp_clock(struct ptp_clock_info *ptp) 107{ 108 return container_of(ptp, struct mlxsw_sp1_ptp_clock, common.ptp_info); 109} 110 111static u64 __mlxsw_sp1_ptp_read_frc(struct mlxsw_sp1_ptp_clock *clock, 112 struct ptp_system_timestamp *sts) 113{ 114 struct mlxsw_core *mlxsw_core = clock->common.core; 115 u32 frc_h1, frc_h2, frc_l; 116 117 frc_h1 = mlxsw_core_read_frc_h(mlxsw_core); 118 ptp_read_system_prets(sts); 119 frc_l = mlxsw_core_read_frc_l(mlxsw_core); 120 ptp_read_system_postts(sts); 121 frc_h2 = mlxsw_core_read_frc_h(mlxsw_core); 122 123 if (frc_h1 != frc_h2) { 124 /* wrap around */ 125 ptp_read_system_prets(sts); 126 frc_l = mlxsw_core_read_frc_l(mlxsw_core); 127 ptp_read_system_postts(sts); 128 } 129 130 return (u64) frc_l | (u64) frc_h2 << 32; 131} 132 133static u64 mlxsw_sp1_ptp_read_frc(const struct cyclecounter *cc) 134{ 135 struct mlxsw_sp1_ptp_clock *clock = 136 container_of(cc, struct mlxsw_sp1_ptp_clock, cycles); 137 138 return __mlxsw_sp1_ptp_read_frc(clock, NULL) & cc->mask; 139} 140 141static int 142mlxsw_sp_ptp_phc_adjfreq(struct mlxsw_sp_ptp_clock *clock, int freq_adj) 143{ 144 struct mlxsw_core *mlxsw_core = clock->core; 145 char mtutc_pl[MLXSW_REG_MTUTC_LEN]; 146 147 mlxsw_reg_mtutc_pack(mtutc_pl, MLXSW_REG_MTUTC_OPERATION_ADJUST_FREQ, 148 freq_adj, 0, 0, 0); 149 return mlxsw_reg_write(mlxsw_core, MLXSW_REG(mtutc), mtutc_pl); 150} 151 152static u64 mlxsw_sp1_ptp_ns2cycles(const struct timecounter *tc, u64 nsec) 153{ 154 u64 cycles = (u64) nsec; 155 156 cycles <<= tc->cc->shift; 157 cycles = div_u64(cycles, tc->cc->mult); 158 159 return cycles; 160} 161 162static int 163mlxsw_sp1_ptp_phc_settime(struct mlxsw_sp1_ptp_clock *clock, u64 nsec) 164{ 165 struct mlxsw_core *mlxsw_core = clock->common.core; 166 u64 next_sec, next_sec_in_nsec, cycles; 167 char mtutc_pl[MLXSW_REG_MTUTC_LEN]; 168 char mtpps_pl[MLXSW_REG_MTPPS_LEN]; 169 int err; 170 171 next_sec = div_u64(nsec, NSEC_PER_SEC) + 1; 172 next_sec_in_nsec = next_sec * NSEC_PER_SEC; 173 174 spin_lock_bh(&clock->lock); 175 cycles = mlxsw_sp1_ptp_ns2cycles(&clock->tc, next_sec_in_nsec); 176 spin_unlock_bh(&clock->lock); 177 178 mlxsw_reg_mtpps_vpin_pack(mtpps_pl, cycles); 179 err = mlxsw_reg_write(mlxsw_core, MLXSW_REG(mtpps), mtpps_pl); 180 if (err) 181 return err; 182 183 mlxsw_reg_mtutc_pack(mtutc_pl, 184 MLXSW_REG_MTUTC_OPERATION_SET_TIME_AT_NEXT_SEC, 185 0, next_sec, 0, 0); 186 return mlxsw_reg_write(mlxsw_core, MLXSW_REG(mtutc), mtutc_pl); 187} 188 189static int mlxsw_sp1_ptp_adjfine(struct ptp_clock_info *ptp, long scaled_ppm) 190{ 191 struct mlxsw_sp1_ptp_clock *clock = mlxsw_sp1_ptp_clock(ptp); 192 s32 ppb; 193 194 ppb = scaled_ppm_to_ppb(scaled_ppm); 195 196 spin_lock_bh(&clock->lock); 197 timecounter_read(&clock->tc); 198 clock->cycles.mult = adjust_by_scaled_ppm(clock->nominal_c_mult, 199 scaled_ppm); 200 spin_unlock_bh(&clock->lock); 201 202 return mlxsw_sp_ptp_phc_adjfreq(&clock->common, ppb); 203} 204 205static int mlxsw_sp1_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta) 206{ 207 struct mlxsw_sp1_ptp_clock *clock = mlxsw_sp1_ptp_clock(ptp); 208 u64 nsec; 209 210 spin_lock_bh(&clock->lock); 211 timecounter_adjtime(&clock->tc, delta); 212 nsec = timecounter_read(&clock->tc); 213 spin_unlock_bh(&clock->lock); 214 215 return mlxsw_sp1_ptp_phc_settime(clock, nsec); 216} 217 218static int mlxsw_sp1_ptp_gettimex(struct ptp_clock_info *ptp, 219 struct timespec64 *ts, 220 struct ptp_system_timestamp *sts) 221{ 222 struct mlxsw_sp1_ptp_clock *clock = mlxsw_sp1_ptp_clock(ptp); 223 u64 cycles, nsec; 224 225 spin_lock_bh(&clock->lock); 226 cycles = __mlxsw_sp1_ptp_read_frc(clock, sts); 227 nsec = timecounter_cyc2time(&clock->tc, cycles); 228 spin_unlock_bh(&clock->lock); 229 230 *ts = ns_to_timespec64(nsec); 231 232 return 0; 233} 234 235static int mlxsw_sp1_ptp_settime(struct ptp_clock_info *ptp, 236 const struct timespec64 *ts) 237{ 238 struct mlxsw_sp1_ptp_clock *clock = mlxsw_sp1_ptp_clock(ptp); 239 u64 nsec = timespec64_to_ns(ts); 240 241 spin_lock_bh(&clock->lock); 242 timecounter_init(&clock->tc, &clock->cycles, nsec); 243 nsec = timecounter_read(&clock->tc); 244 spin_unlock_bh(&clock->lock); 245 246 return mlxsw_sp1_ptp_phc_settime(clock, nsec); 247} 248 249static const struct ptp_clock_info mlxsw_sp1_ptp_clock_info = { 250 .owner = THIS_MODULE, 251 .name = "mlxsw_sp_clock", 252 .max_adj = 100000000, 253 .adjfine = mlxsw_sp1_ptp_adjfine, 254 .adjtime = mlxsw_sp1_ptp_adjtime, 255 .gettimex64 = mlxsw_sp1_ptp_gettimex, 256 .settime64 = mlxsw_sp1_ptp_settime, 257}; 258 259static void mlxsw_sp1_ptp_clock_overflow(struct work_struct *work) 260{ 261 struct delayed_work *dwork = to_delayed_work(work); 262 struct mlxsw_sp1_ptp_clock *clock; 263 264 clock = container_of(dwork, struct mlxsw_sp1_ptp_clock, overflow_work); 265 266 spin_lock_bh(&clock->lock); 267 timecounter_read(&clock->tc); 268 spin_unlock_bh(&clock->lock); 269 mlxsw_core_schedule_dw(&clock->overflow_work, clock->overflow_period); 270} 271 272struct mlxsw_sp_ptp_clock * 273mlxsw_sp1_ptp_clock_init(struct mlxsw_sp *mlxsw_sp, struct device *dev) 274{ 275 u64 overflow_cycles, nsec, frac = 0; 276 struct mlxsw_sp1_ptp_clock *clock; 277 int err; 278 279 clock = kzalloc(sizeof(*clock), GFP_KERNEL); 280 if (!clock) 281 return ERR_PTR(-ENOMEM); 282 283 spin_lock_init(&clock->lock); 284 clock->cycles.read = mlxsw_sp1_ptp_read_frc; 285 clock->cycles.shift = MLXSW_SP1_PTP_CLOCK_CYCLES_SHIFT; 286 clock->cycles.mult = clocksource_khz2mult(MLXSW_SP1_PTP_CLOCK_FREQ_KHZ, 287 clock->cycles.shift); 288 clock->nominal_c_mult = clock->cycles.mult; 289 clock->cycles.mask = CLOCKSOURCE_MASK(MLXSW_SP1_PTP_CLOCK_MASK); 290 clock->common.core = mlxsw_sp->core; 291 292 timecounter_init(&clock->tc, &clock->cycles, 0); 293 294 /* Calculate period in seconds to call the overflow watchdog - to make 295 * sure counter is checked at least twice every wrap around. 296 * The period is calculated as the minimum between max HW cycles count 297 * (The clock source mask) and max amount of cycles that can be 298 * multiplied by clock multiplier where the result doesn't exceed 299 * 64bits. 300 */ 301 overflow_cycles = div64_u64(~0ULL >> 1, clock->cycles.mult); 302 overflow_cycles = min(overflow_cycles, div_u64(clock->cycles.mask, 3)); 303 304 nsec = cyclecounter_cyc2ns(&clock->cycles, overflow_cycles, 0, &frac); 305 clock->overflow_period = nsecs_to_jiffies(nsec); 306 307 INIT_DELAYED_WORK(&clock->overflow_work, mlxsw_sp1_ptp_clock_overflow); 308 mlxsw_core_schedule_dw(&clock->overflow_work, 0); 309 310 clock->common.ptp_info = mlxsw_sp1_ptp_clock_info; 311 clock->common.ptp = ptp_clock_register(&clock->common.ptp_info, dev); 312 if (IS_ERR(clock->common.ptp)) { 313 err = PTR_ERR(clock->common.ptp); 314 dev_err(dev, "ptp_clock_register failed %d\n", err); 315 goto err_ptp_clock_register; 316 } 317 318 return &clock->common; 319 320err_ptp_clock_register: 321 cancel_delayed_work_sync(&clock->overflow_work); 322 kfree(clock); 323 return ERR_PTR(err); 324} 325 326void mlxsw_sp1_ptp_clock_fini(struct mlxsw_sp_ptp_clock *clock_common) 327{ 328 struct mlxsw_sp1_ptp_clock *clock = 329 container_of(clock_common, struct mlxsw_sp1_ptp_clock, common); 330 331 ptp_clock_unregister(clock_common->ptp); 332 cancel_delayed_work_sync(&clock->overflow_work); 333 kfree(clock); 334} 335 336static u64 mlxsw_sp2_ptp_read_utc(struct mlxsw_sp_ptp_clock *clock, 337 struct ptp_system_timestamp *sts) 338{ 339 struct mlxsw_core *mlxsw_core = clock->core; 340 u32 utc_sec1, utc_sec2, utc_nsec; 341 342 utc_sec1 = mlxsw_core_read_utc_sec(mlxsw_core); 343 ptp_read_system_prets(sts); 344 utc_nsec = mlxsw_core_read_utc_nsec(mlxsw_core); 345 ptp_read_system_postts(sts); 346 utc_sec2 = mlxsw_core_read_utc_sec(mlxsw_core); 347 348 if (utc_sec1 != utc_sec2) { 349 /* Wrap around. */ 350 ptp_read_system_prets(sts); 351 utc_nsec = mlxsw_core_read_utc_nsec(mlxsw_core); 352 ptp_read_system_postts(sts); 353 } 354 355 return (u64)utc_sec2 * NSEC_PER_SEC + utc_nsec; 356} 357 358static int 359mlxsw_sp2_ptp_phc_settime(struct mlxsw_sp_ptp_clock *clock, u64 nsec) 360{ 361 struct mlxsw_core *mlxsw_core = clock->core; 362 char mtutc_pl[MLXSW_REG_MTUTC_LEN]; 363 u32 sec, nsec_rem; 364 365 sec = div_u64_rem(nsec, NSEC_PER_SEC, &nsec_rem); 366 mlxsw_reg_mtutc_pack(mtutc_pl, 367 MLXSW_REG_MTUTC_OPERATION_SET_TIME_IMMEDIATE, 368 0, sec, nsec_rem, 0); 369 return mlxsw_reg_write(mlxsw_core, MLXSW_REG(mtutc), mtutc_pl); 370} 371 372static int mlxsw_sp2_ptp_adjfine(struct ptp_clock_info *ptp, long scaled_ppm) 373{ 374 struct mlxsw_sp_ptp_clock *clock = 375 container_of(ptp, struct mlxsw_sp_ptp_clock, ptp_info); 376 s32 ppb = scaled_ppm_to_ppb(scaled_ppm); 377 378 /* In Spectrum-2 and newer ASICs, the frequency adjustment in MTUTC is 379 * reversed, positive values mean to decrease the frequency. Adjust the 380 * sign of PPB to this behavior. 381 */ 382 return mlxsw_sp_ptp_phc_adjfreq(clock, -ppb); 383} 384 385static int mlxsw_sp2_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta) 386{ 387 struct mlxsw_sp_ptp_clock *clock = 388 container_of(ptp, struct mlxsw_sp_ptp_clock, ptp_info); 389 struct mlxsw_core *mlxsw_core = clock->core; 390 char mtutc_pl[MLXSW_REG_MTUTC_LEN]; 391 392 /* HW time adjustment range is s16. If out of range, set time instead. */ 393 if (delta < S16_MIN || delta > S16_MAX) { 394 u64 nsec; 395 396 nsec = mlxsw_sp2_ptp_read_utc(clock, NULL); 397 nsec += delta; 398 399 return mlxsw_sp2_ptp_phc_settime(clock, nsec); 400 } 401 402 mlxsw_reg_mtutc_pack(mtutc_pl, 403 MLXSW_REG_MTUTC_OPERATION_ADJUST_TIME, 404 0, 0, 0, delta); 405 return mlxsw_reg_write(mlxsw_core, MLXSW_REG(mtutc), mtutc_pl); 406} 407 408static int mlxsw_sp2_ptp_gettimex(struct ptp_clock_info *ptp, 409 struct timespec64 *ts, 410 struct ptp_system_timestamp *sts) 411{ 412 struct mlxsw_sp_ptp_clock *clock = 413 container_of(ptp, struct mlxsw_sp_ptp_clock, ptp_info); 414 u64 nsec; 415 416 nsec = mlxsw_sp2_ptp_read_utc(clock, sts); 417 *ts = ns_to_timespec64(nsec); 418 419 return 0; 420} 421 422static int mlxsw_sp2_ptp_settime(struct ptp_clock_info *ptp, 423 const struct timespec64 *ts) 424{ 425 struct mlxsw_sp_ptp_clock *clock = 426 container_of(ptp, struct mlxsw_sp_ptp_clock, ptp_info); 427 u64 nsec = timespec64_to_ns(ts); 428 429 return mlxsw_sp2_ptp_phc_settime(clock, nsec); 430} 431 432static const struct ptp_clock_info mlxsw_sp2_ptp_clock_info = { 433 .owner = THIS_MODULE, 434 .name = "mlxsw_sp_clock", 435 .max_adj = MLXSW_REG_MTUTC_MAX_FREQ_ADJ, 436 .adjfine = mlxsw_sp2_ptp_adjfine, 437 .adjtime = mlxsw_sp2_ptp_adjtime, 438 .gettimex64 = mlxsw_sp2_ptp_gettimex, 439 .settime64 = mlxsw_sp2_ptp_settime, 440}; 441 442struct mlxsw_sp_ptp_clock * 443mlxsw_sp2_ptp_clock_init(struct mlxsw_sp *mlxsw_sp, struct device *dev) 444{ 445 struct mlxsw_sp_ptp_clock *clock; 446 int err; 447 448 clock = kzalloc(sizeof(*clock), GFP_KERNEL); 449 if (!clock) 450 return ERR_PTR(-ENOMEM); 451 452 clock->core = mlxsw_sp->core; 453 454 clock->ptp_info = mlxsw_sp2_ptp_clock_info; 455 456 err = mlxsw_sp2_ptp_phc_settime(clock, 0); 457 if (err) { 458 dev_err(dev, "setting UTC time failed %d\n", err); 459 goto err_ptp_phc_settime; 460 } 461 462 clock->ptp = ptp_clock_register(&clock->ptp_info, dev); 463 if (IS_ERR(clock->ptp)) { 464 err = PTR_ERR(clock->ptp); 465 dev_err(dev, "ptp_clock_register failed %d\n", err); 466 goto err_ptp_clock_register; 467 } 468 469 return clock; 470 471err_ptp_clock_register: 472err_ptp_phc_settime: 473 kfree(clock); 474 return ERR_PTR(err); 475} 476 477void mlxsw_sp2_ptp_clock_fini(struct mlxsw_sp_ptp_clock *clock) 478{ 479 ptp_clock_unregister(clock->ptp); 480 kfree(clock); 481} 482 483static int mlxsw_sp_ptp_parse(struct sk_buff *skb, 484 u8 *p_domain_number, 485 u8 *p_message_type, 486 u16 *p_sequence_id) 487{ 488 unsigned int ptp_class; 489 struct ptp_header *hdr; 490 491 ptp_class = ptp_classify_raw(skb); 492 493 switch (ptp_class & PTP_CLASS_VMASK) { 494 case PTP_CLASS_V1: 495 case PTP_CLASS_V2: 496 break; 497 default: 498 return -ERANGE; 499 } 500 501 hdr = ptp_parse_header(skb, ptp_class); 502 if (!hdr) 503 return -EINVAL; 504 505 *p_message_type = ptp_get_msgtype(hdr, ptp_class); 506 *p_domain_number = hdr->domain_number; 507 *p_sequence_id = be16_to_cpu(hdr->sequence_id); 508 509 return 0; 510} 511 512/* Returns NULL on successful insertion, a pointer on conflict, or an ERR_PTR on 513 * error. 514 */ 515static int 516mlxsw_sp1_ptp_unmatched_save(struct mlxsw_sp *mlxsw_sp, 517 struct mlxsw_sp1_ptp_key key, 518 struct sk_buff *skb, 519 u64 timestamp) 520{ 521 int cycles = MLXSW_SP1_PTP_HT_GC_TIMEOUT / MLXSW_SP1_PTP_HT_GC_INTERVAL; 522 struct mlxsw_sp1_ptp_state *ptp_state = mlxsw_sp1_ptp_state(mlxsw_sp); 523 struct mlxsw_sp1_ptp_unmatched *unmatched; 524 int err; 525 526 unmatched = kzalloc(sizeof(*unmatched), GFP_ATOMIC); 527 if (!unmatched) 528 return -ENOMEM; 529 530 unmatched->key = key; 531 unmatched->skb = skb; 532 unmatched->timestamp = timestamp; 533 unmatched->gc_cycle = ptp_state->gc_cycle + cycles; 534 535 err = rhltable_insert(&ptp_state->unmatched_ht, &unmatched->ht_node, 536 mlxsw_sp1_ptp_unmatched_ht_params); 537 if (err) 538 kfree(unmatched); 539 540 return err; 541} 542 543static struct mlxsw_sp1_ptp_unmatched * 544mlxsw_sp1_ptp_unmatched_lookup(struct mlxsw_sp *mlxsw_sp, 545 struct mlxsw_sp1_ptp_key key, int *p_length) 546{ 547 struct mlxsw_sp1_ptp_state *ptp_state = mlxsw_sp1_ptp_state(mlxsw_sp); 548 struct mlxsw_sp1_ptp_unmatched *unmatched, *last = NULL; 549 struct rhlist_head *tmp, *list; 550 int length = 0; 551 552 list = rhltable_lookup(&ptp_state->unmatched_ht, &key, 553 mlxsw_sp1_ptp_unmatched_ht_params); 554 rhl_for_each_entry_rcu(unmatched, tmp, list, ht_node) { 555 last = unmatched; 556 length++; 557 } 558 559 *p_length = length; 560 return last; 561} 562 563static int 564mlxsw_sp1_ptp_unmatched_remove(struct mlxsw_sp *mlxsw_sp, 565 struct mlxsw_sp1_ptp_unmatched *unmatched) 566{ 567 struct mlxsw_sp1_ptp_state *ptp_state = mlxsw_sp1_ptp_state(mlxsw_sp); 568 569 return rhltable_remove(&ptp_state->unmatched_ht, 570 &unmatched->ht_node, 571 mlxsw_sp1_ptp_unmatched_ht_params); 572} 573 574/* This function is called in the following scenarios: 575 * 576 * 1) When a packet is matched with its timestamp. 577 * 2) In several situation when it is necessary to immediately pass on 578 * an SKB without a timestamp. 579 * 3) From GC indirectly through mlxsw_sp1_ptp_unmatched_finish(). 580 * This case is similar to 2) above. 581 */ 582static void mlxsw_sp1_ptp_packet_finish(struct mlxsw_sp *mlxsw_sp, 583 struct sk_buff *skb, u16 local_port, 584 bool ingress, 585 struct skb_shared_hwtstamps *hwtstamps) 586{ 587 struct mlxsw_sp_port *mlxsw_sp_port; 588 589 /* Between capturing the packet and finishing it, there is a window of 590 * opportunity for the originating port to go away (e.g. due to a 591 * split). Also make sure the SKB device reference is still valid. 592 */ 593 mlxsw_sp_port = mlxsw_sp->ports[local_port]; 594 if (!(mlxsw_sp_port && (!skb->dev || skb->dev == mlxsw_sp_port->dev))) { 595 dev_kfree_skb_any(skb); 596 return; 597 } 598 599 if (ingress) { 600 if (hwtstamps) 601 *skb_hwtstamps(skb) = *hwtstamps; 602 mlxsw_sp_rx_listener_no_mark_func(skb, local_port, mlxsw_sp); 603 } else { 604 /* skb_tstamp_tx() allows hwtstamps to be NULL. */ 605 skb_tstamp_tx(skb, hwtstamps); 606 dev_kfree_skb_any(skb); 607 } 608} 609 610static void mlxsw_sp1_packet_timestamp(struct mlxsw_sp *mlxsw_sp, 611 struct mlxsw_sp1_ptp_key key, 612 struct sk_buff *skb, 613 u64 timestamp) 614{ 615 struct mlxsw_sp_ptp_clock *clock_common = mlxsw_sp->clock; 616 struct mlxsw_sp1_ptp_clock *clock = 617 container_of(clock_common, struct mlxsw_sp1_ptp_clock, common); 618 619 struct skb_shared_hwtstamps hwtstamps; 620 u64 nsec; 621 622 spin_lock_bh(&clock->lock); 623 nsec = timecounter_cyc2time(&clock->tc, timestamp); 624 spin_unlock_bh(&clock->lock); 625 626 hwtstamps.hwtstamp = ns_to_ktime(nsec); 627 mlxsw_sp1_ptp_packet_finish(mlxsw_sp, skb, 628 key.local_port, key.ingress, &hwtstamps); 629} 630 631static void 632mlxsw_sp1_ptp_unmatched_finish(struct mlxsw_sp *mlxsw_sp, 633 struct mlxsw_sp1_ptp_unmatched *unmatched) 634{ 635 if (unmatched->skb && unmatched->timestamp) 636 mlxsw_sp1_packet_timestamp(mlxsw_sp, unmatched->key, 637 unmatched->skb, 638 unmatched->timestamp); 639 else if (unmatched->skb) 640 mlxsw_sp1_ptp_packet_finish(mlxsw_sp, unmatched->skb, 641 unmatched->key.local_port, 642 unmatched->key.ingress, NULL); 643 kfree_rcu(unmatched, rcu); 644} 645 646static void mlxsw_sp1_ptp_unmatched_free_fn(void *ptr, void *arg) 647{ 648 struct mlxsw_sp1_ptp_unmatched *unmatched = ptr; 649 650 /* This is invoked at a point where the ports are gone already. Nothing 651 * to do with whatever is left in the HT but to free it. 652 */ 653 if (unmatched->skb) 654 dev_kfree_skb_any(unmatched->skb); 655 kfree_rcu(unmatched, rcu); 656} 657 658static void mlxsw_sp1_ptp_got_piece(struct mlxsw_sp *mlxsw_sp, 659 struct mlxsw_sp1_ptp_key key, 660 struct sk_buff *skb, u64 timestamp) 661{ 662 struct mlxsw_sp1_ptp_state *ptp_state = mlxsw_sp1_ptp_state(mlxsw_sp); 663 struct mlxsw_sp1_ptp_unmatched *unmatched; 664 int length; 665 int err; 666 667 rcu_read_lock(); 668 669 spin_lock(&ptp_state->unmatched_lock); 670 671 unmatched = mlxsw_sp1_ptp_unmatched_lookup(mlxsw_sp, key, &length); 672 if (skb && unmatched && unmatched->timestamp) { 673 unmatched->skb = skb; 674 } else if (timestamp && unmatched && unmatched->skb) { 675 unmatched->timestamp = timestamp; 676 } else { 677 /* Either there is no entry to match, or one that is there is 678 * incompatible. 679 */ 680 if (length < 100) 681 err = mlxsw_sp1_ptp_unmatched_save(mlxsw_sp, key, 682 skb, timestamp); 683 else 684 err = -E2BIG; 685 if (err && skb) 686 mlxsw_sp1_ptp_packet_finish(mlxsw_sp, skb, 687 key.local_port, 688 key.ingress, NULL); 689 unmatched = NULL; 690 } 691 692 if (unmatched) { 693 err = mlxsw_sp1_ptp_unmatched_remove(mlxsw_sp, unmatched); 694 WARN_ON_ONCE(err); 695 } 696 697 spin_unlock(&ptp_state->unmatched_lock); 698 699 if (unmatched) 700 mlxsw_sp1_ptp_unmatched_finish(mlxsw_sp, unmatched); 701 702 rcu_read_unlock(); 703} 704 705static void mlxsw_sp1_ptp_got_packet(struct mlxsw_sp *mlxsw_sp, 706 struct sk_buff *skb, u16 local_port, 707 bool ingress) 708{ 709 struct mlxsw_sp_port *mlxsw_sp_port; 710 struct mlxsw_sp1_ptp_key key; 711 u8 types; 712 int err; 713 714 mlxsw_sp_port = mlxsw_sp->ports[local_port]; 715 if (!mlxsw_sp_port) 716 goto immediate; 717 718 types = ingress ? mlxsw_sp_port->ptp.ing_types : 719 mlxsw_sp_port->ptp.egr_types; 720 if (!types) 721 goto immediate; 722 723 memset(&key, 0, sizeof(key)); 724 key.local_port = local_port; 725 key.ingress = ingress; 726 727 err = mlxsw_sp_ptp_parse(skb, &key.domain_number, &key.message_type, 728 &key.sequence_id); 729 if (err) 730 goto immediate; 731 732 /* For packets whose timestamping was not enabled on this port, don't 733 * bother trying to match the timestamp. 734 */ 735 if (!((1 << key.message_type) & types)) 736 goto immediate; 737 738 mlxsw_sp1_ptp_got_piece(mlxsw_sp, key, skb, 0); 739 return; 740 741immediate: 742 mlxsw_sp1_ptp_packet_finish(mlxsw_sp, skb, local_port, ingress, NULL); 743} 744 745void mlxsw_sp1_ptp_got_timestamp(struct mlxsw_sp *mlxsw_sp, bool ingress, 746 u16 local_port, u8 message_type, 747 u8 domain_number, u16 sequence_id, 748 u64 timestamp) 749{ 750 struct mlxsw_sp_port *mlxsw_sp_port; 751 struct mlxsw_sp1_ptp_key key; 752 u8 types; 753 754 if (WARN_ON_ONCE(!mlxsw_sp_local_port_is_valid(mlxsw_sp, local_port))) 755 return; 756 mlxsw_sp_port = mlxsw_sp->ports[local_port]; 757 if (!mlxsw_sp_port) 758 return; 759 760 types = ingress ? mlxsw_sp_port->ptp.ing_types : 761 mlxsw_sp_port->ptp.egr_types; 762 763 /* For message types whose timestamping was not enabled on this port, 764 * don't bother with the timestamp. 765 */ 766 if (!((1 << message_type) & types)) 767 return; 768 769 memset(&key, 0, sizeof(key)); 770 key.local_port = local_port; 771 key.domain_number = domain_number; 772 key.message_type = message_type; 773 key.sequence_id = sequence_id; 774 key.ingress = ingress; 775 776 mlxsw_sp1_ptp_got_piece(mlxsw_sp, key, NULL, timestamp); 777} 778 779void mlxsw_sp1_ptp_receive(struct mlxsw_sp *mlxsw_sp, struct sk_buff *skb, 780 u16 local_port) 781{ 782 skb_reset_mac_header(skb); 783 mlxsw_sp1_ptp_got_packet(mlxsw_sp, skb, local_port, true); 784} 785 786void mlxsw_sp1_ptp_transmitted(struct mlxsw_sp *mlxsw_sp, 787 struct sk_buff *skb, u16 local_port) 788{ 789 mlxsw_sp1_ptp_got_packet(mlxsw_sp, skb, local_port, false); 790} 791 792static void 793mlxsw_sp1_ptp_ht_gc_collect(struct mlxsw_sp1_ptp_state *ptp_state, 794 struct mlxsw_sp1_ptp_unmatched *unmatched) 795{ 796 struct mlxsw_sp *mlxsw_sp = ptp_state->common.mlxsw_sp; 797 struct mlxsw_sp_ptp_port_dir_stats *stats; 798 struct mlxsw_sp_port *mlxsw_sp_port; 799 int err; 800 801 /* If an unmatched entry has an SKB, it has to be handed over to the 802 * networking stack. This is usually done from a trap handler, which is 803 * invoked in a softirq context. Here we are going to do it in process 804 * context. If that were to be interrupted by a softirq, it could cause 805 * a deadlock when an attempt is made to take an already-taken lock 806 * somewhere along the sending path. Disable softirqs to prevent this. 807 */ 808 local_bh_disable(); 809 810 spin_lock(&ptp_state->unmatched_lock); 811 err = rhltable_remove(&ptp_state->unmatched_ht, &unmatched->ht_node, 812 mlxsw_sp1_ptp_unmatched_ht_params); 813 spin_unlock(&ptp_state->unmatched_lock); 814 815 if (err) 816 /* The packet was matched with timestamp during the walk. */ 817 goto out; 818 819 mlxsw_sp_port = mlxsw_sp->ports[unmatched->key.local_port]; 820 if (mlxsw_sp_port) { 821 stats = unmatched->key.ingress ? 822 &mlxsw_sp_port->ptp.stats.rx_gcd : 823 &mlxsw_sp_port->ptp.stats.tx_gcd; 824 if (unmatched->skb) 825 stats->packets++; 826 else 827 stats->timestamps++; 828 } 829 830 /* mlxsw_sp1_ptp_unmatched_finish() invokes netif_receive_skb(). While 831 * the comment at that function states that it can only be called in 832 * soft IRQ context, this pattern of local_bh_disable() + 833 * netif_receive_skb(), in process context, is seen elsewhere in the 834 * kernel, notably in pktgen. 835 */ 836 mlxsw_sp1_ptp_unmatched_finish(mlxsw_sp, unmatched); 837 838out: 839 local_bh_enable(); 840} 841 842static void mlxsw_sp1_ptp_ht_gc(struct work_struct *work) 843{ 844 struct delayed_work *dwork = to_delayed_work(work); 845 struct mlxsw_sp1_ptp_unmatched *unmatched; 846 struct mlxsw_sp1_ptp_state *ptp_state; 847 struct rhashtable_iter iter; 848 u32 gc_cycle; 849 void *obj; 850 851 ptp_state = container_of(dwork, struct mlxsw_sp1_ptp_state, ht_gc_dw); 852 gc_cycle = ptp_state->gc_cycle++; 853 854 rhltable_walk_enter(&ptp_state->unmatched_ht, &iter); 855 rhashtable_walk_start(&iter); 856 while ((obj = rhashtable_walk_next(&iter))) { 857 if (IS_ERR(obj)) 858 continue; 859 860 unmatched = obj; 861 if (unmatched->gc_cycle <= gc_cycle) 862 mlxsw_sp1_ptp_ht_gc_collect(ptp_state, unmatched); 863 } 864 rhashtable_walk_stop(&iter); 865 rhashtable_walk_exit(&iter); 866 867 mlxsw_core_schedule_dw(&ptp_state->ht_gc_dw, 868 MLXSW_SP1_PTP_HT_GC_INTERVAL); 869} 870 871static int mlxsw_sp_ptp_mtptpt_set(struct mlxsw_sp *mlxsw_sp, 872 enum mlxsw_reg_mtptpt_trap_id trap_id, 873 u16 message_type) 874{ 875 char mtptpt_pl[MLXSW_REG_MTPTPT_LEN]; 876 877 mlxsw_reg_mtptpt_pack(mtptpt_pl, trap_id, message_type); 878 return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(mtptpt), mtptpt_pl); 879} 880 881static int mlxsw_sp1_ptp_set_fifo_clr_on_trap(struct mlxsw_sp *mlxsw_sp, 882 bool clr) 883{ 884 char mogcr_pl[MLXSW_REG_MOGCR_LEN] = {0}; 885 int err; 886 887 err = mlxsw_reg_query(mlxsw_sp->core, MLXSW_REG(mogcr), mogcr_pl); 888 if (err) 889 return err; 890 891 mlxsw_reg_mogcr_ptp_iftc_set(mogcr_pl, clr); 892 mlxsw_reg_mogcr_ptp_eftc_set(mogcr_pl, clr); 893 return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(mogcr), mogcr_pl); 894} 895 896static int mlxsw_sp1_ptp_mtpppc_set(struct mlxsw_sp *mlxsw_sp, 897 u16 ing_types, u16 egr_types) 898{ 899 char mtpppc_pl[MLXSW_REG_MTPPPC_LEN]; 900 901 mlxsw_reg_mtpppc_pack(mtpppc_pl, ing_types, egr_types); 902 return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(mtpppc), mtpppc_pl); 903} 904 905struct mlxsw_sp1_ptp_shaper_params { 906 u32 ethtool_speed; 907 enum mlxsw_reg_qpsc_port_speed port_speed; 908 u8 shaper_time_exp; 909 u8 shaper_time_mantissa; 910 u8 shaper_inc; 911 u8 shaper_bs; 912 u8 port_to_shaper_credits; 913 int ing_timestamp_inc; 914 int egr_timestamp_inc; 915}; 916 917static const struct mlxsw_sp1_ptp_shaper_params 918mlxsw_sp1_ptp_shaper_params[] = { 919 { 920 .ethtool_speed = SPEED_100, 921 .port_speed = MLXSW_REG_QPSC_PORT_SPEED_100M, 922 .shaper_time_exp = 4, 923 .shaper_time_mantissa = 12, 924 .shaper_inc = 9, 925 .shaper_bs = 1, 926 .port_to_shaper_credits = 1, 927 .ing_timestamp_inc = -313, 928 .egr_timestamp_inc = 313, 929 }, 930 { 931 .ethtool_speed = SPEED_1000, 932 .port_speed = MLXSW_REG_QPSC_PORT_SPEED_1G, 933 .shaper_time_exp = 0, 934 .shaper_time_mantissa = 12, 935 .shaper_inc = 6, 936 .shaper_bs = 0, 937 .port_to_shaper_credits = 1, 938 .ing_timestamp_inc = -35, 939 .egr_timestamp_inc = 35, 940 }, 941 { 942 .ethtool_speed = SPEED_10000, 943 .port_speed = MLXSW_REG_QPSC_PORT_SPEED_10G, 944 .shaper_time_exp = 0, 945 .shaper_time_mantissa = 2, 946 .shaper_inc = 14, 947 .shaper_bs = 1, 948 .port_to_shaper_credits = 1, 949 .ing_timestamp_inc = -11, 950 .egr_timestamp_inc = 11, 951 }, 952 { 953 .ethtool_speed = SPEED_25000, 954 .port_speed = MLXSW_REG_QPSC_PORT_SPEED_25G, 955 .shaper_time_exp = 0, 956 .shaper_time_mantissa = 0, 957 .shaper_inc = 11, 958 .shaper_bs = 1, 959 .port_to_shaper_credits = 1, 960 .ing_timestamp_inc = -14, 961 .egr_timestamp_inc = 14, 962 }, 963}; 964 965#define MLXSW_SP1_PTP_SHAPER_PARAMS_LEN ARRAY_SIZE(mlxsw_sp1_ptp_shaper_params) 966 967static int mlxsw_sp1_ptp_shaper_params_set(struct mlxsw_sp *mlxsw_sp) 968{ 969 const struct mlxsw_sp1_ptp_shaper_params *params; 970 char qpsc_pl[MLXSW_REG_QPSC_LEN]; 971 int i, err; 972 973 for (i = 0; i < MLXSW_SP1_PTP_SHAPER_PARAMS_LEN; i++) { 974 params = &mlxsw_sp1_ptp_shaper_params[i]; 975 mlxsw_reg_qpsc_pack(qpsc_pl, params->port_speed, 976 params->shaper_time_exp, 977 params->shaper_time_mantissa, 978 params->shaper_inc, params->shaper_bs, 979 params->port_to_shaper_credits, 980 params->ing_timestamp_inc, 981 params->egr_timestamp_inc); 982 err = mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(qpsc), qpsc_pl); 983 if (err) 984 return err; 985 } 986 987 return 0; 988} 989 990static int mlxsw_sp_ptp_traps_set(struct mlxsw_sp *mlxsw_sp) 991{ 992 u16 event_message_type; 993 int err; 994 995 /* Deliver these message types as PTP0. */ 996 event_message_type = BIT(PTP_MSGTYPE_SYNC) | 997 BIT(PTP_MSGTYPE_DELAY_REQ) | 998 BIT(PTP_MSGTYPE_PDELAY_REQ) | 999 BIT(PTP_MSGTYPE_PDELAY_RESP); 1000 1001 err = mlxsw_sp_ptp_mtptpt_set(mlxsw_sp, MLXSW_REG_MTPTPT_TRAP_ID_PTP0, 1002 event_message_type); 1003 if (err) 1004 return err; 1005 1006 /* Everything else is PTP1. */ 1007 err = mlxsw_sp_ptp_mtptpt_set(mlxsw_sp, MLXSW_REG_MTPTPT_TRAP_ID_PTP1, 1008 ~event_message_type); 1009 if (err) 1010 goto err_mtptpt1_set; 1011 1012 return 0; 1013 1014err_mtptpt1_set: 1015 mlxsw_sp_ptp_mtptpt_set(mlxsw_sp, MLXSW_REG_MTPTPT_TRAP_ID_PTP0, 0); 1016 return err; 1017} 1018 1019static void mlxsw_sp_ptp_traps_unset(struct mlxsw_sp *mlxsw_sp) 1020{ 1021 mlxsw_sp_ptp_mtptpt_set(mlxsw_sp, MLXSW_REG_MTPTPT_TRAP_ID_PTP1, 0); 1022 mlxsw_sp_ptp_mtptpt_set(mlxsw_sp, MLXSW_REG_MTPTPT_TRAP_ID_PTP0, 0); 1023} 1024 1025struct mlxsw_sp_ptp_state *mlxsw_sp1_ptp_init(struct mlxsw_sp *mlxsw_sp) 1026{ 1027 struct mlxsw_sp1_ptp_state *ptp_state; 1028 int err; 1029 1030 err = mlxsw_sp1_ptp_shaper_params_set(mlxsw_sp); 1031 if (err) 1032 return ERR_PTR(err); 1033 1034 ptp_state = kzalloc(sizeof(*ptp_state), GFP_KERNEL); 1035 if (!ptp_state) 1036 return ERR_PTR(-ENOMEM); 1037 ptp_state->common.mlxsw_sp = mlxsw_sp; 1038 1039 spin_lock_init(&ptp_state->unmatched_lock); 1040 1041 err = rhltable_init(&ptp_state->unmatched_ht, 1042 &mlxsw_sp1_ptp_unmatched_ht_params); 1043 if (err) 1044 goto err_hashtable_init; 1045 1046 err = mlxsw_sp_ptp_traps_set(mlxsw_sp); 1047 if (err) 1048 goto err_ptp_traps_set; 1049 1050 err = mlxsw_sp1_ptp_set_fifo_clr_on_trap(mlxsw_sp, true); 1051 if (err) 1052 goto err_fifo_clr; 1053 1054 INIT_DELAYED_WORK(&ptp_state->ht_gc_dw, mlxsw_sp1_ptp_ht_gc); 1055 mlxsw_core_schedule_dw(&ptp_state->ht_gc_dw, 1056 MLXSW_SP1_PTP_HT_GC_INTERVAL); 1057 return &ptp_state->common; 1058 1059err_fifo_clr: 1060 mlxsw_sp_ptp_traps_unset(mlxsw_sp); 1061err_ptp_traps_set: 1062 rhltable_destroy(&ptp_state->unmatched_ht); 1063err_hashtable_init: 1064 kfree(ptp_state); 1065 return ERR_PTR(err); 1066} 1067 1068void mlxsw_sp1_ptp_fini(struct mlxsw_sp_ptp_state *ptp_state_common) 1069{ 1070 struct mlxsw_sp *mlxsw_sp = ptp_state_common->mlxsw_sp; 1071 struct mlxsw_sp1_ptp_state *ptp_state; 1072 1073 ptp_state = mlxsw_sp1_ptp_state(mlxsw_sp); 1074 1075 cancel_delayed_work_sync(&ptp_state->ht_gc_dw); 1076 mlxsw_sp1_ptp_mtpppc_set(mlxsw_sp, 0, 0); 1077 mlxsw_sp1_ptp_set_fifo_clr_on_trap(mlxsw_sp, false); 1078 mlxsw_sp_ptp_traps_unset(mlxsw_sp); 1079 rhltable_free_and_destroy(&ptp_state->unmatched_ht, 1080 &mlxsw_sp1_ptp_unmatched_free_fn, NULL); 1081 kfree(ptp_state); 1082} 1083 1084int mlxsw_sp1_ptp_hwtstamp_get(struct mlxsw_sp_port *mlxsw_sp_port, 1085 struct hwtstamp_config *config) 1086{ 1087 *config = mlxsw_sp_port->ptp.hwtstamp_config; 1088 return 0; 1089} 1090 1091static int 1092mlxsw_sp1_ptp_get_message_types(const struct hwtstamp_config *config, 1093 u16 *p_ing_types, u16 *p_egr_types, 1094 enum hwtstamp_rx_filters *p_rx_filter) 1095{ 1096 enum hwtstamp_rx_filters rx_filter = config->rx_filter; 1097 enum hwtstamp_tx_types tx_type = config->tx_type; 1098 u16 ing_types = 0x00; 1099 u16 egr_types = 0x00; 1100 1101 switch (tx_type) { 1102 case HWTSTAMP_TX_OFF: 1103 egr_types = 0x00; 1104 break; 1105 case HWTSTAMP_TX_ON: 1106 egr_types = 0xff; 1107 break; 1108 case HWTSTAMP_TX_ONESTEP_SYNC: 1109 case HWTSTAMP_TX_ONESTEP_P2P: 1110 return -ERANGE; 1111 default: 1112 return -EINVAL; 1113 } 1114 1115 switch (rx_filter) { 1116 case HWTSTAMP_FILTER_NONE: 1117 ing_types = 0x00; 1118 break; 1119 case HWTSTAMP_FILTER_PTP_V1_L4_SYNC: 1120 case HWTSTAMP_FILTER_PTP_V2_L4_SYNC: 1121 case HWTSTAMP_FILTER_PTP_V2_L2_SYNC: 1122 case HWTSTAMP_FILTER_PTP_V2_SYNC: 1123 ing_types = 0x01; 1124 break; 1125 case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ: 1126 case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ: 1127 case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ: 1128 case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ: 1129 ing_types = 0x02; 1130 break; 1131 case HWTSTAMP_FILTER_PTP_V1_L4_EVENT: 1132 case HWTSTAMP_FILTER_PTP_V2_L4_EVENT: 1133 case HWTSTAMP_FILTER_PTP_V2_L2_EVENT: 1134 case HWTSTAMP_FILTER_PTP_V2_EVENT: 1135 ing_types = 0x0f; 1136 break; 1137 case HWTSTAMP_FILTER_ALL: 1138 ing_types = 0xff; 1139 break; 1140 case HWTSTAMP_FILTER_SOME: 1141 case HWTSTAMP_FILTER_NTP_ALL: 1142 return -ERANGE; 1143 default: 1144 return -EINVAL; 1145 } 1146 1147 *p_ing_types = ing_types; 1148 *p_egr_types = egr_types; 1149 *p_rx_filter = rx_filter; 1150 return 0; 1151} 1152 1153static int mlxsw_sp1_ptp_mtpppc_update(struct mlxsw_sp_port *mlxsw_sp_port, 1154 u16 ing_types, u16 egr_types) 1155{ 1156 struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp; 1157 struct mlxsw_sp_port *tmp; 1158 u16 orig_ing_types = 0; 1159 u16 orig_egr_types = 0; 1160 int err; 1161 int i; 1162 1163 /* MTPPPC configures timestamping globally, not per port. Find the 1164 * configuration that contains all configured timestamping requests. 1165 */ 1166 for (i = 1; i < mlxsw_core_max_ports(mlxsw_sp->core); i++) { 1167 tmp = mlxsw_sp->ports[i]; 1168 if (tmp) { 1169 orig_ing_types |= tmp->ptp.ing_types; 1170 orig_egr_types |= tmp->ptp.egr_types; 1171 } 1172 if (tmp && tmp != mlxsw_sp_port) { 1173 ing_types |= tmp->ptp.ing_types; 1174 egr_types |= tmp->ptp.egr_types; 1175 } 1176 } 1177 1178 if ((ing_types || egr_types) && !(orig_ing_types || orig_egr_types)) { 1179 err = mlxsw_sp_parsing_depth_inc(mlxsw_sp); 1180 if (err) { 1181 netdev_err(mlxsw_sp_port->dev, "Failed to increase parsing depth"); 1182 return err; 1183 } 1184 } 1185 if (!(ing_types || egr_types) && (orig_ing_types || orig_egr_types)) 1186 mlxsw_sp_parsing_depth_dec(mlxsw_sp); 1187 1188 return mlxsw_sp1_ptp_mtpppc_set(mlxsw_sp_port->mlxsw_sp, 1189 ing_types, egr_types); 1190} 1191 1192static bool mlxsw_sp1_ptp_hwtstamp_enabled(struct mlxsw_sp_port *mlxsw_sp_port) 1193{ 1194 return mlxsw_sp_port->ptp.ing_types || mlxsw_sp_port->ptp.egr_types; 1195} 1196 1197static int 1198mlxsw_sp1_ptp_port_shaper_set(struct mlxsw_sp_port *mlxsw_sp_port, bool enable) 1199{ 1200 struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp; 1201 char qeec_pl[MLXSW_REG_QEEC_LEN]; 1202 1203 mlxsw_reg_qeec_ptps_pack(qeec_pl, mlxsw_sp_port->local_port, enable); 1204 return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(qeec), qeec_pl); 1205} 1206 1207static int mlxsw_sp1_ptp_port_shaper_check(struct mlxsw_sp_port *mlxsw_sp_port) 1208{ 1209 bool ptps = false; 1210 int err, i; 1211 u32 speed; 1212 1213 if (!mlxsw_sp1_ptp_hwtstamp_enabled(mlxsw_sp_port)) 1214 return mlxsw_sp1_ptp_port_shaper_set(mlxsw_sp_port, false); 1215 1216 err = mlxsw_sp_port_speed_get(mlxsw_sp_port, &speed); 1217 if (err) 1218 return err; 1219 1220 for (i = 0; i < MLXSW_SP1_PTP_SHAPER_PARAMS_LEN; i++) { 1221 if (mlxsw_sp1_ptp_shaper_params[i].ethtool_speed == speed) { 1222 ptps = true; 1223 break; 1224 } 1225 } 1226 1227 return mlxsw_sp1_ptp_port_shaper_set(mlxsw_sp_port, ptps); 1228} 1229 1230void mlxsw_sp1_ptp_shaper_work(struct work_struct *work) 1231{ 1232 struct delayed_work *dwork = to_delayed_work(work); 1233 struct mlxsw_sp_port *mlxsw_sp_port; 1234 int err; 1235 1236 mlxsw_sp_port = container_of(dwork, struct mlxsw_sp_port, 1237 ptp.shaper_dw); 1238 1239 if (!mlxsw_sp1_ptp_hwtstamp_enabled(mlxsw_sp_port)) 1240 return; 1241 1242 err = mlxsw_sp1_ptp_port_shaper_check(mlxsw_sp_port); 1243 if (err) 1244 netdev_err(mlxsw_sp_port->dev, "Failed to set up PTP shaper\n"); 1245} 1246 1247int mlxsw_sp1_ptp_hwtstamp_set(struct mlxsw_sp_port *mlxsw_sp_port, 1248 struct hwtstamp_config *config) 1249{ 1250 enum hwtstamp_rx_filters rx_filter; 1251 u16 ing_types; 1252 u16 egr_types; 1253 int err; 1254 1255 err = mlxsw_sp1_ptp_get_message_types(config, &ing_types, &egr_types, 1256 &rx_filter); 1257 if (err) 1258 return err; 1259 1260 err = mlxsw_sp1_ptp_mtpppc_update(mlxsw_sp_port, ing_types, egr_types); 1261 if (err) 1262 return err; 1263 1264 mlxsw_sp_port->ptp.hwtstamp_config = *config; 1265 mlxsw_sp_port->ptp.ing_types = ing_types; 1266 mlxsw_sp_port->ptp.egr_types = egr_types; 1267 1268 err = mlxsw_sp1_ptp_port_shaper_check(mlxsw_sp_port); 1269 if (err) 1270 return err; 1271 1272 /* Notify the ioctl caller what we are actually timestamping. */ 1273 config->rx_filter = rx_filter; 1274 1275 return 0; 1276} 1277 1278int mlxsw_sp1_ptp_get_ts_info(struct mlxsw_sp *mlxsw_sp, 1279 struct ethtool_ts_info *info) 1280{ 1281 info->phc_index = ptp_clock_index(mlxsw_sp->clock->ptp); 1282 1283 info->so_timestamping = SOF_TIMESTAMPING_TX_HARDWARE | 1284 SOF_TIMESTAMPING_RX_HARDWARE | 1285 SOF_TIMESTAMPING_RAW_HARDWARE; 1286 1287 info->tx_types = BIT(HWTSTAMP_TX_OFF) | 1288 BIT(HWTSTAMP_TX_ON); 1289 1290 info->rx_filters = BIT(HWTSTAMP_FILTER_NONE) | 1291 BIT(HWTSTAMP_FILTER_ALL); 1292 1293 return 0; 1294} 1295 1296struct mlxsw_sp_ptp_port_stat { 1297 char str[ETH_GSTRING_LEN]; 1298 ptrdiff_t offset; 1299}; 1300 1301#define MLXSW_SP_PTP_PORT_STAT(NAME, FIELD) \ 1302 { \ 1303 .str = NAME, \ 1304 .offset = offsetof(struct mlxsw_sp_ptp_port_stats, \ 1305 FIELD), \ 1306 } 1307 1308static const struct mlxsw_sp_ptp_port_stat mlxsw_sp_ptp_port_stats[] = { 1309 MLXSW_SP_PTP_PORT_STAT("ptp_rx_gcd_packets", rx_gcd.packets), 1310 MLXSW_SP_PTP_PORT_STAT("ptp_rx_gcd_timestamps", rx_gcd.timestamps), 1311 MLXSW_SP_PTP_PORT_STAT("ptp_tx_gcd_packets", tx_gcd.packets), 1312 MLXSW_SP_PTP_PORT_STAT("ptp_tx_gcd_timestamps", tx_gcd.timestamps), 1313}; 1314 1315#undef MLXSW_SP_PTP_PORT_STAT 1316 1317#define MLXSW_SP_PTP_PORT_STATS_LEN \ 1318 ARRAY_SIZE(mlxsw_sp_ptp_port_stats) 1319 1320int mlxsw_sp1_get_stats_count(void) 1321{ 1322 return MLXSW_SP_PTP_PORT_STATS_LEN; 1323} 1324 1325void mlxsw_sp1_get_stats_strings(u8 **p) 1326{ 1327 int i; 1328 1329 for (i = 0; i < MLXSW_SP_PTP_PORT_STATS_LEN; i++) { 1330 memcpy(*p, mlxsw_sp_ptp_port_stats[i].str, 1331 ETH_GSTRING_LEN); 1332 *p += ETH_GSTRING_LEN; 1333 } 1334} 1335 1336void mlxsw_sp1_get_stats(struct mlxsw_sp_port *mlxsw_sp_port, 1337 u64 *data, int data_index) 1338{ 1339 void *stats = &mlxsw_sp_port->ptp.stats; 1340 ptrdiff_t offset; 1341 int i; 1342 1343 data += data_index; 1344 for (i = 0; i < MLXSW_SP_PTP_PORT_STATS_LEN; i++) { 1345 offset = mlxsw_sp_ptp_port_stats[i].offset; 1346 *data++ = *(u64 *)(stats + offset); 1347 } 1348} 1349 1350struct mlxsw_sp_ptp_state *mlxsw_sp2_ptp_init(struct mlxsw_sp *mlxsw_sp) 1351{ 1352 struct mlxsw_sp2_ptp_state *ptp_state; 1353 int err; 1354 1355 ptp_state = kzalloc(sizeof(*ptp_state), GFP_KERNEL); 1356 if (!ptp_state) 1357 return ERR_PTR(-ENOMEM); 1358 1359 ptp_state->common.mlxsw_sp = mlxsw_sp; 1360 1361 err = mlxsw_sp_ptp_traps_set(mlxsw_sp); 1362 if (err) 1363 goto err_ptp_traps_set; 1364 1365 refcount_set(&ptp_state->ptp_port_enabled_ref, 0); 1366 mutex_init(&ptp_state->lock); 1367 return &ptp_state->common; 1368 1369err_ptp_traps_set: 1370 kfree(ptp_state); 1371 return ERR_PTR(err); 1372} 1373 1374void mlxsw_sp2_ptp_fini(struct mlxsw_sp_ptp_state *ptp_state_common) 1375{ 1376 struct mlxsw_sp *mlxsw_sp = ptp_state_common->mlxsw_sp; 1377 struct mlxsw_sp2_ptp_state *ptp_state; 1378 1379 ptp_state = mlxsw_sp2_ptp_state(mlxsw_sp); 1380 1381 mutex_destroy(&ptp_state->lock); 1382 mlxsw_sp_ptp_traps_unset(mlxsw_sp); 1383 kfree(ptp_state); 1384} 1385 1386static u32 mlxsw_ptp_utc_time_stamp_sec_get(struct mlxsw_core *mlxsw_core, 1387 u8 cqe_ts_sec) 1388{ 1389 u32 utc_sec = mlxsw_core_read_utc_sec(mlxsw_core); 1390 1391 if (cqe_ts_sec > (utc_sec & 0xff)) 1392 /* Time stamp above the last bits of UTC (UTC & 0xff) means the 1393 * latter has wrapped after the time stamp was collected. 1394 */ 1395 utc_sec -= 256; 1396 1397 utc_sec &= ~0xff; 1398 utc_sec |= cqe_ts_sec; 1399 1400 return utc_sec; 1401} 1402 1403static void mlxsw_sp2_ptp_hwtstamp_fill(struct mlxsw_core *mlxsw_core, 1404 const struct mlxsw_skb_cb *cb, 1405 struct skb_shared_hwtstamps *hwtstamps) 1406{ 1407 u64 ts_sec, ts_nsec, nsec; 1408 1409 WARN_ON_ONCE(!cb->cqe_ts.sec && !cb->cqe_ts.nsec); 1410 1411 /* The time stamp in the CQE is represented by 38 bits, which is a short 1412 * representation of UTC time. Software should create the full time 1413 * stamp using the global UTC clock. The seconds have only 8 bits in the 1414 * CQE, to create the full time stamp, use the current UTC time and fix 1415 * the seconds according to the relation between UTC seconds and CQE 1416 * seconds. 1417 */ 1418 ts_sec = mlxsw_ptp_utc_time_stamp_sec_get(mlxsw_core, cb->cqe_ts.sec); 1419 ts_nsec = cb->cqe_ts.nsec; 1420 1421 nsec = ts_sec * NSEC_PER_SEC + ts_nsec; 1422 1423 hwtstamps->hwtstamp = ns_to_ktime(nsec); 1424} 1425 1426void mlxsw_sp2_ptp_receive(struct mlxsw_sp *mlxsw_sp, struct sk_buff *skb, 1427 u16 local_port) 1428{ 1429 struct skb_shared_hwtstamps hwtstamps; 1430 1431 mlxsw_sp2_ptp_hwtstamp_fill(mlxsw_sp->core, mlxsw_skb_cb(skb), 1432 &hwtstamps); 1433 *skb_hwtstamps(skb) = hwtstamps; 1434 mlxsw_sp_rx_listener_no_mark_func(skb, local_port, mlxsw_sp); 1435} 1436 1437void mlxsw_sp2_ptp_transmitted(struct mlxsw_sp *mlxsw_sp, 1438 struct sk_buff *skb, u16 local_port) 1439{ 1440 struct skb_shared_hwtstamps hwtstamps; 1441 1442 mlxsw_sp2_ptp_hwtstamp_fill(mlxsw_sp->core, mlxsw_skb_cb(skb), 1443 &hwtstamps); 1444 skb_tstamp_tx(skb, &hwtstamps); 1445 dev_kfree_skb_any(skb); 1446} 1447 1448int mlxsw_sp2_ptp_hwtstamp_get(struct mlxsw_sp_port *mlxsw_sp_port, 1449 struct hwtstamp_config *config) 1450{ 1451 struct mlxsw_sp2_ptp_state *ptp_state; 1452 1453 ptp_state = mlxsw_sp2_ptp_state(mlxsw_sp_port->mlxsw_sp); 1454 1455 mutex_lock(&ptp_state->lock); 1456 *config = ptp_state->config; 1457 mutex_unlock(&ptp_state->lock); 1458 1459 return 0; 1460} 1461 1462static int 1463mlxsw_sp2_ptp_get_message_types(const struct hwtstamp_config *config, 1464 u16 *p_ing_types, u16 *p_egr_types, 1465 enum hwtstamp_rx_filters *p_rx_filter) 1466{ 1467 enum hwtstamp_rx_filters rx_filter = config->rx_filter; 1468 enum hwtstamp_tx_types tx_type = config->tx_type; 1469 u16 ing_types = 0x00; 1470 u16 egr_types = 0x00; 1471 1472 *p_rx_filter = rx_filter; 1473 1474 switch (rx_filter) { 1475 case HWTSTAMP_FILTER_NONE: 1476 ing_types = 0x00; 1477 break; 1478 case HWTSTAMP_FILTER_PTP_V1_L4_SYNC: 1479 case HWTSTAMP_FILTER_PTP_V2_L4_SYNC: 1480 case HWTSTAMP_FILTER_PTP_V2_L2_SYNC: 1481 case HWTSTAMP_FILTER_PTP_V2_SYNC: 1482 case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ: 1483 case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ: 1484 case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ: 1485 case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ: 1486 case HWTSTAMP_FILTER_PTP_V1_L4_EVENT: 1487 case HWTSTAMP_FILTER_PTP_V2_L4_EVENT: 1488 case HWTSTAMP_FILTER_PTP_V2_L2_EVENT: 1489 case HWTSTAMP_FILTER_PTP_V2_EVENT: 1490 /* In Spectrum-2 and above, all packets get time stamp by 1491 * default and the driver fill the time stamp only for event 1492 * packets. Return all event types even if only specific types 1493 * were required. 1494 */ 1495 ing_types = 0x0f; 1496 *p_rx_filter = HWTSTAMP_FILTER_SOME; 1497 break; 1498 case HWTSTAMP_FILTER_ALL: 1499 case HWTSTAMP_FILTER_SOME: 1500 case HWTSTAMP_FILTER_NTP_ALL: 1501 return -ERANGE; 1502 default: 1503 return -EINVAL; 1504 } 1505 1506 switch (tx_type) { 1507 case HWTSTAMP_TX_OFF: 1508 egr_types = 0x00; 1509 break; 1510 case HWTSTAMP_TX_ON: 1511 egr_types = 0x0f; 1512 break; 1513 case HWTSTAMP_TX_ONESTEP_SYNC: 1514 case HWTSTAMP_TX_ONESTEP_P2P: 1515 return -ERANGE; 1516 default: 1517 return -EINVAL; 1518 } 1519 1520 if ((ing_types && !egr_types) || (!ing_types && egr_types)) 1521 return -EINVAL; 1522 1523 *p_ing_types = ing_types; 1524 *p_egr_types = egr_types; 1525 return 0; 1526} 1527 1528static int mlxsw_sp2_ptp_mtpcpc_set(struct mlxsw_sp *mlxsw_sp, bool ptp_trap_en, 1529 u16 ing_types, u16 egr_types) 1530{ 1531 char mtpcpc_pl[MLXSW_REG_MTPCPC_LEN]; 1532 1533 mlxsw_reg_mtpcpc_pack(mtpcpc_pl, false, 0, ptp_trap_en, ing_types, 1534 egr_types); 1535 return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(mtpcpc), mtpcpc_pl); 1536} 1537 1538static int mlxsw_sp2_ptp_enable(struct mlxsw_sp *mlxsw_sp, u16 ing_types, 1539 u16 egr_types, 1540 struct hwtstamp_config new_config) 1541{ 1542 struct mlxsw_sp2_ptp_state *ptp_state = mlxsw_sp2_ptp_state(mlxsw_sp); 1543 int err; 1544 1545 err = mlxsw_sp2_ptp_mtpcpc_set(mlxsw_sp, true, ing_types, egr_types); 1546 if (err) 1547 return err; 1548 1549 ptp_state->config = new_config; 1550 return 0; 1551} 1552 1553static int mlxsw_sp2_ptp_disable(struct mlxsw_sp *mlxsw_sp, 1554 struct hwtstamp_config new_config) 1555{ 1556 struct mlxsw_sp2_ptp_state *ptp_state = mlxsw_sp2_ptp_state(mlxsw_sp); 1557 int err; 1558 1559 err = mlxsw_sp2_ptp_mtpcpc_set(mlxsw_sp, false, 0, 0); 1560 if (err) 1561 return err; 1562 1563 ptp_state->config = new_config; 1564 return 0; 1565} 1566 1567static int mlxsw_sp2_ptp_configure_port(struct mlxsw_sp_port *mlxsw_sp_port, 1568 u16 ing_types, u16 egr_types, 1569 struct hwtstamp_config new_config) 1570{ 1571 struct mlxsw_sp2_ptp_state *ptp_state; 1572 int err; 1573 1574 ptp_state = mlxsw_sp2_ptp_state(mlxsw_sp_port->mlxsw_sp); 1575 1576 if (refcount_inc_not_zero(&ptp_state->ptp_port_enabled_ref)) 1577 return 0; 1578 1579 err = mlxsw_sp2_ptp_enable(mlxsw_sp_port->mlxsw_sp, ing_types, 1580 egr_types, new_config); 1581 if (err) 1582 return err; 1583 1584 refcount_set(&ptp_state->ptp_port_enabled_ref, 1); 1585 1586 return 0; 1587} 1588 1589static int mlxsw_sp2_ptp_deconfigure_port(struct mlxsw_sp_port *mlxsw_sp_port, 1590 struct hwtstamp_config new_config) 1591{ 1592 struct mlxsw_sp2_ptp_state *ptp_state; 1593 int err; 1594 1595 ptp_state = mlxsw_sp2_ptp_state(mlxsw_sp_port->mlxsw_sp); 1596 1597 if (!refcount_dec_and_test(&ptp_state->ptp_port_enabled_ref)) 1598 return 0; 1599 1600 err = mlxsw_sp2_ptp_disable(mlxsw_sp_port->mlxsw_sp, new_config); 1601 if (err) 1602 goto err_ptp_disable; 1603 1604 return 0; 1605 1606err_ptp_disable: 1607 refcount_set(&ptp_state->ptp_port_enabled_ref, 1); 1608 return err; 1609} 1610 1611int mlxsw_sp2_ptp_hwtstamp_set(struct mlxsw_sp_port *mlxsw_sp_port, 1612 struct hwtstamp_config *config) 1613{ 1614 struct mlxsw_sp2_ptp_state *ptp_state; 1615 enum hwtstamp_rx_filters rx_filter; 1616 struct hwtstamp_config new_config; 1617 u16 new_ing_types, new_egr_types; 1618 bool ptp_enabled; 1619 int err; 1620 1621 ptp_state = mlxsw_sp2_ptp_state(mlxsw_sp_port->mlxsw_sp); 1622 mutex_lock(&ptp_state->lock); 1623 1624 err = mlxsw_sp2_ptp_get_message_types(config, &new_ing_types, 1625 &new_egr_types, &rx_filter); 1626 if (err) 1627 goto err_get_message_types; 1628 1629 new_config.flags = config->flags; 1630 new_config.tx_type = config->tx_type; 1631 new_config.rx_filter = rx_filter; 1632 1633 ptp_enabled = mlxsw_sp_port->ptp.ing_types || 1634 mlxsw_sp_port->ptp.egr_types; 1635 1636 if ((new_ing_types || new_egr_types) && !ptp_enabled) { 1637 err = mlxsw_sp2_ptp_configure_port(mlxsw_sp_port, new_ing_types, 1638 new_egr_types, new_config); 1639 if (err) 1640 goto err_configure_port; 1641 } else if (!new_ing_types && !new_egr_types && ptp_enabled) { 1642 err = mlxsw_sp2_ptp_deconfigure_port(mlxsw_sp_port, new_config); 1643 if (err) 1644 goto err_deconfigure_port; 1645 } 1646 1647 mlxsw_sp_port->ptp.ing_types = new_ing_types; 1648 mlxsw_sp_port->ptp.egr_types = new_egr_types; 1649 1650 /* Notify the ioctl caller what we are actually timestamping. */ 1651 config->rx_filter = rx_filter; 1652 mutex_unlock(&ptp_state->lock); 1653 1654 return 0; 1655 1656err_deconfigure_port: 1657err_configure_port: 1658err_get_message_types: 1659 mutex_unlock(&ptp_state->lock); 1660 return err; 1661} 1662 1663int mlxsw_sp2_ptp_get_ts_info(struct mlxsw_sp *mlxsw_sp, 1664 struct ethtool_ts_info *info) 1665{ 1666 info->phc_index = ptp_clock_index(mlxsw_sp->clock->ptp); 1667 1668 info->so_timestamping = SOF_TIMESTAMPING_TX_HARDWARE | 1669 SOF_TIMESTAMPING_RX_HARDWARE | 1670 SOF_TIMESTAMPING_RAW_HARDWARE; 1671 1672 info->tx_types = BIT(HWTSTAMP_TX_OFF) | 1673 BIT(HWTSTAMP_TX_ON); 1674 1675 info->rx_filters = BIT(HWTSTAMP_FILTER_NONE) | 1676 BIT(HWTSTAMP_FILTER_PTP_V1_L4_EVENT) | 1677 BIT(HWTSTAMP_FILTER_PTP_V2_EVENT); 1678 1679 return 0; 1680} 1681 1682int mlxsw_sp_ptp_txhdr_construct(struct mlxsw_core *mlxsw_core, 1683 struct mlxsw_sp_port *mlxsw_sp_port, 1684 struct sk_buff *skb, 1685 const struct mlxsw_tx_info *tx_info) 1686{ 1687 mlxsw_sp_txhdr_construct(skb, tx_info); 1688 return 0; 1689} 1690 1691int mlxsw_sp2_ptp_txhdr_construct(struct mlxsw_core *mlxsw_core, 1692 struct mlxsw_sp_port *mlxsw_sp_port, 1693 struct sk_buff *skb, 1694 const struct mlxsw_tx_info *tx_info) 1695{ 1696 /* In Spectrum-2 and Spectrum-3, in order for PTP event packets to have 1697 * their correction field correctly set on the egress port they must be 1698 * transmitted as data packets. Such packets ingress the ASIC via the 1699 * CPU port and must have a VLAN tag, as the CPU port is not configured 1700 * with a PVID. Push the default VLAN (4095), which is configured as 1701 * egress untagged on all the ports. 1702 */ 1703 if (!skb_vlan_tagged(skb)) { 1704 skb = vlan_insert_tag_set_proto(skb, htons(ETH_P_8021Q), 1705 MLXSW_SP_DEFAULT_VID); 1706 if (!skb) { 1707 this_cpu_inc(mlxsw_sp_port->pcpu_stats->tx_dropped); 1708 return -ENOMEM; 1709 } 1710 } 1711 1712 return mlxsw_sp_txhdr_ptp_data_construct(mlxsw_core, mlxsw_sp_port, skb, 1713 tx_info); 1714} 1715