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