1 // SPDX-License-Identifier: GPL-2.0+ 2 3 #include <linux/ptp_classify.h> 4 5 #include "lan966x_main.h" 6 #include "vcap_api.h" 7 #include "vcap_api_client.h" 8 9 #define LAN966X_MAX_PTP_ID 512 10 11 /* Represents 1ppm adjustment in 2^59 format with 6.037735849ns as reference 12 * The value is calculated as following: (1/1000000)/((2^-59)/6.037735849) 13 */ 14 #define LAN966X_1PPM_FORMAT 3480517749723LL 15 16 /* Represents 1ppb adjustment in 2^29 format with 6.037735849ns as reference 17 * The value is calculated as following: (1/1000000000)/((2^59)/6.037735849) 18 */ 19 #define LAN966X_1PPB_FORMAT 3480517749LL 20 21 #define TOD_ACC_PIN 0x7 22 23 /* This represents the base rule ID for the PTP rules that are added in the 24 * VCAP to trap frames to CPU. This number needs to be bigger than the maximum 25 * number of entries that can exist in the VCAP. 26 */ 27 #define LAN966X_VCAP_PTP_RULE_ID 1000000 28 #define LAN966X_VCAP_L2_PTP_TRAP (LAN966X_VCAP_PTP_RULE_ID + 0) 29 #define LAN966X_VCAP_IPV4_EV_PTP_TRAP (LAN966X_VCAP_PTP_RULE_ID + 1) 30 #define LAN966X_VCAP_IPV4_GEN_PTP_TRAP (LAN966X_VCAP_PTP_RULE_ID + 2) 31 #define LAN966X_VCAP_IPV6_EV_PTP_TRAP (LAN966X_VCAP_PTP_RULE_ID + 3) 32 #define LAN966X_VCAP_IPV6_GEN_PTP_TRAP (LAN966X_VCAP_PTP_RULE_ID + 4) 33 34 enum { 35 PTP_PIN_ACTION_IDLE = 0, 36 PTP_PIN_ACTION_LOAD, 37 PTP_PIN_ACTION_SAVE, 38 PTP_PIN_ACTION_CLOCK, 39 PTP_PIN_ACTION_DELTA, 40 PTP_PIN_ACTION_TOD 41 }; 42 43 static u64 lan966x_ptp_get_nominal_value(void) 44 { 45 /* This is the default value that for each system clock, the time of day 46 * is increased. It has the format 5.59 nanosecond. 47 */ 48 return 0x304d4873ecade305; 49 } 50 51 static int lan966x_ptp_add_trap(struct lan966x_port *port, 52 int (*add_ptp_key)(struct vcap_rule *vrule, 53 struct lan966x_port*), 54 u32 rule_id, 55 u16 proto) 56 { 57 struct lan966x *lan966x = port->lan966x; 58 struct vcap_rule *vrule; 59 int err; 60 61 vrule = vcap_get_rule(lan966x->vcap_ctrl, rule_id); 62 if (vrule) { 63 u32 value, mask; 64 65 /* Just modify the ingress port mask and exit */ 66 vcap_rule_get_key_u32(vrule, VCAP_KF_IF_IGR_PORT_MASK, 67 &value, &mask); 68 mask &= ~BIT(port->chip_port); 69 vcap_rule_mod_key_u32(vrule, VCAP_KF_IF_IGR_PORT_MASK, 70 value, mask); 71 72 err = vcap_mod_rule(vrule); 73 goto free_rule; 74 } 75 76 vrule = vcap_alloc_rule(lan966x->vcap_ctrl, port->dev, 77 LAN966X_VCAP_CID_IS2_L0, 78 VCAP_USER_PTP, 0, rule_id); 79 if (IS_ERR(vrule)) 80 return PTR_ERR(vrule); 81 82 err = add_ptp_key(vrule, port); 83 if (err) 84 goto free_rule; 85 86 err = vcap_rule_add_action_bit(vrule, VCAP_AF_CPU_COPY_ENA, VCAP_BIT_1); 87 err |= vcap_rule_add_action_u32(vrule, VCAP_AF_MASK_MODE, LAN966X_PMM_REPLACE); 88 err |= vcap_val_rule(vrule, proto); 89 if (err) 90 goto free_rule; 91 92 err = vcap_add_rule(vrule); 93 94 free_rule: 95 /* Free the local copy of the rule */ 96 vcap_free_rule(vrule); 97 return err; 98 } 99 100 static int lan966x_ptp_del_trap(struct lan966x_port *port, 101 u32 rule_id) 102 { 103 struct lan966x *lan966x = port->lan966x; 104 struct vcap_rule *vrule; 105 u32 value, mask; 106 int err; 107 108 vrule = vcap_get_rule(lan966x->vcap_ctrl, rule_id); 109 if (!vrule) 110 return -EEXIST; 111 112 vcap_rule_get_key_u32(vrule, VCAP_KF_IF_IGR_PORT_MASK, &value, &mask); 113 mask |= BIT(port->chip_port); 114 115 /* No other port requires this trap, so it is safe to remove it */ 116 if (mask == GENMASK(lan966x->num_phys_ports, 0)) { 117 err = vcap_del_rule(lan966x->vcap_ctrl, port->dev, rule_id); 118 goto free_rule; 119 } 120 121 vcap_rule_mod_key_u32(vrule, VCAP_KF_IF_IGR_PORT_MASK, value, mask); 122 err = vcap_mod_rule(vrule); 123 124 free_rule: 125 vcap_free_rule(vrule); 126 return err; 127 } 128 129 static int lan966x_ptp_add_l2_key(struct vcap_rule *vrule, 130 struct lan966x_port *port) 131 { 132 return vcap_rule_add_key_u32(vrule, VCAP_KF_ETYPE, ETH_P_1588, ~0); 133 } 134 135 static int lan966x_ptp_add_ip_event_key(struct vcap_rule *vrule, 136 struct lan966x_port *port) 137 { 138 return vcap_rule_add_key_u32(vrule, VCAP_KF_L4_DPORT, PTP_EV_PORT, ~0) || 139 vcap_rule_add_key_bit(vrule, VCAP_KF_TCP_IS, VCAP_BIT_0); 140 } 141 142 static int lan966x_ptp_add_ip_general_key(struct vcap_rule *vrule, 143 struct lan966x_port *port) 144 { 145 return vcap_rule_add_key_u32(vrule, VCAP_KF_L4_DPORT, PTP_GEN_PORT, ~0) || 146 vcap_rule_add_key_bit(vrule, VCAP_KF_TCP_IS, VCAP_BIT_0); 147 } 148 149 static int lan966x_ptp_add_l2_rule(struct lan966x_port *port) 150 { 151 return lan966x_ptp_add_trap(port, lan966x_ptp_add_l2_key, 152 LAN966X_VCAP_L2_PTP_TRAP, ETH_P_ALL); 153 } 154 155 static int lan966x_ptp_add_ipv4_rules(struct lan966x_port *port) 156 { 157 int err; 158 159 err = lan966x_ptp_add_trap(port, lan966x_ptp_add_ip_event_key, 160 LAN966X_VCAP_IPV4_EV_PTP_TRAP, ETH_P_IP); 161 if (err) 162 return err; 163 164 err = lan966x_ptp_add_trap(port, lan966x_ptp_add_ip_general_key, 165 LAN966X_VCAP_IPV4_GEN_PTP_TRAP, ETH_P_IP); 166 if (err) 167 lan966x_ptp_del_trap(port, LAN966X_VCAP_IPV4_EV_PTP_TRAP); 168 169 return err; 170 } 171 172 static int lan966x_ptp_add_ipv6_rules(struct lan966x_port *port) 173 { 174 int err; 175 176 err = lan966x_ptp_add_trap(port, lan966x_ptp_add_ip_event_key, 177 LAN966X_VCAP_IPV6_EV_PTP_TRAP, ETH_P_IPV6); 178 if (err) 179 return err; 180 181 err = lan966x_ptp_add_trap(port, lan966x_ptp_add_ip_general_key, 182 LAN966X_VCAP_IPV6_GEN_PTP_TRAP, ETH_P_IPV6); 183 if (err) 184 lan966x_ptp_del_trap(port, LAN966X_VCAP_IPV6_EV_PTP_TRAP); 185 186 return err; 187 } 188 189 static int lan966x_ptp_del_l2_rule(struct lan966x_port *port) 190 { 191 return lan966x_ptp_del_trap(port, LAN966X_VCAP_L2_PTP_TRAP); 192 } 193 194 static int lan966x_ptp_del_ipv4_rules(struct lan966x_port *port) 195 { 196 int err; 197 198 err = lan966x_ptp_del_trap(port, LAN966X_VCAP_IPV4_EV_PTP_TRAP); 199 err |= lan966x_ptp_del_trap(port, LAN966X_VCAP_IPV4_GEN_PTP_TRAP); 200 201 return err; 202 } 203 204 static int lan966x_ptp_del_ipv6_rules(struct lan966x_port *port) 205 { 206 int err; 207 208 err = lan966x_ptp_del_trap(port, LAN966X_VCAP_IPV6_EV_PTP_TRAP); 209 err |= lan966x_ptp_del_trap(port, LAN966X_VCAP_IPV6_GEN_PTP_TRAP); 210 211 return err; 212 } 213 214 static int lan966x_ptp_add_traps(struct lan966x_port *port) 215 { 216 int err; 217 218 err = lan966x_ptp_add_l2_rule(port); 219 if (err) 220 goto err_l2; 221 222 err = lan966x_ptp_add_ipv4_rules(port); 223 if (err) 224 goto err_ipv4; 225 226 err = lan966x_ptp_add_ipv6_rules(port); 227 if (err) 228 goto err_ipv6; 229 230 return err; 231 232 err_ipv6: 233 lan966x_ptp_del_ipv4_rules(port); 234 err_ipv4: 235 lan966x_ptp_del_l2_rule(port); 236 err_l2: 237 return err; 238 } 239 240 int lan966x_ptp_del_traps(struct lan966x_port *port) 241 { 242 int err; 243 244 err = lan966x_ptp_del_l2_rule(port); 245 err |= lan966x_ptp_del_ipv4_rules(port); 246 err |= lan966x_ptp_del_ipv6_rules(port); 247 248 return err; 249 } 250 251 int lan966x_ptp_setup_traps(struct lan966x_port *port, struct ifreq *ifr) 252 { 253 struct hwtstamp_config cfg; 254 255 if (copy_from_user(&cfg, ifr->ifr_data, sizeof(cfg))) 256 return -EFAULT; 257 258 if (cfg.rx_filter == HWTSTAMP_FILTER_NONE) 259 return lan966x_ptp_del_traps(port); 260 else 261 return lan966x_ptp_add_traps(port); 262 } 263 264 int lan966x_ptp_hwtstamp_set(struct lan966x_port *port, struct ifreq *ifr) 265 { 266 struct lan966x *lan966x = port->lan966x; 267 struct hwtstamp_config cfg; 268 struct lan966x_phc *phc; 269 270 if (copy_from_user(&cfg, ifr->ifr_data, sizeof(cfg))) 271 return -EFAULT; 272 273 switch (cfg.tx_type) { 274 case HWTSTAMP_TX_ON: 275 port->ptp_cmd = IFH_REW_OP_TWO_STEP_PTP; 276 break; 277 case HWTSTAMP_TX_ONESTEP_SYNC: 278 port->ptp_cmd = IFH_REW_OP_ONE_STEP_PTP; 279 break; 280 case HWTSTAMP_TX_OFF: 281 port->ptp_cmd = IFH_REW_OP_NOOP; 282 break; 283 default: 284 return -ERANGE; 285 } 286 287 switch (cfg.rx_filter) { 288 case HWTSTAMP_FILTER_NONE: 289 break; 290 case HWTSTAMP_FILTER_ALL: 291 case HWTSTAMP_FILTER_PTP_V1_L4_EVENT: 292 case HWTSTAMP_FILTER_PTP_V1_L4_SYNC: 293 case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ: 294 case HWTSTAMP_FILTER_PTP_V2_L4_EVENT: 295 case HWTSTAMP_FILTER_PTP_V2_L4_SYNC: 296 case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ: 297 case HWTSTAMP_FILTER_PTP_V2_L2_EVENT: 298 case HWTSTAMP_FILTER_PTP_V2_L2_SYNC: 299 case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ: 300 case HWTSTAMP_FILTER_PTP_V2_EVENT: 301 case HWTSTAMP_FILTER_PTP_V2_SYNC: 302 case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ: 303 case HWTSTAMP_FILTER_NTP_ALL: 304 cfg.rx_filter = HWTSTAMP_FILTER_ALL; 305 break; 306 default: 307 return -ERANGE; 308 } 309 310 /* Commit back the result & save it */ 311 mutex_lock(&lan966x->ptp_lock); 312 phc = &lan966x->phc[LAN966X_PHC_PORT]; 313 memcpy(&phc->hwtstamp_config, &cfg, sizeof(cfg)); 314 mutex_unlock(&lan966x->ptp_lock); 315 316 return copy_to_user(ifr->ifr_data, &cfg, sizeof(cfg)) ? -EFAULT : 0; 317 } 318 319 int lan966x_ptp_hwtstamp_get(struct lan966x_port *port, struct ifreq *ifr) 320 { 321 struct lan966x *lan966x = port->lan966x; 322 struct lan966x_phc *phc; 323 324 phc = &lan966x->phc[LAN966X_PHC_PORT]; 325 return copy_to_user(ifr->ifr_data, &phc->hwtstamp_config, 326 sizeof(phc->hwtstamp_config)) ? -EFAULT : 0; 327 } 328 329 static int lan966x_ptp_classify(struct lan966x_port *port, struct sk_buff *skb) 330 { 331 struct ptp_header *header; 332 u8 msgtype; 333 int type; 334 335 if (port->ptp_cmd == IFH_REW_OP_NOOP) 336 return IFH_REW_OP_NOOP; 337 338 type = ptp_classify_raw(skb); 339 if (type == PTP_CLASS_NONE) 340 return IFH_REW_OP_NOOP; 341 342 header = ptp_parse_header(skb, type); 343 if (!header) 344 return IFH_REW_OP_NOOP; 345 346 if (port->ptp_cmd == IFH_REW_OP_TWO_STEP_PTP) 347 return IFH_REW_OP_TWO_STEP_PTP; 348 349 /* If it is sync and run 1 step then set the correct operation, 350 * otherwise run as 2 step 351 */ 352 msgtype = ptp_get_msgtype(header, type); 353 if ((msgtype & 0xf) == 0) 354 return IFH_REW_OP_ONE_STEP_PTP; 355 356 return IFH_REW_OP_TWO_STEP_PTP; 357 } 358 359 static void lan966x_ptp_txtstamp_old_release(struct lan966x_port *port) 360 { 361 struct sk_buff *skb, *skb_tmp; 362 unsigned long flags; 363 364 spin_lock_irqsave(&port->tx_skbs.lock, flags); 365 skb_queue_walk_safe(&port->tx_skbs, skb, skb_tmp) { 366 if time_after(LAN966X_SKB_CB(skb)->jiffies + LAN966X_PTP_TIMEOUT, 367 jiffies) 368 break; 369 370 __skb_unlink(skb, &port->tx_skbs); 371 dev_kfree_skb_any(skb); 372 } 373 spin_unlock_irqrestore(&port->tx_skbs.lock, flags); 374 } 375 376 int lan966x_ptp_txtstamp_request(struct lan966x_port *port, 377 struct sk_buff *skb) 378 { 379 struct lan966x *lan966x = port->lan966x; 380 unsigned long flags; 381 u8 rew_op; 382 383 rew_op = lan966x_ptp_classify(port, skb); 384 LAN966X_SKB_CB(skb)->rew_op = rew_op; 385 386 if (rew_op != IFH_REW_OP_TWO_STEP_PTP) 387 return 0; 388 389 lan966x_ptp_txtstamp_old_release(port); 390 391 spin_lock_irqsave(&lan966x->ptp_ts_id_lock, flags); 392 if (lan966x->ptp_skbs == LAN966X_MAX_PTP_ID) { 393 spin_unlock_irqrestore(&lan966x->ptp_ts_id_lock, flags); 394 return -EBUSY; 395 } 396 397 skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS; 398 399 skb_queue_tail(&port->tx_skbs, skb); 400 LAN966X_SKB_CB(skb)->ts_id = port->ts_id; 401 LAN966X_SKB_CB(skb)->jiffies = jiffies; 402 403 lan966x->ptp_skbs++; 404 port->ts_id++; 405 if (port->ts_id == LAN966X_MAX_PTP_ID) 406 port->ts_id = 0; 407 408 spin_unlock_irqrestore(&lan966x->ptp_ts_id_lock, flags); 409 410 return 0; 411 } 412 413 void lan966x_ptp_txtstamp_release(struct lan966x_port *port, 414 struct sk_buff *skb) 415 { 416 struct lan966x *lan966x = port->lan966x; 417 unsigned long flags; 418 419 spin_lock_irqsave(&lan966x->ptp_ts_id_lock, flags); 420 port->ts_id--; 421 lan966x->ptp_skbs--; 422 skb_unlink(skb, &port->tx_skbs); 423 spin_unlock_irqrestore(&lan966x->ptp_ts_id_lock, flags); 424 } 425 426 static void lan966x_get_hwtimestamp(struct lan966x *lan966x, 427 struct timespec64 *ts, 428 u32 nsec) 429 { 430 /* Read current PTP time to get seconds */ 431 unsigned long flags; 432 u32 curr_nsec; 433 434 spin_lock_irqsave(&lan966x->ptp_clock_lock, flags); 435 436 lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_SAVE) | 437 PTP_PIN_CFG_PIN_DOM_SET(LAN966X_PHC_PORT) | 438 PTP_PIN_CFG_PIN_SYNC_SET(0), 439 PTP_PIN_CFG_PIN_ACTION | 440 PTP_PIN_CFG_PIN_DOM | 441 PTP_PIN_CFG_PIN_SYNC, 442 lan966x, PTP_PIN_CFG(TOD_ACC_PIN)); 443 444 ts->tv_sec = lan_rd(lan966x, PTP_TOD_SEC_LSB(TOD_ACC_PIN)); 445 curr_nsec = lan_rd(lan966x, PTP_TOD_NSEC(TOD_ACC_PIN)); 446 447 ts->tv_nsec = nsec; 448 449 /* Sec has incremented since the ts was registered */ 450 if (curr_nsec < nsec) 451 ts->tv_sec--; 452 453 spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags); 454 } 455 456 irqreturn_t lan966x_ptp_irq_handler(int irq, void *args) 457 { 458 int budget = LAN966X_MAX_PTP_ID; 459 struct lan966x *lan966x = args; 460 461 while (budget--) { 462 struct sk_buff *skb, *skb_tmp, *skb_match = NULL; 463 struct skb_shared_hwtstamps shhwtstamps; 464 struct lan966x_port *port; 465 struct timespec64 ts; 466 unsigned long flags; 467 u32 val, id, txport; 468 u32 delay; 469 470 val = lan_rd(lan966x, PTP_TWOSTEP_CTRL); 471 472 /* Check if a timestamp can be retrieved */ 473 if (!(val & PTP_TWOSTEP_CTRL_VLD)) 474 break; 475 476 WARN_ON(val & PTP_TWOSTEP_CTRL_OVFL); 477 478 if (!(val & PTP_TWOSTEP_CTRL_STAMP_TX)) 479 continue; 480 481 /* Retrieve the ts Tx port */ 482 txport = PTP_TWOSTEP_CTRL_STAMP_PORT_GET(val); 483 484 /* Retrieve its associated skb */ 485 port = lan966x->ports[txport]; 486 487 /* Retrieve the delay */ 488 delay = lan_rd(lan966x, PTP_TWOSTEP_STAMP); 489 delay = PTP_TWOSTEP_STAMP_STAMP_NSEC_GET(delay); 490 491 /* Get next timestamp from fifo, which needs to be the 492 * rx timestamp which represents the id of the frame 493 */ 494 lan_rmw(PTP_TWOSTEP_CTRL_NXT_SET(1), 495 PTP_TWOSTEP_CTRL_NXT, 496 lan966x, PTP_TWOSTEP_CTRL); 497 498 val = lan_rd(lan966x, PTP_TWOSTEP_CTRL); 499 500 /* Check if a timestamp can be retried */ 501 if (!(val & PTP_TWOSTEP_CTRL_VLD)) 502 break; 503 504 /* Read RX timestamping to get the ID */ 505 id = lan_rd(lan966x, PTP_TWOSTEP_STAMP); 506 507 spin_lock_irqsave(&port->tx_skbs.lock, flags); 508 skb_queue_walk_safe(&port->tx_skbs, skb, skb_tmp) { 509 if (LAN966X_SKB_CB(skb)->ts_id != id) 510 continue; 511 512 __skb_unlink(skb, &port->tx_skbs); 513 skb_match = skb; 514 break; 515 } 516 spin_unlock_irqrestore(&port->tx_skbs.lock, flags); 517 518 /* Next ts */ 519 lan_rmw(PTP_TWOSTEP_CTRL_NXT_SET(1), 520 PTP_TWOSTEP_CTRL_NXT, 521 lan966x, PTP_TWOSTEP_CTRL); 522 523 if (WARN_ON(!skb_match)) 524 continue; 525 526 spin_lock_irqsave(&lan966x->ptp_ts_id_lock, flags); 527 lan966x->ptp_skbs--; 528 spin_unlock_irqrestore(&lan966x->ptp_ts_id_lock, flags); 529 530 /* Get the h/w timestamp */ 531 lan966x_get_hwtimestamp(lan966x, &ts, delay); 532 533 /* Set the timestamp into the skb */ 534 shhwtstamps.hwtstamp = ktime_set(ts.tv_sec, ts.tv_nsec); 535 skb_tstamp_tx(skb_match, &shhwtstamps); 536 537 dev_kfree_skb_any(skb_match); 538 } 539 540 return IRQ_HANDLED; 541 } 542 543 irqreturn_t lan966x_ptp_ext_irq_handler(int irq, void *args) 544 { 545 struct lan966x *lan966x = args; 546 struct lan966x_phc *phc; 547 unsigned long flags; 548 u64 time = 0; 549 time64_t s; 550 int pin, i; 551 s64 ns; 552 553 if (!(lan_rd(lan966x, PTP_PIN_INTR))) 554 return IRQ_NONE; 555 556 /* Go through all domains and see which pin generated the interrupt */ 557 for (i = 0; i < LAN966X_PHC_COUNT; ++i) { 558 struct ptp_clock_event ptp_event = {0}; 559 560 phc = &lan966x->phc[i]; 561 pin = ptp_find_pin_unlocked(phc->clock, PTP_PF_EXTTS, 0); 562 if (pin == -1) 563 continue; 564 565 if (!(lan_rd(lan966x, PTP_PIN_INTR) & BIT(pin))) 566 continue; 567 568 spin_lock_irqsave(&lan966x->ptp_clock_lock, flags); 569 570 /* Enable to get the new interrupt. 571 * By writing 1 it clears the bit 572 */ 573 lan_wr(BIT(pin), lan966x, PTP_PIN_INTR); 574 575 /* Get current time */ 576 s = lan_rd(lan966x, PTP_TOD_SEC_MSB(pin)); 577 s <<= 32; 578 s |= lan_rd(lan966x, PTP_TOD_SEC_LSB(pin)); 579 ns = lan_rd(lan966x, PTP_TOD_NSEC(pin)); 580 ns &= PTP_TOD_NSEC_TOD_NSEC; 581 582 spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags); 583 584 if ((ns & 0xFFFFFFF0) == 0x3FFFFFF0) { 585 s--; 586 ns &= 0xf; 587 ns += 999999984; 588 } 589 time = ktime_set(s, ns); 590 591 ptp_event.index = pin; 592 ptp_event.timestamp = time; 593 ptp_event.type = PTP_CLOCK_EXTTS; 594 ptp_clock_event(phc->clock, &ptp_event); 595 } 596 597 return IRQ_HANDLED; 598 } 599 600 static int lan966x_ptp_adjfine(struct ptp_clock_info *ptp, long scaled_ppm) 601 { 602 struct lan966x_phc *phc = container_of(ptp, struct lan966x_phc, info); 603 struct lan966x *lan966x = phc->lan966x; 604 unsigned long flags; 605 bool neg_adj = 0; 606 u64 tod_inc; 607 u64 ref; 608 609 if (!scaled_ppm) 610 return 0; 611 612 if (scaled_ppm < 0) { 613 neg_adj = 1; 614 scaled_ppm = -scaled_ppm; 615 } 616 617 tod_inc = lan966x_ptp_get_nominal_value(); 618 619 /* The multiplication is split in 2 separate additions because of 620 * overflow issues. If scaled_ppm with 16bit fractional part was bigger 621 * than 20ppm then we got overflow. 622 */ 623 ref = LAN966X_1PPM_FORMAT * (scaled_ppm >> 16); 624 ref += (LAN966X_1PPM_FORMAT * (0xffff & scaled_ppm)) >> 16; 625 tod_inc = neg_adj ? tod_inc - ref : tod_inc + ref; 626 627 spin_lock_irqsave(&lan966x->ptp_clock_lock, flags); 628 629 lan_rmw(PTP_DOM_CFG_CLKCFG_DIS_SET(1 << BIT(phc->index)), 630 PTP_DOM_CFG_CLKCFG_DIS, 631 lan966x, PTP_DOM_CFG); 632 633 lan_wr((u32)tod_inc & 0xFFFFFFFF, lan966x, 634 PTP_CLK_PER_CFG(phc->index, 0)); 635 lan_wr((u32)(tod_inc >> 32), lan966x, 636 PTP_CLK_PER_CFG(phc->index, 1)); 637 638 lan_rmw(PTP_DOM_CFG_CLKCFG_DIS_SET(0), 639 PTP_DOM_CFG_CLKCFG_DIS, 640 lan966x, PTP_DOM_CFG); 641 642 spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags); 643 644 return 0; 645 } 646 647 static int lan966x_ptp_settime64(struct ptp_clock_info *ptp, 648 const struct timespec64 *ts) 649 { 650 struct lan966x_phc *phc = container_of(ptp, struct lan966x_phc, info); 651 struct lan966x *lan966x = phc->lan966x; 652 unsigned long flags; 653 654 spin_lock_irqsave(&lan966x->ptp_clock_lock, flags); 655 656 /* Must be in IDLE mode before the time can be loaded */ 657 lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_IDLE) | 658 PTP_PIN_CFG_PIN_DOM_SET(phc->index) | 659 PTP_PIN_CFG_PIN_SYNC_SET(0), 660 PTP_PIN_CFG_PIN_ACTION | 661 PTP_PIN_CFG_PIN_DOM | 662 PTP_PIN_CFG_PIN_SYNC, 663 lan966x, PTP_PIN_CFG(TOD_ACC_PIN)); 664 665 /* Set new value */ 666 lan_wr(PTP_TOD_SEC_MSB_TOD_SEC_MSB_SET(upper_32_bits(ts->tv_sec)), 667 lan966x, PTP_TOD_SEC_MSB(TOD_ACC_PIN)); 668 lan_wr(lower_32_bits(ts->tv_sec), 669 lan966x, PTP_TOD_SEC_LSB(TOD_ACC_PIN)); 670 lan_wr(ts->tv_nsec, lan966x, PTP_TOD_NSEC(TOD_ACC_PIN)); 671 672 /* Apply new values */ 673 lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_LOAD) | 674 PTP_PIN_CFG_PIN_DOM_SET(phc->index) | 675 PTP_PIN_CFG_PIN_SYNC_SET(0), 676 PTP_PIN_CFG_PIN_ACTION | 677 PTP_PIN_CFG_PIN_DOM | 678 PTP_PIN_CFG_PIN_SYNC, 679 lan966x, PTP_PIN_CFG(TOD_ACC_PIN)); 680 681 spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags); 682 683 return 0; 684 } 685 686 int lan966x_ptp_gettime64(struct ptp_clock_info *ptp, struct timespec64 *ts) 687 { 688 struct lan966x_phc *phc = container_of(ptp, struct lan966x_phc, info); 689 struct lan966x *lan966x = phc->lan966x; 690 unsigned long flags; 691 time64_t s; 692 s64 ns; 693 694 spin_lock_irqsave(&lan966x->ptp_clock_lock, flags); 695 696 lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_SAVE) | 697 PTP_PIN_CFG_PIN_DOM_SET(phc->index) | 698 PTP_PIN_CFG_PIN_SYNC_SET(0), 699 PTP_PIN_CFG_PIN_ACTION | 700 PTP_PIN_CFG_PIN_DOM | 701 PTP_PIN_CFG_PIN_SYNC, 702 lan966x, PTP_PIN_CFG(TOD_ACC_PIN)); 703 704 s = lan_rd(lan966x, PTP_TOD_SEC_MSB(TOD_ACC_PIN)); 705 s <<= 32; 706 s |= lan_rd(lan966x, PTP_TOD_SEC_LSB(TOD_ACC_PIN)); 707 ns = lan_rd(lan966x, PTP_TOD_NSEC(TOD_ACC_PIN)); 708 ns &= PTP_TOD_NSEC_TOD_NSEC; 709 710 spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags); 711 712 /* Deal with negative values */ 713 if ((ns & 0xFFFFFFF0) == 0x3FFFFFF0) { 714 s--; 715 ns &= 0xf; 716 ns += 999999984; 717 } 718 719 set_normalized_timespec64(ts, s, ns); 720 return 0; 721 } 722 723 static int lan966x_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta) 724 { 725 struct lan966x_phc *phc = container_of(ptp, struct lan966x_phc, info); 726 struct lan966x *lan966x = phc->lan966x; 727 728 if (delta > -(NSEC_PER_SEC / 2) && delta < (NSEC_PER_SEC / 2)) { 729 unsigned long flags; 730 731 spin_lock_irqsave(&lan966x->ptp_clock_lock, flags); 732 733 /* Must be in IDLE mode before the time can be loaded */ 734 lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_IDLE) | 735 PTP_PIN_CFG_PIN_DOM_SET(phc->index) | 736 PTP_PIN_CFG_PIN_SYNC_SET(0), 737 PTP_PIN_CFG_PIN_ACTION | 738 PTP_PIN_CFG_PIN_DOM | 739 PTP_PIN_CFG_PIN_SYNC, 740 lan966x, PTP_PIN_CFG(TOD_ACC_PIN)); 741 742 lan_wr(PTP_TOD_NSEC_TOD_NSEC_SET(delta), 743 lan966x, PTP_TOD_NSEC(TOD_ACC_PIN)); 744 745 /* Adjust time with the value of PTP_TOD_NSEC */ 746 lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_DELTA) | 747 PTP_PIN_CFG_PIN_DOM_SET(phc->index) | 748 PTP_PIN_CFG_PIN_SYNC_SET(0), 749 PTP_PIN_CFG_PIN_ACTION | 750 PTP_PIN_CFG_PIN_DOM | 751 PTP_PIN_CFG_PIN_SYNC, 752 lan966x, PTP_PIN_CFG(TOD_ACC_PIN)); 753 754 spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags); 755 } else { 756 /* Fall back using lan966x_ptp_settime64 which is not exact */ 757 struct timespec64 ts; 758 u64 now; 759 760 lan966x_ptp_gettime64(ptp, &ts); 761 762 now = ktime_to_ns(timespec64_to_ktime(ts)); 763 ts = ns_to_timespec64(now + delta); 764 765 lan966x_ptp_settime64(ptp, &ts); 766 } 767 768 return 0; 769 } 770 771 static int lan966x_ptp_verify(struct ptp_clock_info *ptp, unsigned int pin, 772 enum ptp_pin_function func, unsigned int chan) 773 { 774 struct lan966x_phc *phc = container_of(ptp, struct lan966x_phc, info); 775 struct lan966x *lan966x = phc->lan966x; 776 struct ptp_clock_info *info; 777 int i; 778 779 /* Currently support only 1 channel */ 780 if (chan != 0) 781 return -1; 782 783 switch (func) { 784 case PTP_PF_NONE: 785 case PTP_PF_PEROUT: 786 case PTP_PF_EXTTS: 787 break; 788 default: 789 return -1; 790 } 791 792 /* The PTP pins are shared by all the PHC. So it is required to see if 793 * the pin is connected to another PHC. The pin is connected to another 794 * PHC if that pin already has a function on that PHC. 795 */ 796 for (i = 0; i < LAN966X_PHC_COUNT; ++i) { 797 info = &lan966x->phc[i].info; 798 799 /* Ignore the check with ourself */ 800 if (ptp == info) 801 continue; 802 803 if (info->pin_config[pin].func == PTP_PF_PEROUT || 804 info->pin_config[pin].func == PTP_PF_EXTTS) 805 return -1; 806 } 807 808 return 0; 809 } 810 811 static int lan966x_ptp_perout(struct ptp_clock_info *ptp, 812 struct ptp_clock_request *rq, int on) 813 { 814 struct lan966x_phc *phc = container_of(ptp, struct lan966x_phc, info); 815 struct lan966x *lan966x = phc->lan966x; 816 struct timespec64 ts_phase, ts_period; 817 unsigned long flags; 818 s64 wf_high, wf_low; 819 bool pps = false; 820 int pin; 821 822 if (rq->perout.flags & ~(PTP_PEROUT_DUTY_CYCLE | 823 PTP_PEROUT_PHASE)) 824 return -EOPNOTSUPP; 825 826 pin = ptp_find_pin(phc->clock, PTP_PF_PEROUT, rq->perout.index); 827 if (pin == -1 || pin >= LAN966X_PHC_PINS_NUM) 828 return -EINVAL; 829 830 if (!on) { 831 spin_lock_irqsave(&lan966x->ptp_clock_lock, flags); 832 lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_IDLE) | 833 PTP_PIN_CFG_PIN_DOM_SET(phc->index) | 834 PTP_PIN_CFG_PIN_SYNC_SET(0), 835 PTP_PIN_CFG_PIN_ACTION | 836 PTP_PIN_CFG_PIN_DOM | 837 PTP_PIN_CFG_PIN_SYNC, 838 lan966x, PTP_PIN_CFG(pin)); 839 spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags); 840 return 0; 841 } 842 843 if (rq->perout.period.sec == 1 && 844 rq->perout.period.nsec == 0) 845 pps = true; 846 847 if (rq->perout.flags & PTP_PEROUT_PHASE) { 848 ts_phase.tv_sec = rq->perout.phase.sec; 849 ts_phase.tv_nsec = rq->perout.phase.nsec; 850 } else { 851 ts_phase.tv_sec = rq->perout.start.sec; 852 ts_phase.tv_nsec = rq->perout.start.nsec; 853 } 854 855 if (ts_phase.tv_sec || (ts_phase.tv_nsec && !pps)) { 856 dev_warn(lan966x->dev, 857 "Absolute time not supported!\n"); 858 return -EINVAL; 859 } 860 861 if (rq->perout.flags & PTP_PEROUT_DUTY_CYCLE) { 862 struct timespec64 ts_on; 863 864 ts_on.tv_sec = rq->perout.on.sec; 865 ts_on.tv_nsec = rq->perout.on.nsec; 866 867 wf_high = timespec64_to_ns(&ts_on); 868 } else { 869 wf_high = 5000; 870 } 871 872 if (pps) { 873 spin_lock_irqsave(&lan966x->ptp_clock_lock, flags); 874 lan_wr(PTP_WF_LOW_PERIOD_PIN_WFL(ts_phase.tv_nsec), 875 lan966x, PTP_WF_LOW_PERIOD(pin)); 876 lan_wr(PTP_WF_HIGH_PERIOD_PIN_WFH(wf_high), 877 lan966x, PTP_WF_HIGH_PERIOD(pin)); 878 lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_CLOCK) | 879 PTP_PIN_CFG_PIN_DOM_SET(phc->index) | 880 PTP_PIN_CFG_PIN_SYNC_SET(3), 881 PTP_PIN_CFG_PIN_ACTION | 882 PTP_PIN_CFG_PIN_DOM | 883 PTP_PIN_CFG_PIN_SYNC, 884 lan966x, PTP_PIN_CFG(pin)); 885 spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags); 886 return 0; 887 } 888 889 ts_period.tv_sec = rq->perout.period.sec; 890 ts_period.tv_nsec = rq->perout.period.nsec; 891 892 wf_low = timespec64_to_ns(&ts_period); 893 wf_low -= wf_high; 894 895 spin_lock_irqsave(&lan966x->ptp_clock_lock, flags); 896 lan_wr(PTP_WF_LOW_PERIOD_PIN_WFL(wf_low), 897 lan966x, PTP_WF_LOW_PERIOD(pin)); 898 lan_wr(PTP_WF_HIGH_PERIOD_PIN_WFH(wf_high), 899 lan966x, PTP_WF_HIGH_PERIOD(pin)); 900 lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_CLOCK) | 901 PTP_PIN_CFG_PIN_DOM_SET(phc->index) | 902 PTP_PIN_CFG_PIN_SYNC_SET(0), 903 PTP_PIN_CFG_PIN_ACTION | 904 PTP_PIN_CFG_PIN_DOM | 905 PTP_PIN_CFG_PIN_SYNC, 906 lan966x, PTP_PIN_CFG(pin)); 907 spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags); 908 909 return 0; 910 } 911 912 static int lan966x_ptp_extts(struct ptp_clock_info *ptp, 913 struct ptp_clock_request *rq, int on) 914 { 915 struct lan966x_phc *phc = container_of(ptp, struct lan966x_phc, info); 916 struct lan966x *lan966x = phc->lan966x; 917 unsigned long flags; 918 int pin; 919 u32 val; 920 921 if (lan966x->ptp_ext_irq <= 0) 922 return -EOPNOTSUPP; 923 924 /* Reject requests with unsupported flags */ 925 if (rq->extts.flags & ~(PTP_ENABLE_FEATURE | 926 PTP_RISING_EDGE | 927 PTP_STRICT_FLAGS)) 928 return -EOPNOTSUPP; 929 930 pin = ptp_find_pin(phc->clock, PTP_PF_EXTTS, rq->extts.index); 931 if (pin == -1 || pin >= LAN966X_PHC_PINS_NUM) 932 return -EINVAL; 933 934 spin_lock_irqsave(&lan966x->ptp_clock_lock, flags); 935 lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_SAVE) | 936 PTP_PIN_CFG_PIN_SYNC_SET(on ? 3 : 0) | 937 PTP_PIN_CFG_PIN_DOM_SET(phc->index) | 938 PTP_PIN_CFG_PIN_SELECT_SET(pin), 939 PTP_PIN_CFG_PIN_ACTION | 940 PTP_PIN_CFG_PIN_SYNC | 941 PTP_PIN_CFG_PIN_DOM | 942 PTP_PIN_CFG_PIN_SELECT, 943 lan966x, PTP_PIN_CFG(pin)); 944 945 val = lan_rd(lan966x, PTP_PIN_INTR_ENA); 946 if (on) 947 val |= BIT(pin); 948 else 949 val &= ~BIT(pin); 950 lan_wr(val, lan966x, PTP_PIN_INTR_ENA); 951 952 spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags); 953 954 return 0; 955 } 956 957 static int lan966x_ptp_enable(struct ptp_clock_info *ptp, 958 struct ptp_clock_request *rq, int on) 959 { 960 switch (rq->type) { 961 case PTP_CLK_REQ_PEROUT: 962 return lan966x_ptp_perout(ptp, rq, on); 963 case PTP_CLK_REQ_EXTTS: 964 return lan966x_ptp_extts(ptp, rq, on); 965 default: 966 return -EOPNOTSUPP; 967 } 968 969 return 0; 970 } 971 972 static struct ptp_clock_info lan966x_ptp_clock_info = { 973 .owner = THIS_MODULE, 974 .name = "lan966x ptp", 975 .max_adj = 200000, 976 .gettime64 = lan966x_ptp_gettime64, 977 .settime64 = lan966x_ptp_settime64, 978 .adjtime = lan966x_ptp_adjtime, 979 .adjfine = lan966x_ptp_adjfine, 980 .verify = lan966x_ptp_verify, 981 .enable = lan966x_ptp_enable, 982 .n_per_out = LAN966X_PHC_PINS_NUM, 983 .n_ext_ts = LAN966X_PHC_PINS_NUM, 984 .n_pins = LAN966X_PHC_PINS_NUM, 985 }; 986 987 static int lan966x_ptp_phc_init(struct lan966x *lan966x, 988 int index, 989 struct ptp_clock_info *clock_info) 990 { 991 struct lan966x_phc *phc = &lan966x->phc[index]; 992 struct ptp_pin_desc *p; 993 int i; 994 995 for (i = 0; i < LAN966X_PHC_PINS_NUM; i++) { 996 p = &phc->pins[i]; 997 998 snprintf(p->name, sizeof(p->name), "pin%d", i); 999 p->index = i; 1000 p->func = PTP_PF_NONE; 1001 } 1002 1003 phc->info = *clock_info; 1004 phc->info.pin_config = &phc->pins[0]; 1005 phc->clock = ptp_clock_register(&phc->info, lan966x->dev); 1006 if (IS_ERR(phc->clock)) 1007 return PTR_ERR(phc->clock); 1008 1009 phc->index = index; 1010 phc->lan966x = lan966x; 1011 1012 /* PTP Rx stamping is always enabled. */ 1013 phc->hwtstamp_config.rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT; 1014 1015 return 0; 1016 } 1017 1018 int lan966x_ptp_init(struct lan966x *lan966x) 1019 { 1020 u64 tod_adj = lan966x_ptp_get_nominal_value(); 1021 struct lan966x_port *port; 1022 int err, i; 1023 1024 if (!lan966x->ptp) 1025 return 0; 1026 1027 for (i = 0; i < LAN966X_PHC_COUNT; ++i) { 1028 err = lan966x_ptp_phc_init(lan966x, i, &lan966x_ptp_clock_info); 1029 if (err) 1030 return err; 1031 } 1032 1033 spin_lock_init(&lan966x->ptp_clock_lock); 1034 spin_lock_init(&lan966x->ptp_ts_id_lock); 1035 mutex_init(&lan966x->ptp_lock); 1036 1037 /* Disable master counters */ 1038 lan_wr(PTP_DOM_CFG_ENA_SET(0), lan966x, PTP_DOM_CFG); 1039 1040 /* Configure the nominal TOD increment per clock cycle */ 1041 lan_rmw(PTP_DOM_CFG_CLKCFG_DIS_SET(0x7), 1042 PTP_DOM_CFG_CLKCFG_DIS, 1043 lan966x, PTP_DOM_CFG); 1044 1045 for (i = 0; i < LAN966X_PHC_COUNT; ++i) { 1046 lan_wr((u32)tod_adj & 0xFFFFFFFF, lan966x, 1047 PTP_CLK_PER_CFG(i, 0)); 1048 lan_wr((u32)(tod_adj >> 32), lan966x, 1049 PTP_CLK_PER_CFG(i, 1)); 1050 } 1051 1052 lan_rmw(PTP_DOM_CFG_CLKCFG_DIS_SET(0), 1053 PTP_DOM_CFG_CLKCFG_DIS, 1054 lan966x, PTP_DOM_CFG); 1055 1056 /* Enable master counters */ 1057 lan_wr(PTP_DOM_CFG_ENA_SET(0x7), lan966x, PTP_DOM_CFG); 1058 1059 for (i = 0; i < lan966x->num_phys_ports; i++) { 1060 port = lan966x->ports[i]; 1061 if (!port) 1062 continue; 1063 1064 skb_queue_head_init(&port->tx_skbs); 1065 } 1066 1067 return 0; 1068 } 1069 1070 void lan966x_ptp_deinit(struct lan966x *lan966x) 1071 { 1072 struct lan966x_port *port; 1073 int i; 1074 1075 if (!lan966x->ptp) 1076 return; 1077 1078 for (i = 0; i < lan966x->num_phys_ports; i++) { 1079 port = lan966x->ports[i]; 1080 if (!port) 1081 continue; 1082 1083 skb_queue_purge(&port->tx_skbs); 1084 } 1085 1086 for (i = 0; i < LAN966X_PHC_COUNT; ++i) 1087 ptp_clock_unregister(lan966x->phc[i].clock); 1088 } 1089 1090 void lan966x_ptp_rxtstamp(struct lan966x *lan966x, struct sk_buff *skb, 1091 u64 timestamp) 1092 { 1093 struct skb_shared_hwtstamps *shhwtstamps; 1094 struct lan966x_phc *phc; 1095 struct timespec64 ts; 1096 u64 full_ts_in_ns; 1097 1098 if (!lan966x->ptp) 1099 return; 1100 1101 phc = &lan966x->phc[LAN966X_PHC_PORT]; 1102 lan966x_ptp_gettime64(&phc->info, &ts); 1103 1104 /* Drop the sub-ns precision */ 1105 timestamp = timestamp >> 2; 1106 if (ts.tv_nsec < timestamp) 1107 ts.tv_sec--; 1108 ts.tv_nsec = timestamp; 1109 full_ts_in_ns = ktime_set(ts.tv_sec, ts.tv_nsec); 1110 1111 shhwtstamps = skb_hwtstamps(skb); 1112 shhwtstamps->hwtstamp = full_ts_in_ns; 1113 } 1114 1115 u32 lan966x_ptp_get_period_ps(void) 1116 { 1117 /* This represents the system clock period in picoseconds */ 1118 return 15125; 1119 } 1120