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 irqreturn_t lan966x_ptp_ext_irq_handler(int irq, void *args) 325 { 326 struct lan966x *lan966x = args; 327 struct lan966x_phc *phc; 328 unsigned long flags; 329 u64 time = 0; 330 time64_t s; 331 int pin, i; 332 s64 ns; 333 334 if (!(lan_rd(lan966x, PTP_PIN_INTR))) 335 return IRQ_NONE; 336 337 /* Go through all domains and see which pin generated the interrupt */ 338 for (i = 0; i < LAN966X_PHC_COUNT; ++i) { 339 struct ptp_clock_event ptp_event = {0}; 340 341 phc = &lan966x->phc[i]; 342 pin = ptp_find_pin_unlocked(phc->clock, PTP_PF_EXTTS, 0); 343 if (pin == -1) 344 continue; 345 346 if (!(lan_rd(lan966x, PTP_PIN_INTR) & BIT(pin))) 347 continue; 348 349 spin_lock_irqsave(&lan966x->ptp_clock_lock, flags); 350 351 /* Enable to get the new interrupt. 352 * By writing 1 it clears the bit 353 */ 354 lan_wr(BIT(pin), lan966x, PTP_PIN_INTR); 355 356 /* Get current time */ 357 s = lan_rd(lan966x, PTP_TOD_SEC_MSB(pin)); 358 s <<= 32; 359 s |= lan_rd(lan966x, PTP_TOD_SEC_LSB(pin)); 360 ns = lan_rd(lan966x, PTP_TOD_NSEC(pin)); 361 ns &= PTP_TOD_NSEC_TOD_NSEC; 362 363 spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags); 364 365 if ((ns & 0xFFFFFFF0) == 0x3FFFFFF0) { 366 s--; 367 ns &= 0xf; 368 ns += 999999984; 369 } 370 time = ktime_set(s, ns); 371 372 ptp_event.index = pin; 373 ptp_event.timestamp = time; 374 ptp_event.type = PTP_CLOCK_EXTTS; 375 ptp_clock_event(phc->clock, &ptp_event); 376 } 377 378 return IRQ_HANDLED; 379 } 380 381 static int lan966x_ptp_adjfine(struct ptp_clock_info *ptp, long scaled_ppm) 382 { 383 struct lan966x_phc *phc = container_of(ptp, struct lan966x_phc, info); 384 struct lan966x *lan966x = phc->lan966x; 385 unsigned long flags; 386 bool neg_adj = 0; 387 u64 tod_inc; 388 u64 ref; 389 390 if (!scaled_ppm) 391 return 0; 392 393 if (scaled_ppm < 0) { 394 neg_adj = 1; 395 scaled_ppm = -scaled_ppm; 396 } 397 398 tod_inc = lan966x_ptp_get_nominal_value(); 399 400 /* The multiplication is split in 2 separate additions because of 401 * overflow issues. If scaled_ppm with 16bit fractional part was bigger 402 * than 20ppm then we got overflow. 403 */ 404 ref = LAN966X_1PPM_FORMAT * (scaled_ppm >> 16); 405 ref += (LAN966X_1PPM_FORMAT * (0xffff & scaled_ppm)) >> 16; 406 tod_inc = neg_adj ? tod_inc - ref : tod_inc + ref; 407 408 spin_lock_irqsave(&lan966x->ptp_clock_lock, flags); 409 410 lan_rmw(PTP_DOM_CFG_CLKCFG_DIS_SET(1 << BIT(phc->index)), 411 PTP_DOM_CFG_CLKCFG_DIS, 412 lan966x, PTP_DOM_CFG); 413 414 lan_wr((u32)tod_inc & 0xFFFFFFFF, lan966x, 415 PTP_CLK_PER_CFG(phc->index, 0)); 416 lan_wr((u32)(tod_inc >> 32), lan966x, 417 PTP_CLK_PER_CFG(phc->index, 1)); 418 419 lan_rmw(PTP_DOM_CFG_CLKCFG_DIS_SET(0), 420 PTP_DOM_CFG_CLKCFG_DIS, 421 lan966x, PTP_DOM_CFG); 422 423 spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags); 424 425 return 0; 426 } 427 428 static int lan966x_ptp_settime64(struct ptp_clock_info *ptp, 429 const struct timespec64 *ts) 430 { 431 struct lan966x_phc *phc = container_of(ptp, struct lan966x_phc, info); 432 struct lan966x *lan966x = phc->lan966x; 433 unsigned long flags; 434 435 spin_lock_irqsave(&lan966x->ptp_clock_lock, flags); 436 437 /* Must be in IDLE mode before the time can be loaded */ 438 lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_IDLE) | 439 PTP_PIN_CFG_PIN_DOM_SET(phc->index) | 440 PTP_PIN_CFG_PIN_SYNC_SET(0), 441 PTP_PIN_CFG_PIN_ACTION | 442 PTP_PIN_CFG_PIN_DOM | 443 PTP_PIN_CFG_PIN_SYNC, 444 lan966x, PTP_PIN_CFG(TOD_ACC_PIN)); 445 446 /* Set new value */ 447 lan_wr(PTP_TOD_SEC_MSB_TOD_SEC_MSB_SET(upper_32_bits(ts->tv_sec)), 448 lan966x, PTP_TOD_SEC_MSB(TOD_ACC_PIN)); 449 lan_wr(lower_32_bits(ts->tv_sec), 450 lan966x, PTP_TOD_SEC_LSB(TOD_ACC_PIN)); 451 lan_wr(ts->tv_nsec, lan966x, PTP_TOD_NSEC(TOD_ACC_PIN)); 452 453 /* Apply new values */ 454 lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_LOAD) | 455 PTP_PIN_CFG_PIN_DOM_SET(phc->index) | 456 PTP_PIN_CFG_PIN_SYNC_SET(0), 457 PTP_PIN_CFG_PIN_ACTION | 458 PTP_PIN_CFG_PIN_DOM | 459 PTP_PIN_CFG_PIN_SYNC, 460 lan966x, PTP_PIN_CFG(TOD_ACC_PIN)); 461 462 spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags); 463 464 return 0; 465 } 466 467 static int lan966x_ptp_gettime64(struct ptp_clock_info *ptp, 468 struct timespec64 *ts) 469 { 470 struct lan966x_phc *phc = container_of(ptp, struct lan966x_phc, info); 471 struct lan966x *lan966x = phc->lan966x; 472 unsigned long flags; 473 time64_t s; 474 s64 ns; 475 476 spin_lock_irqsave(&lan966x->ptp_clock_lock, flags); 477 478 lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_SAVE) | 479 PTP_PIN_CFG_PIN_DOM_SET(phc->index) | 480 PTP_PIN_CFG_PIN_SYNC_SET(0), 481 PTP_PIN_CFG_PIN_ACTION | 482 PTP_PIN_CFG_PIN_DOM | 483 PTP_PIN_CFG_PIN_SYNC, 484 lan966x, PTP_PIN_CFG(TOD_ACC_PIN)); 485 486 s = lan_rd(lan966x, PTP_TOD_SEC_MSB(TOD_ACC_PIN)); 487 s <<= 32; 488 s |= lan_rd(lan966x, PTP_TOD_SEC_LSB(TOD_ACC_PIN)); 489 ns = lan_rd(lan966x, PTP_TOD_NSEC(TOD_ACC_PIN)); 490 ns &= PTP_TOD_NSEC_TOD_NSEC; 491 492 spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags); 493 494 /* Deal with negative values */ 495 if ((ns & 0xFFFFFFF0) == 0x3FFFFFF0) { 496 s--; 497 ns &= 0xf; 498 ns += 999999984; 499 } 500 501 set_normalized_timespec64(ts, s, ns); 502 return 0; 503 } 504 505 static int lan966x_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta) 506 { 507 struct lan966x_phc *phc = container_of(ptp, struct lan966x_phc, info); 508 struct lan966x *lan966x = phc->lan966x; 509 510 if (delta > -(NSEC_PER_SEC / 2) && delta < (NSEC_PER_SEC / 2)) { 511 unsigned long flags; 512 513 spin_lock_irqsave(&lan966x->ptp_clock_lock, flags); 514 515 /* Must be in IDLE mode before the time can be loaded */ 516 lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_IDLE) | 517 PTP_PIN_CFG_PIN_DOM_SET(phc->index) | 518 PTP_PIN_CFG_PIN_SYNC_SET(0), 519 PTP_PIN_CFG_PIN_ACTION | 520 PTP_PIN_CFG_PIN_DOM | 521 PTP_PIN_CFG_PIN_SYNC, 522 lan966x, PTP_PIN_CFG(TOD_ACC_PIN)); 523 524 lan_wr(PTP_TOD_NSEC_TOD_NSEC_SET(delta), 525 lan966x, PTP_TOD_NSEC(TOD_ACC_PIN)); 526 527 /* Adjust time with the value of PTP_TOD_NSEC */ 528 lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_DELTA) | 529 PTP_PIN_CFG_PIN_DOM_SET(phc->index) | 530 PTP_PIN_CFG_PIN_SYNC_SET(0), 531 PTP_PIN_CFG_PIN_ACTION | 532 PTP_PIN_CFG_PIN_DOM | 533 PTP_PIN_CFG_PIN_SYNC, 534 lan966x, PTP_PIN_CFG(TOD_ACC_PIN)); 535 536 spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags); 537 } else { 538 /* Fall back using lan966x_ptp_settime64 which is not exact */ 539 struct timespec64 ts; 540 u64 now; 541 542 lan966x_ptp_gettime64(ptp, &ts); 543 544 now = ktime_to_ns(timespec64_to_ktime(ts)); 545 ts = ns_to_timespec64(now + delta); 546 547 lan966x_ptp_settime64(ptp, &ts); 548 } 549 550 return 0; 551 } 552 553 static int lan966x_ptp_verify(struct ptp_clock_info *ptp, unsigned int pin, 554 enum ptp_pin_function func, unsigned int chan) 555 { 556 struct lan966x_phc *phc = container_of(ptp, struct lan966x_phc, info); 557 struct lan966x *lan966x = phc->lan966x; 558 struct ptp_clock_info *info; 559 int i; 560 561 /* Currently support only 1 channel */ 562 if (chan != 0) 563 return -1; 564 565 switch (func) { 566 case PTP_PF_NONE: 567 case PTP_PF_PEROUT: 568 case PTP_PF_EXTTS: 569 break; 570 default: 571 return -1; 572 } 573 574 /* The PTP pins are shared by all the PHC. So it is required to see if 575 * the pin is connected to another PHC. The pin is connected to another 576 * PHC if that pin already has a function on that PHC. 577 */ 578 for (i = 0; i < LAN966X_PHC_COUNT; ++i) { 579 info = &lan966x->phc[i].info; 580 581 /* Ignore the check with ourself */ 582 if (ptp == info) 583 continue; 584 585 if (info->pin_config[pin].func == PTP_PF_PEROUT || 586 info->pin_config[pin].func == PTP_PF_EXTTS) 587 return -1; 588 } 589 590 return 0; 591 } 592 593 static int lan966x_ptp_perout(struct ptp_clock_info *ptp, 594 struct ptp_clock_request *rq, int on) 595 { 596 struct lan966x_phc *phc = container_of(ptp, struct lan966x_phc, info); 597 struct lan966x *lan966x = phc->lan966x; 598 struct timespec64 ts_phase, ts_period; 599 unsigned long flags; 600 s64 wf_high, wf_low; 601 bool pps = false; 602 int pin; 603 604 if (rq->perout.flags & ~(PTP_PEROUT_DUTY_CYCLE | 605 PTP_PEROUT_PHASE)) 606 return -EOPNOTSUPP; 607 608 pin = ptp_find_pin(phc->clock, PTP_PF_PEROUT, rq->perout.index); 609 if (pin == -1 || pin >= LAN966X_PHC_PINS_NUM) 610 return -EINVAL; 611 612 if (!on) { 613 spin_lock_irqsave(&lan966x->ptp_clock_lock, flags); 614 lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_IDLE) | 615 PTP_PIN_CFG_PIN_DOM_SET(phc->index) | 616 PTP_PIN_CFG_PIN_SYNC_SET(0), 617 PTP_PIN_CFG_PIN_ACTION | 618 PTP_PIN_CFG_PIN_DOM | 619 PTP_PIN_CFG_PIN_SYNC, 620 lan966x, PTP_PIN_CFG(pin)); 621 spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags); 622 return 0; 623 } 624 625 if (rq->perout.period.sec == 1 && 626 rq->perout.period.nsec == 0) 627 pps = true; 628 629 if (rq->perout.flags & PTP_PEROUT_PHASE) { 630 ts_phase.tv_sec = rq->perout.phase.sec; 631 ts_phase.tv_nsec = rq->perout.phase.nsec; 632 } else { 633 ts_phase.tv_sec = rq->perout.start.sec; 634 ts_phase.tv_nsec = rq->perout.start.nsec; 635 } 636 637 if (ts_phase.tv_sec || (ts_phase.tv_nsec && !pps)) { 638 dev_warn(lan966x->dev, 639 "Absolute time not supported!\n"); 640 return -EINVAL; 641 } 642 643 if (rq->perout.flags & PTP_PEROUT_DUTY_CYCLE) { 644 struct timespec64 ts_on; 645 646 ts_on.tv_sec = rq->perout.on.sec; 647 ts_on.tv_nsec = rq->perout.on.nsec; 648 649 wf_high = timespec64_to_ns(&ts_on); 650 } else { 651 wf_high = 5000; 652 } 653 654 if (pps) { 655 spin_lock_irqsave(&lan966x->ptp_clock_lock, flags); 656 lan_wr(PTP_WF_LOW_PERIOD_PIN_WFL(ts_phase.tv_nsec), 657 lan966x, PTP_WF_LOW_PERIOD(pin)); 658 lan_wr(PTP_WF_HIGH_PERIOD_PIN_WFH(wf_high), 659 lan966x, PTP_WF_HIGH_PERIOD(pin)); 660 lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_CLOCK) | 661 PTP_PIN_CFG_PIN_DOM_SET(phc->index) | 662 PTP_PIN_CFG_PIN_SYNC_SET(3), 663 PTP_PIN_CFG_PIN_ACTION | 664 PTP_PIN_CFG_PIN_DOM | 665 PTP_PIN_CFG_PIN_SYNC, 666 lan966x, PTP_PIN_CFG(pin)); 667 spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags); 668 return 0; 669 } 670 671 ts_period.tv_sec = rq->perout.period.sec; 672 ts_period.tv_nsec = rq->perout.period.nsec; 673 674 wf_low = timespec64_to_ns(&ts_period); 675 wf_low -= wf_high; 676 677 spin_lock_irqsave(&lan966x->ptp_clock_lock, flags); 678 lan_wr(PTP_WF_LOW_PERIOD_PIN_WFL(wf_low), 679 lan966x, PTP_WF_LOW_PERIOD(pin)); 680 lan_wr(PTP_WF_HIGH_PERIOD_PIN_WFH(wf_high), 681 lan966x, PTP_WF_HIGH_PERIOD(pin)); 682 lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_CLOCK) | 683 PTP_PIN_CFG_PIN_DOM_SET(phc->index) | 684 PTP_PIN_CFG_PIN_SYNC_SET(0), 685 PTP_PIN_CFG_PIN_ACTION | 686 PTP_PIN_CFG_PIN_DOM | 687 PTP_PIN_CFG_PIN_SYNC, 688 lan966x, PTP_PIN_CFG(pin)); 689 spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags); 690 691 return 0; 692 } 693 694 static int lan966x_ptp_extts(struct ptp_clock_info *ptp, 695 struct ptp_clock_request *rq, int on) 696 { 697 struct lan966x_phc *phc = container_of(ptp, struct lan966x_phc, info); 698 struct lan966x *lan966x = phc->lan966x; 699 unsigned long flags; 700 int pin; 701 u32 val; 702 703 if (lan966x->ptp_ext_irq <= 0) 704 return -EOPNOTSUPP; 705 706 /* Reject requests with unsupported flags */ 707 if (rq->extts.flags & ~(PTP_ENABLE_FEATURE | 708 PTP_RISING_EDGE | 709 PTP_STRICT_FLAGS)) 710 return -EOPNOTSUPP; 711 712 pin = ptp_find_pin(phc->clock, PTP_PF_EXTTS, rq->extts.index); 713 if (pin == -1 || pin >= LAN966X_PHC_PINS_NUM) 714 return -EINVAL; 715 716 spin_lock_irqsave(&lan966x->ptp_clock_lock, flags); 717 lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_SAVE) | 718 PTP_PIN_CFG_PIN_SYNC_SET(on ? 3 : 0) | 719 PTP_PIN_CFG_PIN_DOM_SET(phc->index) | 720 PTP_PIN_CFG_PIN_SELECT_SET(pin), 721 PTP_PIN_CFG_PIN_ACTION | 722 PTP_PIN_CFG_PIN_SYNC | 723 PTP_PIN_CFG_PIN_DOM | 724 PTP_PIN_CFG_PIN_SELECT, 725 lan966x, PTP_PIN_CFG(pin)); 726 727 val = lan_rd(lan966x, PTP_PIN_INTR_ENA); 728 if (on) 729 val |= BIT(pin); 730 else 731 val &= ~BIT(pin); 732 lan_wr(val, lan966x, PTP_PIN_INTR_ENA); 733 734 spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags); 735 736 return 0; 737 } 738 739 static int lan966x_ptp_enable(struct ptp_clock_info *ptp, 740 struct ptp_clock_request *rq, int on) 741 { 742 switch (rq->type) { 743 case PTP_CLK_REQ_PEROUT: 744 return lan966x_ptp_perout(ptp, rq, on); 745 case PTP_CLK_REQ_EXTTS: 746 return lan966x_ptp_extts(ptp, rq, on); 747 default: 748 return -EOPNOTSUPP; 749 } 750 751 return 0; 752 } 753 754 static struct ptp_clock_info lan966x_ptp_clock_info = { 755 .owner = THIS_MODULE, 756 .name = "lan966x ptp", 757 .max_adj = 200000, 758 .gettime64 = lan966x_ptp_gettime64, 759 .settime64 = lan966x_ptp_settime64, 760 .adjtime = lan966x_ptp_adjtime, 761 .adjfine = lan966x_ptp_adjfine, 762 .verify = lan966x_ptp_verify, 763 .enable = lan966x_ptp_enable, 764 .n_per_out = LAN966X_PHC_PINS_NUM, 765 .n_ext_ts = LAN966X_PHC_PINS_NUM, 766 .n_pins = LAN966X_PHC_PINS_NUM, 767 }; 768 769 static int lan966x_ptp_phc_init(struct lan966x *lan966x, 770 int index, 771 struct ptp_clock_info *clock_info) 772 { 773 struct lan966x_phc *phc = &lan966x->phc[index]; 774 struct ptp_pin_desc *p; 775 int i; 776 777 for (i = 0; i < LAN966X_PHC_PINS_NUM; i++) { 778 p = &phc->pins[i]; 779 780 snprintf(p->name, sizeof(p->name), "pin%d", i); 781 p->index = i; 782 p->func = PTP_PF_NONE; 783 } 784 785 phc->info = *clock_info; 786 phc->info.pin_config = &phc->pins[0]; 787 phc->clock = ptp_clock_register(&phc->info, lan966x->dev); 788 if (IS_ERR(phc->clock)) 789 return PTR_ERR(phc->clock); 790 791 phc->index = index; 792 phc->lan966x = lan966x; 793 794 /* PTP Rx stamping is always enabled. */ 795 phc->hwtstamp_config.rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT; 796 797 return 0; 798 } 799 800 int lan966x_ptp_init(struct lan966x *lan966x) 801 { 802 u64 tod_adj = lan966x_ptp_get_nominal_value(); 803 struct lan966x_port *port; 804 int err, i; 805 806 if (!lan966x->ptp) 807 return 0; 808 809 for (i = 0; i < LAN966X_PHC_COUNT; ++i) { 810 err = lan966x_ptp_phc_init(lan966x, i, &lan966x_ptp_clock_info); 811 if (err) 812 return err; 813 } 814 815 spin_lock_init(&lan966x->ptp_clock_lock); 816 spin_lock_init(&lan966x->ptp_ts_id_lock); 817 mutex_init(&lan966x->ptp_lock); 818 819 /* Disable master counters */ 820 lan_wr(PTP_DOM_CFG_ENA_SET(0), lan966x, PTP_DOM_CFG); 821 822 /* Configure the nominal TOD increment per clock cycle */ 823 lan_rmw(PTP_DOM_CFG_CLKCFG_DIS_SET(0x7), 824 PTP_DOM_CFG_CLKCFG_DIS, 825 lan966x, PTP_DOM_CFG); 826 827 for (i = 0; i < LAN966X_PHC_COUNT; ++i) { 828 lan_wr((u32)tod_adj & 0xFFFFFFFF, lan966x, 829 PTP_CLK_PER_CFG(i, 0)); 830 lan_wr((u32)(tod_adj >> 32), lan966x, 831 PTP_CLK_PER_CFG(i, 1)); 832 } 833 834 lan_rmw(PTP_DOM_CFG_CLKCFG_DIS_SET(0), 835 PTP_DOM_CFG_CLKCFG_DIS, 836 lan966x, PTP_DOM_CFG); 837 838 /* Enable master counters */ 839 lan_wr(PTP_DOM_CFG_ENA_SET(0x7), lan966x, PTP_DOM_CFG); 840 841 for (i = 0; i < lan966x->num_phys_ports; i++) { 842 port = lan966x->ports[i]; 843 if (!port) 844 continue; 845 846 skb_queue_head_init(&port->tx_skbs); 847 } 848 849 return 0; 850 } 851 852 void lan966x_ptp_deinit(struct lan966x *lan966x) 853 { 854 struct lan966x_port *port; 855 int i; 856 857 for (i = 0; i < lan966x->num_phys_ports; i++) { 858 port = lan966x->ports[i]; 859 if (!port) 860 continue; 861 862 skb_queue_purge(&port->tx_skbs); 863 } 864 865 for (i = 0; i < LAN966X_PHC_COUNT; ++i) 866 ptp_clock_unregister(lan966x->phc[i].clock); 867 } 868 869 void lan966x_ptp_rxtstamp(struct lan966x *lan966x, struct sk_buff *skb, 870 u64 timestamp) 871 { 872 struct skb_shared_hwtstamps *shhwtstamps; 873 struct lan966x_phc *phc; 874 struct timespec64 ts; 875 u64 full_ts_in_ns; 876 877 if (!lan966x->ptp) 878 return; 879 880 phc = &lan966x->phc[LAN966X_PHC_PORT]; 881 lan966x_ptp_gettime64(&phc->info, &ts); 882 883 /* Drop the sub-ns precision */ 884 timestamp = timestamp >> 2; 885 if (ts.tv_nsec < timestamp) 886 ts.tv_sec--; 887 ts.tv_nsec = timestamp; 888 full_ts_in_ns = ktime_set(ts.tv_sec, ts.tv_nsec); 889 890 shhwtstamps = skb_hwtstamps(skb); 891 shhwtstamps->hwtstamp = full_ts_in_ns; 892 } 893