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 int lan966x_ptp_gettime64(struct ptp_clock_info *ptp, struct timespec64 *ts) 468 { 469 struct lan966x_phc *phc = container_of(ptp, struct lan966x_phc, info); 470 struct lan966x *lan966x = phc->lan966x; 471 unsigned long flags; 472 time64_t s; 473 s64 ns; 474 475 spin_lock_irqsave(&lan966x->ptp_clock_lock, flags); 476 477 lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_SAVE) | 478 PTP_PIN_CFG_PIN_DOM_SET(phc->index) | 479 PTP_PIN_CFG_PIN_SYNC_SET(0), 480 PTP_PIN_CFG_PIN_ACTION | 481 PTP_PIN_CFG_PIN_DOM | 482 PTP_PIN_CFG_PIN_SYNC, 483 lan966x, PTP_PIN_CFG(TOD_ACC_PIN)); 484 485 s = lan_rd(lan966x, PTP_TOD_SEC_MSB(TOD_ACC_PIN)); 486 s <<= 32; 487 s |= lan_rd(lan966x, PTP_TOD_SEC_LSB(TOD_ACC_PIN)); 488 ns = lan_rd(lan966x, PTP_TOD_NSEC(TOD_ACC_PIN)); 489 ns &= PTP_TOD_NSEC_TOD_NSEC; 490 491 spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags); 492 493 /* Deal with negative values */ 494 if ((ns & 0xFFFFFFF0) == 0x3FFFFFF0) { 495 s--; 496 ns &= 0xf; 497 ns += 999999984; 498 } 499 500 set_normalized_timespec64(ts, s, ns); 501 return 0; 502 } 503 504 static int lan966x_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta) 505 { 506 struct lan966x_phc *phc = container_of(ptp, struct lan966x_phc, info); 507 struct lan966x *lan966x = phc->lan966x; 508 509 if (delta > -(NSEC_PER_SEC / 2) && delta < (NSEC_PER_SEC / 2)) { 510 unsigned long flags; 511 512 spin_lock_irqsave(&lan966x->ptp_clock_lock, flags); 513 514 /* Must be in IDLE mode before the time can be loaded */ 515 lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_IDLE) | 516 PTP_PIN_CFG_PIN_DOM_SET(phc->index) | 517 PTP_PIN_CFG_PIN_SYNC_SET(0), 518 PTP_PIN_CFG_PIN_ACTION | 519 PTP_PIN_CFG_PIN_DOM | 520 PTP_PIN_CFG_PIN_SYNC, 521 lan966x, PTP_PIN_CFG(TOD_ACC_PIN)); 522 523 lan_wr(PTP_TOD_NSEC_TOD_NSEC_SET(delta), 524 lan966x, PTP_TOD_NSEC(TOD_ACC_PIN)); 525 526 /* Adjust time with the value of PTP_TOD_NSEC */ 527 lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_DELTA) | 528 PTP_PIN_CFG_PIN_DOM_SET(phc->index) | 529 PTP_PIN_CFG_PIN_SYNC_SET(0), 530 PTP_PIN_CFG_PIN_ACTION | 531 PTP_PIN_CFG_PIN_DOM | 532 PTP_PIN_CFG_PIN_SYNC, 533 lan966x, PTP_PIN_CFG(TOD_ACC_PIN)); 534 535 spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags); 536 } else { 537 /* Fall back using lan966x_ptp_settime64 which is not exact */ 538 struct timespec64 ts; 539 u64 now; 540 541 lan966x_ptp_gettime64(ptp, &ts); 542 543 now = ktime_to_ns(timespec64_to_ktime(ts)); 544 ts = ns_to_timespec64(now + delta); 545 546 lan966x_ptp_settime64(ptp, &ts); 547 } 548 549 return 0; 550 } 551 552 static int lan966x_ptp_verify(struct ptp_clock_info *ptp, unsigned int pin, 553 enum ptp_pin_function func, unsigned int chan) 554 { 555 struct lan966x_phc *phc = container_of(ptp, struct lan966x_phc, info); 556 struct lan966x *lan966x = phc->lan966x; 557 struct ptp_clock_info *info; 558 int i; 559 560 /* Currently support only 1 channel */ 561 if (chan != 0) 562 return -1; 563 564 switch (func) { 565 case PTP_PF_NONE: 566 case PTP_PF_PEROUT: 567 case PTP_PF_EXTTS: 568 break; 569 default: 570 return -1; 571 } 572 573 /* The PTP pins are shared by all the PHC. So it is required to see if 574 * the pin is connected to another PHC. The pin is connected to another 575 * PHC if that pin already has a function on that PHC. 576 */ 577 for (i = 0; i < LAN966X_PHC_COUNT; ++i) { 578 info = &lan966x->phc[i].info; 579 580 /* Ignore the check with ourself */ 581 if (ptp == info) 582 continue; 583 584 if (info->pin_config[pin].func == PTP_PF_PEROUT || 585 info->pin_config[pin].func == PTP_PF_EXTTS) 586 return -1; 587 } 588 589 return 0; 590 } 591 592 static int lan966x_ptp_perout(struct ptp_clock_info *ptp, 593 struct ptp_clock_request *rq, int on) 594 { 595 struct lan966x_phc *phc = container_of(ptp, struct lan966x_phc, info); 596 struct lan966x *lan966x = phc->lan966x; 597 struct timespec64 ts_phase, ts_period; 598 unsigned long flags; 599 s64 wf_high, wf_low; 600 bool pps = false; 601 int pin; 602 603 if (rq->perout.flags & ~(PTP_PEROUT_DUTY_CYCLE | 604 PTP_PEROUT_PHASE)) 605 return -EOPNOTSUPP; 606 607 pin = ptp_find_pin(phc->clock, PTP_PF_PEROUT, rq->perout.index); 608 if (pin == -1 || pin >= LAN966X_PHC_PINS_NUM) 609 return -EINVAL; 610 611 if (!on) { 612 spin_lock_irqsave(&lan966x->ptp_clock_lock, flags); 613 lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_IDLE) | 614 PTP_PIN_CFG_PIN_DOM_SET(phc->index) | 615 PTP_PIN_CFG_PIN_SYNC_SET(0), 616 PTP_PIN_CFG_PIN_ACTION | 617 PTP_PIN_CFG_PIN_DOM | 618 PTP_PIN_CFG_PIN_SYNC, 619 lan966x, PTP_PIN_CFG(pin)); 620 spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags); 621 return 0; 622 } 623 624 if (rq->perout.period.sec == 1 && 625 rq->perout.period.nsec == 0) 626 pps = true; 627 628 if (rq->perout.flags & PTP_PEROUT_PHASE) { 629 ts_phase.tv_sec = rq->perout.phase.sec; 630 ts_phase.tv_nsec = rq->perout.phase.nsec; 631 } else { 632 ts_phase.tv_sec = rq->perout.start.sec; 633 ts_phase.tv_nsec = rq->perout.start.nsec; 634 } 635 636 if (ts_phase.tv_sec || (ts_phase.tv_nsec && !pps)) { 637 dev_warn(lan966x->dev, 638 "Absolute time not supported!\n"); 639 return -EINVAL; 640 } 641 642 if (rq->perout.flags & PTP_PEROUT_DUTY_CYCLE) { 643 struct timespec64 ts_on; 644 645 ts_on.tv_sec = rq->perout.on.sec; 646 ts_on.tv_nsec = rq->perout.on.nsec; 647 648 wf_high = timespec64_to_ns(&ts_on); 649 } else { 650 wf_high = 5000; 651 } 652 653 if (pps) { 654 spin_lock_irqsave(&lan966x->ptp_clock_lock, flags); 655 lan_wr(PTP_WF_LOW_PERIOD_PIN_WFL(ts_phase.tv_nsec), 656 lan966x, PTP_WF_LOW_PERIOD(pin)); 657 lan_wr(PTP_WF_HIGH_PERIOD_PIN_WFH(wf_high), 658 lan966x, PTP_WF_HIGH_PERIOD(pin)); 659 lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_CLOCK) | 660 PTP_PIN_CFG_PIN_DOM_SET(phc->index) | 661 PTP_PIN_CFG_PIN_SYNC_SET(3), 662 PTP_PIN_CFG_PIN_ACTION | 663 PTP_PIN_CFG_PIN_DOM | 664 PTP_PIN_CFG_PIN_SYNC, 665 lan966x, PTP_PIN_CFG(pin)); 666 spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags); 667 return 0; 668 } 669 670 ts_period.tv_sec = rq->perout.period.sec; 671 ts_period.tv_nsec = rq->perout.period.nsec; 672 673 wf_low = timespec64_to_ns(&ts_period); 674 wf_low -= wf_high; 675 676 spin_lock_irqsave(&lan966x->ptp_clock_lock, flags); 677 lan_wr(PTP_WF_LOW_PERIOD_PIN_WFL(wf_low), 678 lan966x, PTP_WF_LOW_PERIOD(pin)); 679 lan_wr(PTP_WF_HIGH_PERIOD_PIN_WFH(wf_high), 680 lan966x, PTP_WF_HIGH_PERIOD(pin)); 681 lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_CLOCK) | 682 PTP_PIN_CFG_PIN_DOM_SET(phc->index) | 683 PTP_PIN_CFG_PIN_SYNC_SET(0), 684 PTP_PIN_CFG_PIN_ACTION | 685 PTP_PIN_CFG_PIN_DOM | 686 PTP_PIN_CFG_PIN_SYNC, 687 lan966x, PTP_PIN_CFG(pin)); 688 spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags); 689 690 return 0; 691 } 692 693 static int lan966x_ptp_extts(struct ptp_clock_info *ptp, 694 struct ptp_clock_request *rq, int on) 695 { 696 struct lan966x_phc *phc = container_of(ptp, struct lan966x_phc, info); 697 struct lan966x *lan966x = phc->lan966x; 698 unsigned long flags; 699 int pin; 700 u32 val; 701 702 if (lan966x->ptp_ext_irq <= 0) 703 return -EOPNOTSUPP; 704 705 /* Reject requests with unsupported flags */ 706 if (rq->extts.flags & ~(PTP_ENABLE_FEATURE | 707 PTP_RISING_EDGE | 708 PTP_STRICT_FLAGS)) 709 return -EOPNOTSUPP; 710 711 pin = ptp_find_pin(phc->clock, PTP_PF_EXTTS, rq->extts.index); 712 if (pin == -1 || pin >= LAN966X_PHC_PINS_NUM) 713 return -EINVAL; 714 715 spin_lock_irqsave(&lan966x->ptp_clock_lock, flags); 716 lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_SAVE) | 717 PTP_PIN_CFG_PIN_SYNC_SET(on ? 3 : 0) | 718 PTP_PIN_CFG_PIN_DOM_SET(phc->index) | 719 PTP_PIN_CFG_PIN_SELECT_SET(pin), 720 PTP_PIN_CFG_PIN_ACTION | 721 PTP_PIN_CFG_PIN_SYNC | 722 PTP_PIN_CFG_PIN_DOM | 723 PTP_PIN_CFG_PIN_SELECT, 724 lan966x, PTP_PIN_CFG(pin)); 725 726 val = lan_rd(lan966x, PTP_PIN_INTR_ENA); 727 if (on) 728 val |= BIT(pin); 729 else 730 val &= ~BIT(pin); 731 lan_wr(val, lan966x, PTP_PIN_INTR_ENA); 732 733 spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags); 734 735 return 0; 736 } 737 738 static int lan966x_ptp_enable(struct ptp_clock_info *ptp, 739 struct ptp_clock_request *rq, int on) 740 { 741 switch (rq->type) { 742 case PTP_CLK_REQ_PEROUT: 743 return lan966x_ptp_perout(ptp, rq, on); 744 case PTP_CLK_REQ_EXTTS: 745 return lan966x_ptp_extts(ptp, rq, on); 746 default: 747 return -EOPNOTSUPP; 748 } 749 750 return 0; 751 } 752 753 static struct ptp_clock_info lan966x_ptp_clock_info = { 754 .owner = THIS_MODULE, 755 .name = "lan966x ptp", 756 .max_adj = 200000, 757 .gettime64 = lan966x_ptp_gettime64, 758 .settime64 = lan966x_ptp_settime64, 759 .adjtime = lan966x_ptp_adjtime, 760 .adjfine = lan966x_ptp_adjfine, 761 .verify = lan966x_ptp_verify, 762 .enable = lan966x_ptp_enable, 763 .n_per_out = LAN966X_PHC_PINS_NUM, 764 .n_ext_ts = LAN966X_PHC_PINS_NUM, 765 .n_pins = LAN966X_PHC_PINS_NUM, 766 }; 767 768 static int lan966x_ptp_phc_init(struct lan966x *lan966x, 769 int index, 770 struct ptp_clock_info *clock_info) 771 { 772 struct lan966x_phc *phc = &lan966x->phc[index]; 773 struct ptp_pin_desc *p; 774 int i; 775 776 for (i = 0; i < LAN966X_PHC_PINS_NUM; i++) { 777 p = &phc->pins[i]; 778 779 snprintf(p->name, sizeof(p->name), "pin%d", i); 780 p->index = i; 781 p->func = PTP_PF_NONE; 782 } 783 784 phc->info = *clock_info; 785 phc->info.pin_config = &phc->pins[0]; 786 phc->clock = ptp_clock_register(&phc->info, lan966x->dev); 787 if (IS_ERR(phc->clock)) 788 return PTR_ERR(phc->clock); 789 790 phc->index = index; 791 phc->lan966x = lan966x; 792 793 /* PTP Rx stamping is always enabled. */ 794 phc->hwtstamp_config.rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT; 795 796 return 0; 797 } 798 799 int lan966x_ptp_init(struct lan966x *lan966x) 800 { 801 u64 tod_adj = lan966x_ptp_get_nominal_value(); 802 struct lan966x_port *port; 803 int err, i; 804 805 if (!lan966x->ptp) 806 return 0; 807 808 for (i = 0; i < LAN966X_PHC_COUNT; ++i) { 809 err = lan966x_ptp_phc_init(lan966x, i, &lan966x_ptp_clock_info); 810 if (err) 811 return err; 812 } 813 814 spin_lock_init(&lan966x->ptp_clock_lock); 815 spin_lock_init(&lan966x->ptp_ts_id_lock); 816 mutex_init(&lan966x->ptp_lock); 817 818 /* Disable master counters */ 819 lan_wr(PTP_DOM_CFG_ENA_SET(0), lan966x, PTP_DOM_CFG); 820 821 /* Configure the nominal TOD increment per clock cycle */ 822 lan_rmw(PTP_DOM_CFG_CLKCFG_DIS_SET(0x7), 823 PTP_DOM_CFG_CLKCFG_DIS, 824 lan966x, PTP_DOM_CFG); 825 826 for (i = 0; i < LAN966X_PHC_COUNT; ++i) { 827 lan_wr((u32)tod_adj & 0xFFFFFFFF, lan966x, 828 PTP_CLK_PER_CFG(i, 0)); 829 lan_wr((u32)(tod_adj >> 32), lan966x, 830 PTP_CLK_PER_CFG(i, 1)); 831 } 832 833 lan_rmw(PTP_DOM_CFG_CLKCFG_DIS_SET(0), 834 PTP_DOM_CFG_CLKCFG_DIS, 835 lan966x, PTP_DOM_CFG); 836 837 /* Enable master counters */ 838 lan_wr(PTP_DOM_CFG_ENA_SET(0x7), lan966x, PTP_DOM_CFG); 839 840 for (i = 0; i < lan966x->num_phys_ports; i++) { 841 port = lan966x->ports[i]; 842 if (!port) 843 continue; 844 845 skb_queue_head_init(&port->tx_skbs); 846 } 847 848 return 0; 849 } 850 851 void lan966x_ptp_deinit(struct lan966x *lan966x) 852 { 853 struct lan966x_port *port; 854 int i; 855 856 for (i = 0; i < lan966x->num_phys_ports; i++) { 857 port = lan966x->ports[i]; 858 if (!port) 859 continue; 860 861 skb_queue_purge(&port->tx_skbs); 862 } 863 864 for (i = 0; i < LAN966X_PHC_COUNT; ++i) 865 ptp_clock_unregister(lan966x->phc[i].clock); 866 } 867 868 void lan966x_ptp_rxtstamp(struct lan966x *lan966x, struct sk_buff *skb, 869 u64 timestamp) 870 { 871 struct skb_shared_hwtstamps *shhwtstamps; 872 struct lan966x_phc *phc; 873 struct timespec64 ts; 874 u64 full_ts_in_ns; 875 876 if (!lan966x->ptp) 877 return; 878 879 phc = &lan966x->phc[LAN966X_PHC_PORT]; 880 lan966x_ptp_gettime64(&phc->info, &ts); 881 882 /* Drop the sub-ns precision */ 883 timestamp = timestamp >> 2; 884 if (ts.tv_nsec < timestamp) 885 ts.tv_sec--; 886 ts.tv_nsec = timestamp; 887 full_ts_in_ns = ktime_set(ts.tv_sec, ts.tv_nsec); 888 889 shhwtstamps = skb_hwtstamps(skb); 890 shhwtstamps->hwtstamp = full_ts_in_ns; 891 } 892 893 u32 lan966x_ptp_get_period_ps(void) 894 { 895 /* This represents the system clock period in picoseconds */ 896 return 15125; 897 } 898