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 15 #include "spectrum.h" 16 #include "spectrum_ptp.h" 17 #include "core.h" 18 19 #define MLXSW_SP1_PTP_CLOCK_CYCLES_SHIFT 29 20 #define MLXSW_SP1_PTP_CLOCK_FREQ_KHZ 156257 /* 6.4nSec */ 21 #define MLXSW_SP1_PTP_CLOCK_MASK 64 22 23 #define MLXSW_SP1_PTP_HT_GC_INTERVAL 500 /* ms */ 24 25 /* How long, approximately, should the unmatched entries stay in the hash table 26 * before they are collected. Should be evenly divisible by the GC interval. 27 */ 28 #define MLXSW_SP1_PTP_HT_GC_TIMEOUT 1000 /* ms */ 29 30 struct mlxsw_sp_ptp_state { 31 struct mlxsw_sp *mlxsw_sp; 32 struct rhashtable unmatched_ht; 33 spinlock_t unmatched_lock; /* protects the HT */ 34 struct delayed_work ht_gc_dw; 35 u32 gc_cycle; 36 }; 37 38 struct mlxsw_sp1_ptp_key { 39 u8 local_port; 40 u8 message_type; 41 u16 sequence_id; 42 u8 domain_number; 43 bool ingress; 44 }; 45 46 struct mlxsw_sp1_ptp_unmatched { 47 struct mlxsw_sp1_ptp_key key; 48 struct rhash_head ht_node; 49 struct rcu_head rcu; 50 struct sk_buff *skb; 51 u64 timestamp; 52 u32 gc_cycle; 53 }; 54 55 static const struct rhashtable_params mlxsw_sp1_ptp_unmatched_ht_params = { 56 .key_len = sizeof_field(struct mlxsw_sp1_ptp_unmatched, key), 57 .key_offset = offsetof(struct mlxsw_sp1_ptp_unmatched, key), 58 .head_offset = offsetof(struct mlxsw_sp1_ptp_unmatched, ht_node), 59 }; 60 61 struct mlxsw_sp_ptp_clock { 62 struct mlxsw_core *core; 63 spinlock_t lock; /* protect this structure */ 64 struct cyclecounter cycles; 65 struct timecounter tc; 66 u32 nominal_c_mult; 67 struct ptp_clock *ptp; 68 struct ptp_clock_info ptp_info; 69 unsigned long overflow_period; 70 struct delayed_work overflow_work; 71 }; 72 73 static u64 __mlxsw_sp1_ptp_read_frc(struct mlxsw_sp_ptp_clock *clock, 74 struct ptp_system_timestamp *sts) 75 { 76 struct mlxsw_core *mlxsw_core = clock->core; 77 u32 frc_h1, frc_h2, frc_l; 78 79 frc_h1 = mlxsw_core_read_frc_h(mlxsw_core); 80 ptp_read_system_prets(sts); 81 frc_l = mlxsw_core_read_frc_l(mlxsw_core); 82 ptp_read_system_postts(sts); 83 frc_h2 = mlxsw_core_read_frc_h(mlxsw_core); 84 85 if (frc_h1 != frc_h2) { 86 /* wrap around */ 87 ptp_read_system_prets(sts); 88 frc_l = mlxsw_core_read_frc_l(mlxsw_core); 89 ptp_read_system_postts(sts); 90 } 91 92 return (u64) frc_l | (u64) frc_h2 << 32; 93 } 94 95 static u64 mlxsw_sp1_ptp_read_frc(const struct cyclecounter *cc) 96 { 97 struct mlxsw_sp_ptp_clock *clock = 98 container_of(cc, struct mlxsw_sp_ptp_clock, cycles); 99 100 return __mlxsw_sp1_ptp_read_frc(clock, NULL) & cc->mask; 101 } 102 103 static int 104 mlxsw_sp1_ptp_phc_adjfreq(struct mlxsw_sp_ptp_clock *clock, int freq_adj) 105 { 106 struct mlxsw_core *mlxsw_core = clock->core; 107 char mtutc_pl[MLXSW_REG_MTUTC_LEN]; 108 109 mlxsw_reg_mtutc_pack(mtutc_pl, MLXSW_REG_MTUTC_OPERATION_ADJUST_FREQ, 110 freq_adj, 0); 111 return mlxsw_reg_write(mlxsw_core, MLXSW_REG(mtutc), mtutc_pl); 112 } 113 114 static u64 mlxsw_sp1_ptp_ns2cycles(const struct timecounter *tc, u64 nsec) 115 { 116 u64 cycles = (u64) nsec; 117 118 cycles <<= tc->cc->shift; 119 cycles = div_u64(cycles, tc->cc->mult); 120 121 return cycles; 122 } 123 124 static int 125 mlxsw_sp1_ptp_phc_settime(struct mlxsw_sp_ptp_clock *clock, u64 nsec) 126 { 127 struct mlxsw_core *mlxsw_core = clock->core; 128 u64 next_sec, next_sec_in_nsec, cycles; 129 char mtutc_pl[MLXSW_REG_MTUTC_LEN]; 130 char mtpps_pl[MLXSW_REG_MTPPS_LEN]; 131 int err; 132 133 next_sec = div_u64(nsec, NSEC_PER_SEC) + 1; 134 next_sec_in_nsec = next_sec * NSEC_PER_SEC; 135 136 spin_lock_bh(&clock->lock); 137 cycles = mlxsw_sp1_ptp_ns2cycles(&clock->tc, next_sec_in_nsec); 138 spin_unlock_bh(&clock->lock); 139 140 mlxsw_reg_mtpps_vpin_pack(mtpps_pl, cycles); 141 err = mlxsw_reg_write(mlxsw_core, MLXSW_REG(mtpps), mtpps_pl); 142 if (err) 143 return err; 144 145 mlxsw_reg_mtutc_pack(mtutc_pl, 146 MLXSW_REG_MTUTC_OPERATION_SET_TIME_AT_NEXT_SEC, 147 0, next_sec); 148 return mlxsw_reg_write(mlxsw_core, MLXSW_REG(mtutc), mtutc_pl); 149 } 150 151 static int mlxsw_sp1_ptp_adjfine(struct ptp_clock_info *ptp, long scaled_ppm) 152 { 153 struct mlxsw_sp_ptp_clock *clock = 154 container_of(ptp, struct mlxsw_sp_ptp_clock, ptp_info); 155 int neg_adj = 0; 156 u32 diff; 157 u64 adj; 158 s32 ppb; 159 160 ppb = scaled_ppm_to_ppb(scaled_ppm); 161 162 if (ppb < 0) { 163 neg_adj = 1; 164 ppb = -ppb; 165 } 166 167 adj = clock->nominal_c_mult; 168 adj *= ppb; 169 diff = div_u64(adj, NSEC_PER_SEC); 170 171 spin_lock_bh(&clock->lock); 172 timecounter_read(&clock->tc); 173 clock->cycles.mult = neg_adj ? clock->nominal_c_mult - diff : 174 clock->nominal_c_mult + diff; 175 spin_unlock_bh(&clock->lock); 176 177 return mlxsw_sp1_ptp_phc_adjfreq(clock, neg_adj ? -ppb : ppb); 178 } 179 180 static int mlxsw_sp1_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta) 181 { 182 struct mlxsw_sp_ptp_clock *clock = 183 container_of(ptp, struct mlxsw_sp_ptp_clock, ptp_info); 184 u64 nsec; 185 186 spin_lock_bh(&clock->lock); 187 timecounter_adjtime(&clock->tc, delta); 188 nsec = timecounter_read(&clock->tc); 189 spin_unlock_bh(&clock->lock); 190 191 return mlxsw_sp1_ptp_phc_settime(clock, nsec); 192 } 193 194 static int mlxsw_sp1_ptp_gettimex(struct ptp_clock_info *ptp, 195 struct timespec64 *ts, 196 struct ptp_system_timestamp *sts) 197 { 198 struct mlxsw_sp_ptp_clock *clock = 199 container_of(ptp, struct mlxsw_sp_ptp_clock, ptp_info); 200 u64 cycles, nsec; 201 202 spin_lock_bh(&clock->lock); 203 cycles = __mlxsw_sp1_ptp_read_frc(clock, sts); 204 nsec = timecounter_cyc2time(&clock->tc, cycles); 205 spin_unlock_bh(&clock->lock); 206 207 *ts = ns_to_timespec64(nsec); 208 209 return 0; 210 } 211 212 static int mlxsw_sp1_ptp_settime(struct ptp_clock_info *ptp, 213 const struct timespec64 *ts) 214 { 215 struct mlxsw_sp_ptp_clock *clock = 216 container_of(ptp, struct mlxsw_sp_ptp_clock, ptp_info); 217 u64 nsec = timespec64_to_ns(ts); 218 219 spin_lock_bh(&clock->lock); 220 timecounter_init(&clock->tc, &clock->cycles, nsec); 221 nsec = timecounter_read(&clock->tc); 222 spin_unlock_bh(&clock->lock); 223 224 return mlxsw_sp1_ptp_phc_settime(clock, nsec); 225 } 226 227 static const struct ptp_clock_info mlxsw_sp1_ptp_clock_info = { 228 .owner = THIS_MODULE, 229 .name = "mlxsw_sp_clock", 230 .max_adj = 100000000, 231 .adjfine = mlxsw_sp1_ptp_adjfine, 232 .adjtime = mlxsw_sp1_ptp_adjtime, 233 .gettimex64 = mlxsw_sp1_ptp_gettimex, 234 .settime64 = mlxsw_sp1_ptp_settime, 235 }; 236 237 static void mlxsw_sp1_ptp_clock_overflow(struct work_struct *work) 238 { 239 struct delayed_work *dwork = to_delayed_work(work); 240 struct mlxsw_sp_ptp_clock *clock; 241 242 clock = container_of(dwork, struct mlxsw_sp_ptp_clock, overflow_work); 243 244 spin_lock_bh(&clock->lock); 245 timecounter_read(&clock->tc); 246 spin_unlock_bh(&clock->lock); 247 mlxsw_core_schedule_dw(&clock->overflow_work, clock->overflow_period); 248 } 249 250 struct mlxsw_sp_ptp_clock * 251 mlxsw_sp1_ptp_clock_init(struct mlxsw_sp *mlxsw_sp, struct device *dev) 252 { 253 u64 overflow_cycles, nsec, frac = 0; 254 struct mlxsw_sp_ptp_clock *clock; 255 int err; 256 257 clock = kzalloc(sizeof(*clock), GFP_KERNEL); 258 if (!clock) 259 return ERR_PTR(-ENOMEM); 260 261 spin_lock_init(&clock->lock); 262 clock->cycles.read = mlxsw_sp1_ptp_read_frc; 263 clock->cycles.shift = MLXSW_SP1_PTP_CLOCK_CYCLES_SHIFT; 264 clock->cycles.mult = clocksource_khz2mult(MLXSW_SP1_PTP_CLOCK_FREQ_KHZ, 265 clock->cycles.shift); 266 clock->nominal_c_mult = clock->cycles.mult; 267 clock->cycles.mask = CLOCKSOURCE_MASK(MLXSW_SP1_PTP_CLOCK_MASK); 268 clock->core = mlxsw_sp->core; 269 270 timecounter_init(&clock->tc, &clock->cycles, 271 ktime_to_ns(ktime_get_real())); 272 273 /* Calculate period in seconds to call the overflow watchdog - to make 274 * sure counter is checked at least twice every wrap around. 275 * The period is calculated as the minimum between max HW cycles count 276 * (The clock source mask) and max amount of cycles that can be 277 * multiplied by clock multiplier where the result doesn't exceed 278 * 64bits. 279 */ 280 overflow_cycles = div64_u64(~0ULL >> 1, clock->cycles.mult); 281 overflow_cycles = min(overflow_cycles, div_u64(clock->cycles.mask, 3)); 282 283 nsec = cyclecounter_cyc2ns(&clock->cycles, overflow_cycles, 0, &frac); 284 clock->overflow_period = nsecs_to_jiffies(nsec); 285 286 INIT_DELAYED_WORK(&clock->overflow_work, mlxsw_sp1_ptp_clock_overflow); 287 mlxsw_core_schedule_dw(&clock->overflow_work, 0); 288 289 clock->ptp_info = mlxsw_sp1_ptp_clock_info; 290 clock->ptp = ptp_clock_register(&clock->ptp_info, dev); 291 if (IS_ERR(clock->ptp)) { 292 err = PTR_ERR(clock->ptp); 293 dev_err(dev, "ptp_clock_register failed %d\n", err); 294 goto err_ptp_clock_register; 295 } 296 297 return clock; 298 299 err_ptp_clock_register: 300 cancel_delayed_work_sync(&clock->overflow_work); 301 kfree(clock); 302 return ERR_PTR(err); 303 } 304 305 void mlxsw_sp1_ptp_clock_fini(struct mlxsw_sp_ptp_clock *clock) 306 { 307 ptp_clock_unregister(clock->ptp); 308 cancel_delayed_work_sync(&clock->overflow_work); 309 kfree(clock); 310 } 311 312 static int mlxsw_sp_ptp_parse(struct sk_buff *skb, 313 u8 *p_domain_number, 314 u8 *p_message_type, 315 u16 *p_sequence_id) 316 { 317 unsigned int offset = 0; 318 unsigned int ptp_class; 319 u8 *data; 320 321 data = skb_mac_header(skb); 322 ptp_class = ptp_classify_raw(skb); 323 324 switch (ptp_class & PTP_CLASS_VMASK) { 325 case PTP_CLASS_V1: 326 case PTP_CLASS_V2: 327 break; 328 default: 329 return -ERANGE; 330 } 331 332 if (ptp_class & PTP_CLASS_VLAN) 333 offset += VLAN_HLEN; 334 335 switch (ptp_class & PTP_CLASS_PMASK) { 336 case PTP_CLASS_IPV4: 337 offset += ETH_HLEN + IPV4_HLEN(data + offset) + UDP_HLEN; 338 break; 339 case PTP_CLASS_IPV6: 340 offset += ETH_HLEN + IP6_HLEN + UDP_HLEN; 341 break; 342 case PTP_CLASS_L2: 343 offset += ETH_HLEN; 344 break; 345 default: 346 return -ERANGE; 347 } 348 349 /* PTP header is 34 bytes. */ 350 if (skb->len < offset + 34) 351 return -EINVAL; 352 353 *p_message_type = data[offset] & 0x0f; 354 *p_domain_number = data[offset + 4]; 355 *p_sequence_id = (u16)(data[offset + 30]) << 8 | data[offset + 31]; 356 return 0; 357 } 358 359 /* Returns NULL on successful insertion, a pointer on conflict, or an ERR_PTR on 360 * error. 361 */ 362 static struct mlxsw_sp1_ptp_unmatched * 363 mlxsw_sp1_ptp_unmatched_save(struct mlxsw_sp *mlxsw_sp, 364 struct mlxsw_sp1_ptp_key key, 365 struct sk_buff *skb, 366 u64 timestamp) 367 { 368 int cycles = MLXSW_SP1_PTP_HT_GC_TIMEOUT / MLXSW_SP1_PTP_HT_GC_INTERVAL; 369 struct mlxsw_sp_ptp_state *ptp_state = mlxsw_sp->ptp_state; 370 struct mlxsw_sp1_ptp_unmatched *unmatched; 371 struct mlxsw_sp1_ptp_unmatched *conflict; 372 373 unmatched = kzalloc(sizeof(*unmatched), GFP_ATOMIC); 374 if (!unmatched) 375 return ERR_PTR(-ENOMEM); 376 377 unmatched->key = key; 378 unmatched->skb = skb; 379 unmatched->timestamp = timestamp; 380 unmatched->gc_cycle = mlxsw_sp->ptp_state->gc_cycle + cycles; 381 382 conflict = rhashtable_lookup_get_insert_fast(&ptp_state->unmatched_ht, 383 &unmatched->ht_node, 384 mlxsw_sp1_ptp_unmatched_ht_params); 385 if (conflict) 386 kfree(unmatched); 387 388 return conflict; 389 } 390 391 static struct mlxsw_sp1_ptp_unmatched * 392 mlxsw_sp1_ptp_unmatched_lookup(struct mlxsw_sp *mlxsw_sp, 393 struct mlxsw_sp1_ptp_key key) 394 { 395 return rhashtable_lookup(&mlxsw_sp->ptp_state->unmatched_ht, &key, 396 mlxsw_sp1_ptp_unmatched_ht_params); 397 } 398 399 static int 400 mlxsw_sp1_ptp_unmatched_remove(struct mlxsw_sp *mlxsw_sp, 401 struct mlxsw_sp1_ptp_unmatched *unmatched) 402 { 403 return rhashtable_remove_fast(&mlxsw_sp->ptp_state->unmatched_ht, 404 &unmatched->ht_node, 405 mlxsw_sp1_ptp_unmatched_ht_params); 406 } 407 408 /* This function is called in the following scenarios: 409 * 410 * 1) When a packet is matched with its timestamp. 411 * 2) In several situation when it is necessary to immediately pass on 412 * an SKB without a timestamp. 413 * 3) From GC indirectly through mlxsw_sp1_ptp_unmatched_finish(). 414 * This case is similar to 2) above. 415 */ 416 static void mlxsw_sp1_ptp_packet_finish(struct mlxsw_sp *mlxsw_sp, 417 struct sk_buff *skb, u8 local_port, 418 bool ingress, 419 struct skb_shared_hwtstamps *hwtstamps) 420 { 421 struct mlxsw_sp_port *mlxsw_sp_port; 422 423 /* Between capturing the packet and finishing it, there is a window of 424 * opportunity for the originating port to go away (e.g. due to a 425 * split). Also make sure the SKB device reference is still valid. 426 */ 427 mlxsw_sp_port = mlxsw_sp->ports[local_port]; 428 if (!(mlxsw_sp_port && (!skb->dev || skb->dev == mlxsw_sp_port->dev))) { 429 dev_kfree_skb_any(skb); 430 return; 431 } 432 433 if (ingress) { 434 if (hwtstamps) 435 *skb_hwtstamps(skb) = *hwtstamps; 436 mlxsw_sp_rx_listener_no_mark_func(skb, local_port, mlxsw_sp); 437 } else { 438 /* skb_tstamp_tx() allows hwtstamps to be NULL. */ 439 skb_tstamp_tx(skb, hwtstamps); 440 dev_kfree_skb_any(skb); 441 } 442 } 443 444 static void mlxsw_sp1_packet_timestamp(struct mlxsw_sp *mlxsw_sp, 445 struct mlxsw_sp1_ptp_key key, 446 struct sk_buff *skb, 447 u64 timestamp) 448 { 449 struct skb_shared_hwtstamps hwtstamps; 450 u64 nsec; 451 452 spin_lock_bh(&mlxsw_sp->clock->lock); 453 nsec = timecounter_cyc2time(&mlxsw_sp->clock->tc, timestamp); 454 spin_unlock_bh(&mlxsw_sp->clock->lock); 455 456 hwtstamps.hwtstamp = ns_to_ktime(nsec); 457 mlxsw_sp1_ptp_packet_finish(mlxsw_sp, skb, 458 key.local_port, key.ingress, &hwtstamps); 459 } 460 461 static void 462 mlxsw_sp1_ptp_unmatched_finish(struct mlxsw_sp *mlxsw_sp, 463 struct mlxsw_sp1_ptp_unmatched *unmatched) 464 { 465 if (unmatched->skb && unmatched->timestamp) 466 mlxsw_sp1_packet_timestamp(mlxsw_sp, unmatched->key, 467 unmatched->skb, 468 unmatched->timestamp); 469 else if (unmatched->skb) 470 mlxsw_sp1_ptp_packet_finish(mlxsw_sp, unmatched->skb, 471 unmatched->key.local_port, 472 unmatched->key.ingress, NULL); 473 kfree_rcu(unmatched, rcu); 474 } 475 476 static void mlxsw_sp1_ptp_unmatched_free_fn(void *ptr, void *arg) 477 { 478 struct mlxsw_sp1_ptp_unmatched *unmatched = ptr; 479 480 /* This is invoked at a point where the ports are gone already. Nothing 481 * to do with whatever is left in the HT but to free it. 482 */ 483 if (unmatched->skb) 484 dev_kfree_skb_any(unmatched->skb); 485 kfree_rcu(unmatched, rcu); 486 } 487 488 static void mlxsw_sp1_ptp_got_piece(struct mlxsw_sp *mlxsw_sp, 489 struct mlxsw_sp1_ptp_key key, 490 struct sk_buff *skb, u64 timestamp) 491 { 492 struct mlxsw_sp1_ptp_unmatched *unmatched, *conflict; 493 int err; 494 495 rcu_read_lock(); 496 497 unmatched = mlxsw_sp1_ptp_unmatched_lookup(mlxsw_sp, key); 498 499 spin_lock(&mlxsw_sp->ptp_state->unmatched_lock); 500 501 if (unmatched) { 502 /* There was an unmatched entry when we looked, but it may have 503 * been removed before we took the lock. 504 */ 505 err = mlxsw_sp1_ptp_unmatched_remove(mlxsw_sp, unmatched); 506 if (err) 507 unmatched = NULL; 508 } 509 510 if (!unmatched) { 511 /* We have no unmatched entry, but one may have been added after 512 * we looked, but before we took the lock. 513 */ 514 unmatched = mlxsw_sp1_ptp_unmatched_save(mlxsw_sp, key, 515 skb, timestamp); 516 if (IS_ERR(unmatched)) { 517 if (skb) 518 mlxsw_sp1_ptp_packet_finish(mlxsw_sp, skb, 519 key.local_port, 520 key.ingress, NULL); 521 unmatched = NULL; 522 } else if (unmatched) { 523 /* Save just told us, under lock, that the entry is 524 * there, so this has to work. 525 */ 526 err = mlxsw_sp1_ptp_unmatched_remove(mlxsw_sp, 527 unmatched); 528 WARN_ON_ONCE(err); 529 } 530 } 531 532 /* If unmatched is non-NULL here, it comes either from the lookup, or 533 * from the save attempt above. In either case the entry was removed 534 * from the hash table. If unmatched is NULL, a new unmatched entry was 535 * added to the hash table, and there was no conflict. 536 */ 537 538 if (skb && unmatched && unmatched->timestamp) { 539 unmatched->skb = skb; 540 } else if (timestamp && unmatched && unmatched->skb) { 541 unmatched->timestamp = timestamp; 542 } else if (unmatched) { 543 /* unmatched holds an older entry of the same type: either an 544 * skb if we are handling skb, or a timestamp if we are handling 545 * timestamp. We can't match that up, so save what we have. 546 */ 547 conflict = mlxsw_sp1_ptp_unmatched_save(mlxsw_sp, key, 548 skb, timestamp); 549 if (IS_ERR(conflict)) { 550 if (skb) 551 mlxsw_sp1_ptp_packet_finish(mlxsw_sp, skb, 552 key.local_port, 553 key.ingress, NULL); 554 } else { 555 /* Above, we removed an object with this key from the 556 * hash table, under lock, so conflict can not be a 557 * valid pointer. 558 */ 559 WARN_ON_ONCE(conflict); 560 } 561 } 562 563 spin_unlock(&mlxsw_sp->ptp_state->unmatched_lock); 564 565 if (unmatched) 566 mlxsw_sp1_ptp_unmatched_finish(mlxsw_sp, unmatched); 567 568 rcu_read_unlock(); 569 } 570 571 static void mlxsw_sp1_ptp_got_packet(struct mlxsw_sp *mlxsw_sp, 572 struct sk_buff *skb, u8 local_port, 573 bool ingress) 574 { 575 struct mlxsw_sp_port *mlxsw_sp_port; 576 struct mlxsw_sp1_ptp_key key; 577 u8 types; 578 int err; 579 580 mlxsw_sp_port = mlxsw_sp->ports[local_port]; 581 if (!mlxsw_sp_port) 582 goto immediate; 583 584 types = ingress ? mlxsw_sp_port->ptp.ing_types : 585 mlxsw_sp_port->ptp.egr_types; 586 if (!types) 587 goto immediate; 588 589 memset(&key, 0, sizeof(key)); 590 key.local_port = local_port; 591 key.ingress = ingress; 592 593 err = mlxsw_sp_ptp_parse(skb, &key.domain_number, &key.message_type, 594 &key.sequence_id); 595 if (err) 596 goto immediate; 597 598 /* For packets whose timestamping was not enabled on this port, don't 599 * bother trying to match the timestamp. 600 */ 601 if (!((1 << key.message_type) & types)) 602 goto immediate; 603 604 mlxsw_sp1_ptp_got_piece(mlxsw_sp, key, skb, 0); 605 return; 606 607 immediate: 608 mlxsw_sp1_ptp_packet_finish(mlxsw_sp, skb, local_port, ingress, NULL); 609 } 610 611 void mlxsw_sp1_ptp_got_timestamp(struct mlxsw_sp *mlxsw_sp, bool ingress, 612 u8 local_port, u8 message_type, 613 u8 domain_number, u16 sequence_id, 614 u64 timestamp) 615 { 616 struct mlxsw_sp_port *mlxsw_sp_port; 617 struct mlxsw_sp1_ptp_key key; 618 u8 types; 619 620 mlxsw_sp_port = mlxsw_sp->ports[local_port]; 621 if (!mlxsw_sp_port) 622 return; 623 624 types = ingress ? mlxsw_sp_port->ptp.ing_types : 625 mlxsw_sp_port->ptp.egr_types; 626 627 /* For message types whose timestamping was not enabled on this port, 628 * don't bother with the timestamp. 629 */ 630 if (!((1 << message_type) & types)) 631 return; 632 633 memset(&key, 0, sizeof(key)); 634 key.local_port = local_port; 635 key.domain_number = domain_number; 636 key.message_type = message_type; 637 key.sequence_id = sequence_id; 638 key.ingress = ingress; 639 640 mlxsw_sp1_ptp_got_piece(mlxsw_sp, key, NULL, timestamp); 641 } 642 643 void mlxsw_sp1_ptp_receive(struct mlxsw_sp *mlxsw_sp, struct sk_buff *skb, 644 u8 local_port) 645 { 646 skb_reset_mac_header(skb); 647 mlxsw_sp1_ptp_got_packet(mlxsw_sp, skb, local_port, true); 648 } 649 650 void mlxsw_sp1_ptp_transmitted(struct mlxsw_sp *mlxsw_sp, 651 struct sk_buff *skb, u8 local_port) 652 { 653 mlxsw_sp1_ptp_got_packet(mlxsw_sp, skb, local_port, false); 654 } 655 656 static void 657 mlxsw_sp1_ptp_ht_gc_collect(struct mlxsw_sp_ptp_state *ptp_state, 658 struct mlxsw_sp1_ptp_unmatched *unmatched) 659 { 660 int err; 661 662 /* If an unmatched entry has an SKB, it has to be handed over to the 663 * networking stack. This is usually done from a trap handler, which is 664 * invoked in a softirq context. Here we are going to do it in process 665 * context. If that were to be interrupted by a softirq, it could cause 666 * a deadlock when an attempt is made to take an already-taken lock 667 * somewhere along the sending path. Disable softirqs to prevent this. 668 */ 669 local_bh_disable(); 670 671 spin_lock(&ptp_state->unmatched_lock); 672 err = rhashtable_remove_fast(&ptp_state->unmatched_ht, 673 &unmatched->ht_node, 674 mlxsw_sp1_ptp_unmatched_ht_params); 675 spin_unlock(&ptp_state->unmatched_lock); 676 677 if (err) 678 /* The packet was matched with timestamp during the walk. */ 679 goto out; 680 681 /* mlxsw_sp1_ptp_unmatched_finish() invokes netif_receive_skb(). While 682 * the comment at that function states that it can only be called in 683 * soft IRQ context, this pattern of local_bh_disable() + 684 * netif_receive_skb(), in process context, is seen elsewhere in the 685 * kernel, notably in pktgen. 686 */ 687 mlxsw_sp1_ptp_unmatched_finish(ptp_state->mlxsw_sp, unmatched); 688 689 out: 690 local_bh_enable(); 691 } 692 693 static void mlxsw_sp1_ptp_ht_gc(struct work_struct *work) 694 { 695 struct delayed_work *dwork = to_delayed_work(work); 696 struct mlxsw_sp1_ptp_unmatched *unmatched; 697 struct mlxsw_sp_ptp_state *ptp_state; 698 struct rhashtable_iter iter; 699 u32 gc_cycle; 700 void *obj; 701 702 ptp_state = container_of(dwork, struct mlxsw_sp_ptp_state, ht_gc_dw); 703 gc_cycle = ptp_state->gc_cycle++; 704 705 rhashtable_walk_enter(&ptp_state->unmatched_ht, &iter); 706 rhashtable_walk_start(&iter); 707 while ((obj = rhashtable_walk_next(&iter))) { 708 if (IS_ERR(obj)) 709 continue; 710 711 unmatched = obj; 712 if (unmatched->gc_cycle <= gc_cycle) 713 mlxsw_sp1_ptp_ht_gc_collect(ptp_state, unmatched); 714 } 715 rhashtable_walk_stop(&iter); 716 rhashtable_walk_exit(&iter); 717 718 mlxsw_core_schedule_dw(&ptp_state->ht_gc_dw, 719 MLXSW_SP1_PTP_HT_GC_INTERVAL); 720 } 721 722 static int mlxsw_sp_ptp_mtptpt_set(struct mlxsw_sp *mlxsw_sp, 723 enum mlxsw_reg_mtptpt_trap_id trap_id, 724 u16 message_type) 725 { 726 char mtptpt_pl[MLXSW_REG_MTPTPT_LEN]; 727 728 mlxsw_reg_mtptptp_pack(mtptpt_pl, trap_id, message_type); 729 return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(mtptpt), mtptpt_pl); 730 } 731 732 static int mlxsw_sp1_ptp_set_fifo_clr_on_trap(struct mlxsw_sp *mlxsw_sp, 733 bool clr) 734 { 735 char mogcr_pl[MLXSW_REG_MOGCR_LEN] = {0}; 736 int err; 737 738 err = mlxsw_reg_query(mlxsw_sp->core, MLXSW_REG(mogcr), mogcr_pl); 739 if (err) 740 return err; 741 742 mlxsw_reg_mogcr_ptp_iftc_set(mogcr_pl, clr); 743 mlxsw_reg_mogcr_ptp_eftc_set(mogcr_pl, clr); 744 return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(mogcr), mogcr_pl); 745 } 746 747 static int mlxsw_sp1_ptp_mtpppc_set(struct mlxsw_sp *mlxsw_sp, 748 u16 ing_types, u16 egr_types) 749 { 750 char mtpppc_pl[MLXSW_REG_MTPPPC_LEN]; 751 752 mlxsw_reg_mtpppc_pack(mtpppc_pl, ing_types, egr_types); 753 return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(mtpppc), mtpppc_pl); 754 } 755 756 struct mlxsw_sp1_ptp_shaper_params { 757 u32 ethtool_speed; 758 enum mlxsw_reg_qpsc_port_speed port_speed; 759 u8 shaper_time_exp; 760 u8 shaper_time_mantissa; 761 u8 shaper_inc; 762 u8 shaper_bs; 763 u8 port_to_shaper_credits; 764 int ing_timestamp_inc; 765 int egr_timestamp_inc; 766 }; 767 768 static const struct mlxsw_sp1_ptp_shaper_params 769 mlxsw_sp1_ptp_shaper_params[] = { 770 { 771 .ethtool_speed = SPEED_100, 772 .port_speed = MLXSW_REG_QPSC_PORT_SPEED_100M, 773 .shaper_time_exp = 4, 774 .shaper_time_mantissa = 12, 775 .shaper_inc = 9, 776 .shaper_bs = 1, 777 .port_to_shaper_credits = 1, 778 .ing_timestamp_inc = -313, 779 .egr_timestamp_inc = 313, 780 }, 781 { 782 .ethtool_speed = SPEED_1000, 783 .port_speed = MLXSW_REG_QPSC_PORT_SPEED_1G, 784 .shaper_time_exp = 0, 785 .shaper_time_mantissa = 12, 786 .shaper_inc = 6, 787 .shaper_bs = 0, 788 .port_to_shaper_credits = 1, 789 .ing_timestamp_inc = -35, 790 .egr_timestamp_inc = 35, 791 }, 792 { 793 .ethtool_speed = SPEED_10000, 794 .port_speed = MLXSW_REG_QPSC_PORT_SPEED_10G, 795 .shaper_time_exp = 0, 796 .shaper_time_mantissa = 2, 797 .shaper_inc = 14, 798 .shaper_bs = 1, 799 .port_to_shaper_credits = 1, 800 .ing_timestamp_inc = -11, 801 .egr_timestamp_inc = 11, 802 }, 803 { 804 .ethtool_speed = SPEED_25000, 805 .port_speed = MLXSW_REG_QPSC_PORT_SPEED_25G, 806 .shaper_time_exp = 0, 807 .shaper_time_mantissa = 0, 808 .shaper_inc = 11, 809 .shaper_bs = 1, 810 .port_to_shaper_credits = 1, 811 .ing_timestamp_inc = -14, 812 .egr_timestamp_inc = 14, 813 }, 814 }; 815 816 #define MLXSW_SP1_PTP_SHAPER_PARAMS_LEN ARRAY_SIZE(mlxsw_sp1_ptp_shaper_params) 817 818 static int mlxsw_sp1_ptp_shaper_params_set(struct mlxsw_sp *mlxsw_sp) 819 { 820 const struct mlxsw_sp1_ptp_shaper_params *params; 821 char qpsc_pl[MLXSW_REG_QPSC_LEN]; 822 int i, err; 823 824 for (i = 0; i < MLXSW_SP1_PTP_SHAPER_PARAMS_LEN; i++) { 825 params = &mlxsw_sp1_ptp_shaper_params[i]; 826 mlxsw_reg_qpsc_pack(qpsc_pl, params->port_speed, 827 params->shaper_time_exp, 828 params->shaper_time_mantissa, 829 params->shaper_inc, params->shaper_bs, 830 params->port_to_shaper_credits, 831 params->ing_timestamp_inc, 832 params->egr_timestamp_inc); 833 err = mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(qpsc), qpsc_pl); 834 if (err) 835 return err; 836 } 837 838 return 0; 839 } 840 841 struct mlxsw_sp_ptp_state *mlxsw_sp1_ptp_init(struct mlxsw_sp *mlxsw_sp) 842 { 843 struct mlxsw_sp_ptp_state *ptp_state; 844 u16 message_type; 845 int err; 846 847 err = mlxsw_sp1_ptp_shaper_params_set(mlxsw_sp); 848 if (err) 849 return ERR_PTR(err); 850 851 ptp_state = kzalloc(sizeof(*ptp_state), GFP_KERNEL); 852 if (!ptp_state) 853 return ERR_PTR(-ENOMEM); 854 ptp_state->mlxsw_sp = mlxsw_sp; 855 856 spin_lock_init(&ptp_state->unmatched_lock); 857 858 err = rhashtable_init(&ptp_state->unmatched_ht, 859 &mlxsw_sp1_ptp_unmatched_ht_params); 860 if (err) 861 goto err_hashtable_init; 862 863 /* Delive these message types as PTP0. */ 864 message_type = BIT(MLXSW_SP_PTP_MESSAGE_TYPE_SYNC) | 865 BIT(MLXSW_SP_PTP_MESSAGE_TYPE_DELAY_REQ) | 866 BIT(MLXSW_SP_PTP_MESSAGE_TYPE_PDELAY_REQ) | 867 BIT(MLXSW_SP_PTP_MESSAGE_TYPE_PDELAY_RESP); 868 err = mlxsw_sp_ptp_mtptpt_set(mlxsw_sp, MLXSW_REG_MTPTPT_TRAP_ID_PTP0, 869 message_type); 870 if (err) 871 goto err_mtptpt_set; 872 873 /* Everything else is PTP1. */ 874 message_type = ~message_type; 875 err = mlxsw_sp_ptp_mtptpt_set(mlxsw_sp, MLXSW_REG_MTPTPT_TRAP_ID_PTP1, 876 message_type); 877 if (err) 878 goto err_mtptpt1_set; 879 880 err = mlxsw_sp1_ptp_set_fifo_clr_on_trap(mlxsw_sp, true); 881 if (err) 882 goto err_fifo_clr; 883 884 INIT_DELAYED_WORK(&ptp_state->ht_gc_dw, mlxsw_sp1_ptp_ht_gc); 885 mlxsw_core_schedule_dw(&ptp_state->ht_gc_dw, 886 MLXSW_SP1_PTP_HT_GC_INTERVAL); 887 return ptp_state; 888 889 err_fifo_clr: 890 mlxsw_sp_ptp_mtptpt_set(mlxsw_sp, MLXSW_REG_MTPTPT_TRAP_ID_PTP1, 0); 891 err_mtptpt1_set: 892 mlxsw_sp_ptp_mtptpt_set(mlxsw_sp, MLXSW_REG_MTPTPT_TRAP_ID_PTP0, 0); 893 err_mtptpt_set: 894 rhashtable_destroy(&ptp_state->unmatched_ht); 895 err_hashtable_init: 896 kfree(ptp_state); 897 return ERR_PTR(err); 898 } 899 900 void mlxsw_sp1_ptp_fini(struct mlxsw_sp_ptp_state *ptp_state) 901 { 902 struct mlxsw_sp *mlxsw_sp = ptp_state->mlxsw_sp; 903 904 cancel_delayed_work_sync(&ptp_state->ht_gc_dw); 905 mlxsw_sp1_ptp_mtpppc_set(mlxsw_sp, 0, 0); 906 mlxsw_sp1_ptp_set_fifo_clr_on_trap(mlxsw_sp, false); 907 mlxsw_sp_ptp_mtptpt_set(mlxsw_sp, MLXSW_REG_MTPTPT_TRAP_ID_PTP1, 0); 908 mlxsw_sp_ptp_mtptpt_set(mlxsw_sp, MLXSW_REG_MTPTPT_TRAP_ID_PTP0, 0); 909 rhashtable_free_and_destroy(&ptp_state->unmatched_ht, 910 &mlxsw_sp1_ptp_unmatched_free_fn, NULL); 911 kfree(ptp_state); 912 } 913 914 int mlxsw_sp1_ptp_hwtstamp_get(struct mlxsw_sp_port *mlxsw_sp_port, 915 struct hwtstamp_config *config) 916 { 917 *config = mlxsw_sp_port->ptp.hwtstamp_config; 918 return 0; 919 } 920 921 static int mlxsw_sp_ptp_get_message_types(const struct hwtstamp_config *config, 922 u16 *p_ing_types, u16 *p_egr_types, 923 enum hwtstamp_rx_filters *p_rx_filter) 924 { 925 enum hwtstamp_rx_filters rx_filter = config->rx_filter; 926 enum hwtstamp_tx_types tx_type = config->tx_type; 927 u16 ing_types = 0x00; 928 u16 egr_types = 0x00; 929 930 switch (tx_type) { 931 case HWTSTAMP_TX_OFF: 932 egr_types = 0x00; 933 break; 934 case HWTSTAMP_TX_ON: 935 egr_types = 0xff; 936 break; 937 case HWTSTAMP_TX_ONESTEP_SYNC: 938 return -ERANGE; 939 } 940 941 switch (rx_filter) { 942 case HWTSTAMP_FILTER_NONE: 943 ing_types = 0x00; 944 break; 945 case HWTSTAMP_FILTER_PTP_V1_L4_SYNC: 946 case HWTSTAMP_FILTER_PTP_V2_L4_SYNC: 947 case HWTSTAMP_FILTER_PTP_V2_L2_SYNC: 948 case HWTSTAMP_FILTER_PTP_V2_SYNC: 949 ing_types = 0x01; 950 break; 951 case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ: 952 case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ: 953 case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ: 954 case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ: 955 ing_types = 0x02; 956 break; 957 case HWTSTAMP_FILTER_PTP_V1_L4_EVENT: 958 case HWTSTAMP_FILTER_PTP_V2_L4_EVENT: 959 case HWTSTAMP_FILTER_PTP_V2_L2_EVENT: 960 case HWTSTAMP_FILTER_PTP_V2_EVENT: 961 ing_types = 0x0f; 962 break; 963 case HWTSTAMP_FILTER_ALL: 964 ing_types = 0xff; 965 break; 966 case HWTSTAMP_FILTER_SOME: 967 case HWTSTAMP_FILTER_NTP_ALL: 968 return -ERANGE; 969 } 970 971 *p_ing_types = ing_types; 972 *p_egr_types = egr_types; 973 *p_rx_filter = rx_filter; 974 return 0; 975 } 976 977 static int mlxsw_sp1_ptp_mtpppc_update(struct mlxsw_sp_port *mlxsw_sp_port, 978 u16 ing_types, u16 egr_types) 979 { 980 struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp; 981 struct mlxsw_sp_port *tmp; 982 int i; 983 984 /* MTPPPC configures timestamping globally, not per port. Find the 985 * configuration that contains all configured timestamping requests. 986 */ 987 for (i = 1; i < mlxsw_core_max_ports(mlxsw_sp->core); i++) { 988 tmp = mlxsw_sp->ports[i]; 989 if (tmp && tmp != mlxsw_sp_port) { 990 ing_types |= tmp->ptp.ing_types; 991 egr_types |= tmp->ptp.egr_types; 992 } 993 } 994 995 return mlxsw_sp1_ptp_mtpppc_set(mlxsw_sp_port->mlxsw_sp, 996 ing_types, egr_types); 997 } 998 999 static bool mlxsw_sp1_ptp_hwtstamp_enabled(struct mlxsw_sp_port *mlxsw_sp_port) 1000 { 1001 return mlxsw_sp_port->ptp.ing_types || mlxsw_sp_port->ptp.egr_types; 1002 } 1003 1004 static int 1005 mlxsw_sp1_ptp_port_shaper_set(struct mlxsw_sp_port *mlxsw_sp_port, bool enable) 1006 { 1007 struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp; 1008 char qeec_pl[MLXSW_REG_QEEC_LEN]; 1009 1010 mlxsw_reg_qeec_ptps_pack(qeec_pl, mlxsw_sp_port->local_port, enable); 1011 return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(qeec), qeec_pl); 1012 } 1013 1014 static int mlxsw_sp1_ptp_port_shaper_check(struct mlxsw_sp_port *mlxsw_sp_port) 1015 { 1016 const struct mlxsw_sp_port_type_speed_ops *port_type_speed_ops; 1017 struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp; 1018 char ptys_pl[MLXSW_REG_PTYS_LEN]; 1019 u32 eth_proto_oper, speed; 1020 bool ptps = false; 1021 int err, i; 1022 1023 if (!mlxsw_sp1_ptp_hwtstamp_enabled(mlxsw_sp_port)) 1024 return mlxsw_sp1_ptp_port_shaper_set(mlxsw_sp_port, false); 1025 1026 port_type_speed_ops = mlxsw_sp->port_type_speed_ops; 1027 port_type_speed_ops->reg_ptys_eth_pack(mlxsw_sp, ptys_pl, 1028 mlxsw_sp_port->local_port, 0, 1029 false); 1030 err = mlxsw_reg_query(mlxsw_sp->core, MLXSW_REG(ptys), ptys_pl); 1031 if (err) 1032 return err; 1033 port_type_speed_ops->reg_ptys_eth_unpack(mlxsw_sp, ptys_pl, NULL, NULL, 1034 ð_proto_oper); 1035 1036 speed = port_type_speed_ops->from_ptys_speed(mlxsw_sp, eth_proto_oper); 1037 for (i = 0; i < MLXSW_SP1_PTP_SHAPER_PARAMS_LEN; i++) { 1038 if (mlxsw_sp1_ptp_shaper_params[i].ethtool_speed == speed) { 1039 ptps = true; 1040 break; 1041 } 1042 } 1043 1044 return mlxsw_sp1_ptp_port_shaper_set(mlxsw_sp_port, ptps); 1045 } 1046 1047 void mlxsw_sp1_ptp_shaper_work(struct work_struct *work) 1048 { 1049 struct delayed_work *dwork = to_delayed_work(work); 1050 struct mlxsw_sp_port *mlxsw_sp_port; 1051 int err; 1052 1053 mlxsw_sp_port = container_of(dwork, struct mlxsw_sp_port, 1054 ptp.shaper_dw); 1055 1056 if (!mlxsw_sp1_ptp_hwtstamp_enabled(mlxsw_sp_port)) 1057 return; 1058 1059 err = mlxsw_sp1_ptp_port_shaper_check(mlxsw_sp_port); 1060 if (err) 1061 netdev_err(mlxsw_sp_port->dev, "Failed to set up PTP shaper\n"); 1062 } 1063 1064 int mlxsw_sp1_ptp_hwtstamp_set(struct mlxsw_sp_port *mlxsw_sp_port, 1065 struct hwtstamp_config *config) 1066 { 1067 enum hwtstamp_rx_filters rx_filter; 1068 u16 ing_types; 1069 u16 egr_types; 1070 int err; 1071 1072 err = mlxsw_sp_ptp_get_message_types(config, &ing_types, &egr_types, 1073 &rx_filter); 1074 if (err) 1075 return err; 1076 1077 err = mlxsw_sp1_ptp_mtpppc_update(mlxsw_sp_port, ing_types, egr_types); 1078 if (err) 1079 return err; 1080 1081 mlxsw_sp_port->ptp.hwtstamp_config = *config; 1082 mlxsw_sp_port->ptp.ing_types = ing_types; 1083 mlxsw_sp_port->ptp.egr_types = egr_types; 1084 1085 err = mlxsw_sp1_ptp_port_shaper_check(mlxsw_sp_port); 1086 if (err) 1087 return err; 1088 1089 /* Notify the ioctl caller what we are actually timestamping. */ 1090 config->rx_filter = rx_filter; 1091 1092 return 0; 1093 } 1094 1095 int mlxsw_sp1_ptp_get_ts_info(struct mlxsw_sp *mlxsw_sp, 1096 struct ethtool_ts_info *info) 1097 { 1098 info->phc_index = ptp_clock_index(mlxsw_sp->clock->ptp); 1099 1100 info->so_timestamping = SOF_TIMESTAMPING_TX_HARDWARE | 1101 SOF_TIMESTAMPING_RX_HARDWARE | 1102 SOF_TIMESTAMPING_RAW_HARDWARE; 1103 1104 info->tx_types = BIT(HWTSTAMP_TX_OFF) | 1105 BIT(HWTSTAMP_TX_ON); 1106 1107 info->rx_filters = BIT(HWTSTAMP_FILTER_NONE) | 1108 BIT(HWTSTAMP_FILTER_ALL); 1109 1110 return 0; 1111 } 1112