1 // SPDX-License-Identifier: GPL-2.0+ 2 3 #include <linux/ptp_classify.h> 4 5 #include "lan966x_main.h" 6 7 #define LAN966X_MAX_PTP_ID 512 8 9 /* Represents 1ppm adjustment in 2^59 format with 6.037735849ns as reference 10 * The value is calculated as following: (1/1000000)/((2^-59)/6.037735849) 11 */ 12 #define LAN966X_1PPM_FORMAT 3480517749723LL 13 14 /* Represents 1ppb adjustment in 2^29 format with 6.037735849ns as reference 15 * The value is calculated as following: (1/1000000000)/((2^59)/6.037735849) 16 */ 17 #define LAN966X_1PPB_FORMAT 3480517749LL 18 19 #define TOD_ACC_PIN 0x7 20 21 enum { 22 PTP_PIN_ACTION_IDLE = 0, 23 PTP_PIN_ACTION_LOAD, 24 PTP_PIN_ACTION_SAVE, 25 PTP_PIN_ACTION_CLOCK, 26 PTP_PIN_ACTION_DELTA, 27 PTP_PIN_ACTION_TOD 28 }; 29 30 static u64 lan966x_ptp_get_nominal_value(void) 31 { 32 /* This is the default value that for each system clock, the time of day 33 * is increased. It has the format 5.59 nanosecond. 34 */ 35 return 0x304d4873ecade305; 36 } 37 38 int lan966x_ptp_hwtstamp_set(struct lan966x_port *port, struct ifreq *ifr) 39 { 40 struct lan966x *lan966x = port->lan966x; 41 struct hwtstamp_config cfg; 42 struct lan966x_phc *phc; 43 44 /* For now don't allow to run ptp on ports that are part of a bridge, 45 * because in case of transparent clock the HW will still forward the 46 * frames, so there would be duplicate frames 47 */ 48 if (lan966x->bridge_mask & BIT(port->chip_port)) 49 return -EINVAL; 50 51 if (copy_from_user(&cfg, ifr->ifr_data, sizeof(cfg))) 52 return -EFAULT; 53 54 switch (cfg.tx_type) { 55 case HWTSTAMP_TX_ON: 56 port->ptp_cmd = IFH_REW_OP_TWO_STEP_PTP; 57 break; 58 case HWTSTAMP_TX_ONESTEP_SYNC: 59 port->ptp_cmd = IFH_REW_OP_ONE_STEP_PTP; 60 break; 61 case HWTSTAMP_TX_OFF: 62 port->ptp_cmd = IFH_REW_OP_NOOP; 63 break; 64 default: 65 return -ERANGE; 66 } 67 68 switch (cfg.rx_filter) { 69 case HWTSTAMP_FILTER_NONE: 70 break; 71 case HWTSTAMP_FILTER_ALL: 72 case HWTSTAMP_FILTER_PTP_V1_L4_EVENT: 73 case HWTSTAMP_FILTER_PTP_V1_L4_SYNC: 74 case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ: 75 case HWTSTAMP_FILTER_PTP_V2_L4_EVENT: 76 case HWTSTAMP_FILTER_PTP_V2_L4_SYNC: 77 case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ: 78 case HWTSTAMP_FILTER_PTP_V2_L2_EVENT: 79 case HWTSTAMP_FILTER_PTP_V2_L2_SYNC: 80 case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ: 81 case HWTSTAMP_FILTER_PTP_V2_EVENT: 82 case HWTSTAMP_FILTER_PTP_V2_SYNC: 83 case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ: 84 case HWTSTAMP_FILTER_NTP_ALL: 85 cfg.rx_filter = HWTSTAMP_FILTER_ALL; 86 break; 87 default: 88 return -ERANGE; 89 } 90 91 /* Commit back the result & save it */ 92 mutex_lock(&lan966x->ptp_lock); 93 phc = &lan966x->phc[LAN966X_PHC_PORT]; 94 memcpy(&phc->hwtstamp_config, &cfg, sizeof(cfg)); 95 mutex_unlock(&lan966x->ptp_lock); 96 97 return copy_to_user(ifr->ifr_data, &cfg, sizeof(cfg)) ? -EFAULT : 0; 98 } 99 100 int lan966x_ptp_hwtstamp_get(struct lan966x_port *port, struct ifreq *ifr) 101 { 102 struct lan966x *lan966x = port->lan966x; 103 struct lan966x_phc *phc; 104 105 phc = &lan966x->phc[LAN966X_PHC_PORT]; 106 return copy_to_user(ifr->ifr_data, &phc->hwtstamp_config, 107 sizeof(phc->hwtstamp_config)) ? -EFAULT : 0; 108 } 109 110 static int lan966x_ptp_classify(struct lan966x_port *port, struct sk_buff *skb) 111 { 112 struct ptp_header *header; 113 u8 msgtype; 114 int type; 115 116 if (port->ptp_cmd == IFH_REW_OP_NOOP) 117 return IFH_REW_OP_NOOP; 118 119 type = ptp_classify_raw(skb); 120 if (type == PTP_CLASS_NONE) 121 return IFH_REW_OP_NOOP; 122 123 header = ptp_parse_header(skb, type); 124 if (!header) 125 return IFH_REW_OP_NOOP; 126 127 if (port->ptp_cmd == IFH_REW_OP_TWO_STEP_PTP) 128 return IFH_REW_OP_TWO_STEP_PTP; 129 130 /* If it is sync and run 1 step then set the correct operation, 131 * otherwise run as 2 step 132 */ 133 msgtype = ptp_get_msgtype(header, type); 134 if ((msgtype & 0xf) == 0) 135 return IFH_REW_OP_ONE_STEP_PTP; 136 137 return IFH_REW_OP_TWO_STEP_PTP; 138 } 139 140 static void lan966x_ptp_txtstamp_old_release(struct lan966x_port *port) 141 { 142 struct sk_buff *skb, *skb_tmp; 143 unsigned long flags; 144 145 spin_lock_irqsave(&port->tx_skbs.lock, flags); 146 skb_queue_walk_safe(&port->tx_skbs, skb, skb_tmp) { 147 if time_after(LAN966X_SKB_CB(skb)->jiffies + LAN966X_PTP_TIMEOUT, 148 jiffies) 149 break; 150 151 __skb_unlink(skb, &port->tx_skbs); 152 dev_kfree_skb_any(skb); 153 } 154 spin_unlock_irqrestore(&port->tx_skbs.lock, flags); 155 } 156 157 int lan966x_ptp_txtstamp_request(struct lan966x_port *port, 158 struct sk_buff *skb) 159 { 160 struct lan966x *lan966x = port->lan966x; 161 unsigned long flags; 162 u8 rew_op; 163 164 rew_op = lan966x_ptp_classify(port, skb); 165 LAN966X_SKB_CB(skb)->rew_op = rew_op; 166 167 if (rew_op != IFH_REW_OP_TWO_STEP_PTP) 168 return 0; 169 170 lan966x_ptp_txtstamp_old_release(port); 171 172 spin_lock_irqsave(&lan966x->ptp_ts_id_lock, flags); 173 if (lan966x->ptp_skbs == LAN966X_MAX_PTP_ID) { 174 spin_unlock_irqrestore(&lan966x->ptp_ts_id_lock, flags); 175 return -EBUSY; 176 } 177 178 skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS; 179 180 skb_queue_tail(&port->tx_skbs, skb); 181 LAN966X_SKB_CB(skb)->ts_id = port->ts_id; 182 LAN966X_SKB_CB(skb)->jiffies = jiffies; 183 184 lan966x->ptp_skbs++; 185 port->ts_id++; 186 if (port->ts_id == LAN966X_MAX_PTP_ID) 187 port->ts_id = 0; 188 189 spin_unlock_irqrestore(&lan966x->ptp_ts_id_lock, flags); 190 191 return 0; 192 } 193 194 void lan966x_ptp_txtstamp_release(struct lan966x_port *port, 195 struct sk_buff *skb) 196 { 197 struct lan966x *lan966x = port->lan966x; 198 unsigned long flags; 199 200 spin_lock_irqsave(&lan966x->ptp_ts_id_lock, flags); 201 port->ts_id--; 202 lan966x->ptp_skbs--; 203 skb_unlink(skb, &port->tx_skbs); 204 spin_unlock_irqrestore(&lan966x->ptp_ts_id_lock, flags); 205 } 206 207 static void lan966x_get_hwtimestamp(struct lan966x *lan966x, 208 struct timespec64 *ts, 209 u32 nsec) 210 { 211 /* Read current PTP time to get seconds */ 212 unsigned long flags; 213 u32 curr_nsec; 214 215 spin_lock_irqsave(&lan966x->ptp_clock_lock, flags); 216 217 lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_SAVE) | 218 PTP_PIN_CFG_PIN_DOM_SET(LAN966X_PHC_PORT) | 219 PTP_PIN_CFG_PIN_SYNC_SET(0), 220 PTP_PIN_CFG_PIN_ACTION | 221 PTP_PIN_CFG_PIN_DOM | 222 PTP_PIN_CFG_PIN_SYNC, 223 lan966x, PTP_PIN_CFG(TOD_ACC_PIN)); 224 225 ts->tv_sec = lan_rd(lan966x, PTP_TOD_SEC_LSB(TOD_ACC_PIN)); 226 curr_nsec = lan_rd(lan966x, PTP_TOD_NSEC(TOD_ACC_PIN)); 227 228 ts->tv_nsec = nsec; 229 230 /* Sec has incremented since the ts was registered */ 231 if (curr_nsec < nsec) 232 ts->tv_sec--; 233 234 spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags); 235 } 236 237 irqreturn_t lan966x_ptp_irq_handler(int irq, void *args) 238 { 239 int budget = LAN966X_MAX_PTP_ID; 240 struct lan966x *lan966x = args; 241 242 while (budget--) { 243 struct sk_buff *skb, *skb_tmp, *skb_match = NULL; 244 struct skb_shared_hwtstamps shhwtstamps; 245 struct lan966x_port *port; 246 struct timespec64 ts; 247 unsigned long flags; 248 u32 val, id, txport; 249 u32 delay; 250 251 val = lan_rd(lan966x, PTP_TWOSTEP_CTRL); 252 253 /* Check if a timestamp can be retrieved */ 254 if (!(val & PTP_TWOSTEP_CTRL_VLD)) 255 break; 256 257 WARN_ON(val & PTP_TWOSTEP_CTRL_OVFL); 258 259 if (!(val & PTP_TWOSTEP_CTRL_STAMP_TX)) 260 continue; 261 262 /* Retrieve the ts Tx port */ 263 txport = PTP_TWOSTEP_CTRL_STAMP_PORT_GET(val); 264 265 /* Retrieve its associated skb */ 266 port = lan966x->ports[txport]; 267 268 /* Retrieve the delay */ 269 delay = lan_rd(lan966x, PTP_TWOSTEP_STAMP); 270 delay = PTP_TWOSTEP_STAMP_STAMP_NSEC_GET(delay); 271 272 /* Get next timestamp from fifo, which needs to be the 273 * rx timestamp which represents the id of the frame 274 */ 275 lan_rmw(PTP_TWOSTEP_CTRL_NXT_SET(1), 276 PTP_TWOSTEP_CTRL_NXT, 277 lan966x, PTP_TWOSTEP_CTRL); 278 279 val = lan_rd(lan966x, PTP_TWOSTEP_CTRL); 280 281 /* Check if a timestamp can be retried */ 282 if (!(val & PTP_TWOSTEP_CTRL_VLD)) 283 break; 284 285 /* Read RX timestamping to get the ID */ 286 id = lan_rd(lan966x, PTP_TWOSTEP_STAMP); 287 288 spin_lock_irqsave(&port->tx_skbs.lock, flags); 289 skb_queue_walk_safe(&port->tx_skbs, skb, skb_tmp) { 290 if (LAN966X_SKB_CB(skb)->ts_id != id) 291 continue; 292 293 __skb_unlink(skb, &port->tx_skbs); 294 skb_match = skb; 295 break; 296 } 297 spin_unlock_irqrestore(&port->tx_skbs.lock, flags); 298 299 /* Next ts */ 300 lan_rmw(PTP_TWOSTEP_CTRL_NXT_SET(1), 301 PTP_TWOSTEP_CTRL_NXT, 302 lan966x, PTP_TWOSTEP_CTRL); 303 304 if (WARN_ON(!skb_match)) 305 continue; 306 307 spin_lock(&lan966x->ptp_ts_id_lock); 308 lan966x->ptp_skbs--; 309 spin_unlock(&lan966x->ptp_ts_id_lock); 310 311 /* Get the h/w timestamp */ 312 lan966x_get_hwtimestamp(lan966x, &ts, delay); 313 314 /* Set the timestamp into the skb */ 315 shhwtstamps.hwtstamp = ktime_set(ts.tv_sec, ts.tv_nsec); 316 skb_tstamp_tx(skb_match, &shhwtstamps); 317 318 dev_kfree_skb_any(skb_match); 319 } 320 321 return IRQ_HANDLED; 322 } 323 324 static int lan966x_ptp_adjfine(struct ptp_clock_info *ptp, long scaled_ppm) 325 { 326 struct lan966x_phc *phc = container_of(ptp, struct lan966x_phc, info); 327 struct lan966x *lan966x = phc->lan966x; 328 unsigned long flags; 329 bool neg_adj = 0; 330 u64 tod_inc; 331 u64 ref; 332 333 if (!scaled_ppm) 334 return 0; 335 336 if (scaled_ppm < 0) { 337 neg_adj = 1; 338 scaled_ppm = -scaled_ppm; 339 } 340 341 tod_inc = lan966x_ptp_get_nominal_value(); 342 343 /* The multiplication is split in 2 separate additions because of 344 * overflow issues. If scaled_ppm with 16bit fractional part was bigger 345 * than 20ppm then we got overflow. 346 */ 347 ref = LAN966X_1PPM_FORMAT * (scaled_ppm >> 16); 348 ref += (LAN966X_1PPM_FORMAT * (0xffff & scaled_ppm)) >> 16; 349 tod_inc = neg_adj ? tod_inc - ref : tod_inc + ref; 350 351 spin_lock_irqsave(&lan966x->ptp_clock_lock, flags); 352 353 lan_rmw(PTP_DOM_CFG_CLKCFG_DIS_SET(1 << BIT(phc->index)), 354 PTP_DOM_CFG_CLKCFG_DIS, 355 lan966x, PTP_DOM_CFG); 356 357 lan_wr((u32)tod_inc & 0xFFFFFFFF, lan966x, 358 PTP_CLK_PER_CFG(phc->index, 0)); 359 lan_wr((u32)(tod_inc >> 32), lan966x, 360 PTP_CLK_PER_CFG(phc->index, 1)); 361 362 lan_rmw(PTP_DOM_CFG_CLKCFG_DIS_SET(0), 363 PTP_DOM_CFG_CLKCFG_DIS, 364 lan966x, PTP_DOM_CFG); 365 366 spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags); 367 368 return 0; 369 } 370 371 static int lan966x_ptp_settime64(struct ptp_clock_info *ptp, 372 const struct timespec64 *ts) 373 { 374 struct lan966x_phc *phc = container_of(ptp, struct lan966x_phc, info); 375 struct lan966x *lan966x = phc->lan966x; 376 unsigned long flags; 377 378 spin_lock_irqsave(&lan966x->ptp_clock_lock, flags); 379 380 /* Must be in IDLE mode before the time can be loaded */ 381 lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_IDLE) | 382 PTP_PIN_CFG_PIN_DOM_SET(phc->index) | 383 PTP_PIN_CFG_PIN_SYNC_SET(0), 384 PTP_PIN_CFG_PIN_ACTION | 385 PTP_PIN_CFG_PIN_DOM | 386 PTP_PIN_CFG_PIN_SYNC, 387 lan966x, PTP_PIN_CFG(TOD_ACC_PIN)); 388 389 /* Set new value */ 390 lan_wr(PTP_TOD_SEC_MSB_TOD_SEC_MSB_SET(upper_32_bits(ts->tv_sec)), 391 lan966x, PTP_TOD_SEC_MSB(TOD_ACC_PIN)); 392 lan_wr(lower_32_bits(ts->tv_sec), 393 lan966x, PTP_TOD_SEC_LSB(TOD_ACC_PIN)); 394 lan_wr(ts->tv_nsec, lan966x, PTP_TOD_NSEC(TOD_ACC_PIN)); 395 396 /* Apply new values */ 397 lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_LOAD) | 398 PTP_PIN_CFG_PIN_DOM_SET(phc->index) | 399 PTP_PIN_CFG_PIN_SYNC_SET(0), 400 PTP_PIN_CFG_PIN_ACTION | 401 PTP_PIN_CFG_PIN_DOM | 402 PTP_PIN_CFG_PIN_SYNC, 403 lan966x, PTP_PIN_CFG(TOD_ACC_PIN)); 404 405 spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags); 406 407 return 0; 408 } 409 410 static int lan966x_ptp_gettime64(struct ptp_clock_info *ptp, 411 struct timespec64 *ts) 412 { 413 struct lan966x_phc *phc = container_of(ptp, struct lan966x_phc, info); 414 struct lan966x *lan966x = phc->lan966x; 415 unsigned long flags; 416 time64_t s; 417 s64 ns; 418 419 spin_lock_irqsave(&lan966x->ptp_clock_lock, flags); 420 421 lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_SAVE) | 422 PTP_PIN_CFG_PIN_DOM_SET(phc->index) | 423 PTP_PIN_CFG_PIN_SYNC_SET(0), 424 PTP_PIN_CFG_PIN_ACTION | 425 PTP_PIN_CFG_PIN_DOM | 426 PTP_PIN_CFG_PIN_SYNC, 427 lan966x, PTP_PIN_CFG(TOD_ACC_PIN)); 428 429 s = lan_rd(lan966x, PTP_TOD_SEC_MSB(TOD_ACC_PIN)); 430 s <<= 32; 431 s |= lan_rd(lan966x, PTP_TOD_SEC_LSB(TOD_ACC_PIN)); 432 ns = lan_rd(lan966x, PTP_TOD_NSEC(TOD_ACC_PIN)); 433 ns &= PTP_TOD_NSEC_TOD_NSEC; 434 435 spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags); 436 437 /* Deal with negative values */ 438 if ((ns & 0xFFFFFFF0) == 0x3FFFFFF0) { 439 s--; 440 ns &= 0xf; 441 ns += 999999984; 442 } 443 444 set_normalized_timespec64(ts, s, ns); 445 return 0; 446 } 447 448 static int lan966x_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta) 449 { 450 struct lan966x_phc *phc = container_of(ptp, struct lan966x_phc, info); 451 struct lan966x *lan966x = phc->lan966x; 452 453 if (delta > -(NSEC_PER_SEC / 2) && delta < (NSEC_PER_SEC / 2)) { 454 unsigned long flags; 455 456 spin_lock_irqsave(&lan966x->ptp_clock_lock, flags); 457 458 /* Must be in IDLE mode before the time can be loaded */ 459 lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_IDLE) | 460 PTP_PIN_CFG_PIN_DOM_SET(phc->index) | 461 PTP_PIN_CFG_PIN_SYNC_SET(0), 462 PTP_PIN_CFG_PIN_ACTION | 463 PTP_PIN_CFG_PIN_DOM | 464 PTP_PIN_CFG_PIN_SYNC, 465 lan966x, PTP_PIN_CFG(TOD_ACC_PIN)); 466 467 lan_wr(PTP_TOD_NSEC_TOD_NSEC_SET(delta), 468 lan966x, PTP_TOD_NSEC(TOD_ACC_PIN)); 469 470 /* Adjust time with the value of PTP_TOD_NSEC */ 471 lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_DELTA) | 472 PTP_PIN_CFG_PIN_DOM_SET(phc->index) | 473 PTP_PIN_CFG_PIN_SYNC_SET(0), 474 PTP_PIN_CFG_PIN_ACTION | 475 PTP_PIN_CFG_PIN_DOM | 476 PTP_PIN_CFG_PIN_SYNC, 477 lan966x, PTP_PIN_CFG(TOD_ACC_PIN)); 478 479 spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags); 480 } else { 481 /* Fall back using lan966x_ptp_settime64 which is not exact */ 482 struct timespec64 ts; 483 u64 now; 484 485 lan966x_ptp_gettime64(ptp, &ts); 486 487 now = ktime_to_ns(timespec64_to_ktime(ts)); 488 ts = ns_to_timespec64(now + delta); 489 490 lan966x_ptp_settime64(ptp, &ts); 491 } 492 493 return 0; 494 } 495 496 static struct ptp_clock_info lan966x_ptp_clock_info = { 497 .owner = THIS_MODULE, 498 .name = "lan966x ptp", 499 .max_adj = 200000, 500 .gettime64 = lan966x_ptp_gettime64, 501 .settime64 = lan966x_ptp_settime64, 502 .adjtime = lan966x_ptp_adjtime, 503 .adjfine = lan966x_ptp_adjfine, 504 }; 505 506 static int lan966x_ptp_phc_init(struct lan966x *lan966x, 507 int index, 508 struct ptp_clock_info *clock_info) 509 { 510 struct lan966x_phc *phc = &lan966x->phc[index]; 511 512 phc->info = *clock_info; 513 phc->clock = ptp_clock_register(&phc->info, lan966x->dev); 514 if (IS_ERR(phc->clock)) 515 return PTR_ERR(phc->clock); 516 517 phc->index = index; 518 phc->lan966x = lan966x; 519 520 /* PTP Rx stamping is always enabled. */ 521 phc->hwtstamp_config.rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT; 522 523 return 0; 524 } 525 526 int lan966x_ptp_init(struct lan966x *lan966x) 527 { 528 u64 tod_adj = lan966x_ptp_get_nominal_value(); 529 struct lan966x_port *port; 530 int err, i; 531 532 if (!lan966x->ptp) 533 return 0; 534 535 for (i = 0; i < LAN966X_PHC_COUNT; ++i) { 536 err = lan966x_ptp_phc_init(lan966x, i, &lan966x_ptp_clock_info); 537 if (err) 538 return err; 539 } 540 541 spin_lock_init(&lan966x->ptp_clock_lock); 542 spin_lock_init(&lan966x->ptp_ts_id_lock); 543 mutex_init(&lan966x->ptp_lock); 544 545 /* Disable master counters */ 546 lan_wr(PTP_DOM_CFG_ENA_SET(0), lan966x, PTP_DOM_CFG); 547 548 /* Configure the nominal TOD increment per clock cycle */ 549 lan_rmw(PTP_DOM_CFG_CLKCFG_DIS_SET(0x7), 550 PTP_DOM_CFG_CLKCFG_DIS, 551 lan966x, PTP_DOM_CFG); 552 553 for (i = 0; i < LAN966X_PHC_COUNT; ++i) { 554 lan_wr((u32)tod_adj & 0xFFFFFFFF, lan966x, 555 PTP_CLK_PER_CFG(i, 0)); 556 lan_wr((u32)(tod_adj >> 32), lan966x, 557 PTP_CLK_PER_CFG(i, 1)); 558 } 559 560 lan_rmw(PTP_DOM_CFG_CLKCFG_DIS_SET(0), 561 PTP_DOM_CFG_CLKCFG_DIS, 562 lan966x, PTP_DOM_CFG); 563 564 /* Enable master counters */ 565 lan_wr(PTP_DOM_CFG_ENA_SET(0x7), lan966x, PTP_DOM_CFG); 566 567 for (i = 0; i < lan966x->num_phys_ports; i++) { 568 port = lan966x->ports[i]; 569 if (!port) 570 continue; 571 572 skb_queue_head_init(&port->tx_skbs); 573 } 574 575 return 0; 576 } 577 578 void lan966x_ptp_deinit(struct lan966x *lan966x) 579 { 580 struct lan966x_port *port; 581 int i; 582 583 for (i = 0; i < lan966x->num_phys_ports; i++) { 584 port = lan966x->ports[i]; 585 if (!port) 586 continue; 587 588 skb_queue_purge(&port->tx_skbs); 589 } 590 591 for (i = 0; i < LAN966X_PHC_COUNT; ++i) 592 ptp_clock_unregister(lan966x->phc[i].clock); 593 } 594 595 void lan966x_ptp_rxtstamp(struct lan966x *lan966x, struct sk_buff *skb, 596 u64 timestamp) 597 { 598 struct skb_shared_hwtstamps *shhwtstamps; 599 struct lan966x_phc *phc; 600 struct timespec64 ts; 601 u64 full_ts_in_ns; 602 603 if (!lan966x->ptp) 604 return; 605 606 phc = &lan966x->phc[LAN966X_PHC_PORT]; 607 lan966x_ptp_gettime64(&phc->info, &ts); 608 609 /* Drop the sub-ns precision */ 610 timestamp = timestamp >> 2; 611 if (ts.tv_nsec < timestamp) 612 ts.tv_sec--; 613 ts.tv_nsec = timestamp; 614 full_ts_in_ns = ktime_set(ts.tv_sec, ts.tv_nsec); 615 616 shhwtstamps = skb_hwtstamps(skb); 617 shhwtstamps->hwtstamp = full_ts_in_ns; 618 } 619