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 (!IS_ERR(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 (IS_ERR(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, 252 struct kernel_hwtstamp_config *cfg) 253 { 254 if (cfg->rx_filter == HWTSTAMP_FILTER_NONE) 255 return lan966x_ptp_del_traps(port); 256 else 257 return lan966x_ptp_add_traps(port); 258 } 259 260 int lan966x_ptp_hwtstamp_set(struct lan966x_port *port, 261 struct kernel_hwtstamp_config *cfg, 262 struct netlink_ext_ack *extack) 263 { 264 struct lan966x *lan966x = port->lan966x; 265 struct lan966x_phc *phc; 266 267 switch (cfg->tx_type) { 268 case HWTSTAMP_TX_ON: 269 port->ptp_tx_cmd = IFH_REW_OP_TWO_STEP_PTP; 270 break; 271 case HWTSTAMP_TX_ONESTEP_SYNC: 272 port->ptp_tx_cmd = IFH_REW_OP_ONE_STEP_PTP; 273 break; 274 case HWTSTAMP_TX_OFF: 275 port->ptp_tx_cmd = IFH_REW_OP_NOOP; 276 break; 277 default: 278 return -ERANGE; 279 } 280 281 switch (cfg->rx_filter) { 282 case HWTSTAMP_FILTER_NONE: 283 port->ptp_rx_cmd = false; 284 break; 285 case HWTSTAMP_FILTER_ALL: 286 case HWTSTAMP_FILTER_PTP_V1_L4_EVENT: 287 case HWTSTAMP_FILTER_PTP_V1_L4_SYNC: 288 case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ: 289 case HWTSTAMP_FILTER_PTP_V2_L4_EVENT: 290 case HWTSTAMP_FILTER_PTP_V2_L4_SYNC: 291 case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ: 292 case HWTSTAMP_FILTER_PTP_V2_L2_EVENT: 293 case HWTSTAMP_FILTER_PTP_V2_L2_SYNC: 294 case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ: 295 case HWTSTAMP_FILTER_PTP_V2_EVENT: 296 case HWTSTAMP_FILTER_PTP_V2_SYNC: 297 case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ: 298 case HWTSTAMP_FILTER_NTP_ALL: 299 port->ptp_rx_cmd = true; 300 cfg->rx_filter = HWTSTAMP_FILTER_ALL; 301 break; 302 default: 303 return -ERANGE; 304 } 305 306 /* Commit back the result & save it */ 307 mutex_lock(&lan966x->ptp_lock); 308 phc = &lan966x->phc[LAN966X_PHC_PORT]; 309 phc->hwtstamp_config = *cfg; 310 mutex_unlock(&lan966x->ptp_lock); 311 312 return 0; 313 } 314 315 void lan966x_ptp_hwtstamp_get(struct lan966x_port *port, 316 struct kernel_hwtstamp_config *cfg) 317 { 318 struct lan966x *lan966x = port->lan966x; 319 struct lan966x_phc *phc; 320 321 phc = &lan966x->phc[LAN966X_PHC_PORT]; 322 *cfg = phc->hwtstamp_config; 323 } 324 325 static int lan966x_ptp_classify(struct lan966x_port *port, struct sk_buff *skb) 326 { 327 struct ptp_header *header; 328 u8 msgtype; 329 int type; 330 331 if (port->ptp_tx_cmd == IFH_REW_OP_NOOP) 332 return IFH_REW_OP_NOOP; 333 334 type = ptp_classify_raw(skb); 335 if (type == PTP_CLASS_NONE) 336 return IFH_REW_OP_NOOP; 337 338 header = ptp_parse_header(skb, type); 339 if (!header) 340 return IFH_REW_OP_NOOP; 341 342 if (port->ptp_tx_cmd == IFH_REW_OP_TWO_STEP_PTP) 343 return IFH_REW_OP_TWO_STEP_PTP; 344 345 /* If it is sync and run 1 step then set the correct operation, 346 * otherwise run as 2 step 347 */ 348 msgtype = ptp_get_msgtype(header, type); 349 if ((msgtype & 0xf) == 0) 350 return IFH_REW_OP_ONE_STEP_PTP; 351 352 return IFH_REW_OP_TWO_STEP_PTP; 353 } 354 355 static void lan966x_ptp_txtstamp_old_release(struct lan966x_port *port) 356 { 357 struct sk_buff *skb, *skb_tmp; 358 unsigned long flags; 359 360 spin_lock_irqsave(&port->tx_skbs.lock, flags); 361 skb_queue_walk_safe(&port->tx_skbs, skb, skb_tmp) { 362 if time_after(LAN966X_SKB_CB(skb)->jiffies + LAN966X_PTP_TIMEOUT, 363 jiffies) 364 break; 365 366 __skb_unlink(skb, &port->tx_skbs); 367 dev_kfree_skb_any(skb); 368 } 369 spin_unlock_irqrestore(&port->tx_skbs.lock, flags); 370 } 371 372 int lan966x_ptp_txtstamp_request(struct lan966x_port *port, 373 struct sk_buff *skb) 374 { 375 struct lan966x *lan966x = port->lan966x; 376 unsigned long flags; 377 u8 rew_op; 378 379 rew_op = lan966x_ptp_classify(port, skb); 380 LAN966X_SKB_CB(skb)->rew_op = rew_op; 381 382 if (rew_op != IFH_REW_OP_TWO_STEP_PTP) 383 return 0; 384 385 lan966x_ptp_txtstamp_old_release(port); 386 387 spin_lock_irqsave(&lan966x->ptp_ts_id_lock, flags); 388 if (lan966x->ptp_skbs == LAN966X_MAX_PTP_ID) { 389 spin_unlock_irqrestore(&lan966x->ptp_ts_id_lock, flags); 390 return -EBUSY; 391 } 392 393 skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS; 394 395 skb_queue_tail(&port->tx_skbs, skb); 396 LAN966X_SKB_CB(skb)->ts_id = port->ts_id; 397 LAN966X_SKB_CB(skb)->jiffies = jiffies; 398 399 lan966x->ptp_skbs++; 400 port->ts_id++; 401 if (port->ts_id == LAN966X_MAX_PTP_ID) 402 port->ts_id = 0; 403 404 spin_unlock_irqrestore(&lan966x->ptp_ts_id_lock, flags); 405 406 return 0; 407 } 408 409 void lan966x_ptp_txtstamp_release(struct lan966x_port *port, 410 struct sk_buff *skb) 411 { 412 struct lan966x *lan966x = port->lan966x; 413 unsigned long flags; 414 415 spin_lock_irqsave(&lan966x->ptp_ts_id_lock, flags); 416 port->ts_id--; 417 lan966x->ptp_skbs--; 418 skb_unlink(skb, &port->tx_skbs); 419 spin_unlock_irqrestore(&lan966x->ptp_ts_id_lock, flags); 420 } 421 422 static void lan966x_get_hwtimestamp(struct lan966x *lan966x, 423 struct timespec64 *ts, 424 u32 nsec) 425 { 426 /* Read current PTP time to get seconds */ 427 unsigned long flags; 428 u32 curr_nsec; 429 430 spin_lock_irqsave(&lan966x->ptp_clock_lock, flags); 431 432 lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_SAVE) | 433 PTP_PIN_CFG_PIN_DOM_SET(LAN966X_PHC_PORT) | 434 PTP_PIN_CFG_PIN_SYNC_SET(0), 435 PTP_PIN_CFG_PIN_ACTION | 436 PTP_PIN_CFG_PIN_DOM | 437 PTP_PIN_CFG_PIN_SYNC, 438 lan966x, PTP_PIN_CFG(TOD_ACC_PIN)); 439 440 ts->tv_sec = lan_rd(lan966x, PTP_TOD_SEC_LSB(TOD_ACC_PIN)); 441 curr_nsec = lan_rd(lan966x, PTP_TOD_NSEC(TOD_ACC_PIN)); 442 443 ts->tv_nsec = nsec; 444 445 /* Sec has incremented since the ts was registered */ 446 if (curr_nsec < nsec) 447 ts->tv_sec--; 448 449 spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags); 450 } 451 452 irqreturn_t lan966x_ptp_irq_handler(int irq, void *args) 453 { 454 int budget = LAN966X_MAX_PTP_ID; 455 struct lan966x *lan966x = args; 456 457 while (budget--) { 458 struct sk_buff *skb, *skb_tmp, *skb_match = NULL; 459 struct skb_shared_hwtstamps shhwtstamps; 460 struct lan966x_port *port; 461 struct timespec64 ts; 462 unsigned long flags; 463 u32 val, id, txport; 464 u32 delay; 465 466 val = lan_rd(lan966x, PTP_TWOSTEP_CTRL); 467 468 /* Check if a timestamp can be retrieved */ 469 if (!(val & PTP_TWOSTEP_CTRL_VLD)) 470 break; 471 472 WARN_ON(val & PTP_TWOSTEP_CTRL_OVFL); 473 474 if (!(val & PTP_TWOSTEP_CTRL_STAMP_TX)) 475 continue; 476 477 /* Retrieve the ts Tx port */ 478 txport = PTP_TWOSTEP_CTRL_STAMP_PORT_GET(val); 479 480 /* Retrieve its associated skb */ 481 port = lan966x->ports[txport]; 482 483 /* Retrieve the delay */ 484 delay = lan_rd(lan966x, PTP_TWOSTEP_STAMP); 485 delay = PTP_TWOSTEP_STAMP_STAMP_NSEC_GET(delay); 486 487 /* Get next timestamp from fifo, which needs to be the 488 * rx timestamp which represents the id of the frame 489 */ 490 lan_rmw(PTP_TWOSTEP_CTRL_NXT_SET(1), 491 PTP_TWOSTEP_CTRL_NXT, 492 lan966x, PTP_TWOSTEP_CTRL); 493 494 val = lan_rd(lan966x, PTP_TWOSTEP_CTRL); 495 496 /* Check if a timestamp can be retried */ 497 if (!(val & PTP_TWOSTEP_CTRL_VLD)) 498 break; 499 500 /* Read RX timestamping to get the ID */ 501 id = lan_rd(lan966x, PTP_TWOSTEP_STAMP); 502 503 spin_lock_irqsave(&port->tx_skbs.lock, flags); 504 skb_queue_walk_safe(&port->tx_skbs, skb, skb_tmp) { 505 if (LAN966X_SKB_CB(skb)->ts_id != id) 506 continue; 507 508 __skb_unlink(skb, &port->tx_skbs); 509 skb_match = skb; 510 break; 511 } 512 spin_unlock_irqrestore(&port->tx_skbs.lock, flags); 513 514 /* Next ts */ 515 lan_rmw(PTP_TWOSTEP_CTRL_NXT_SET(1), 516 PTP_TWOSTEP_CTRL_NXT, 517 lan966x, PTP_TWOSTEP_CTRL); 518 519 if (WARN_ON(!skb_match)) 520 continue; 521 522 spin_lock_irqsave(&lan966x->ptp_ts_id_lock, flags); 523 lan966x->ptp_skbs--; 524 spin_unlock_irqrestore(&lan966x->ptp_ts_id_lock, flags); 525 526 /* Get the h/w timestamp */ 527 lan966x_get_hwtimestamp(lan966x, &ts, delay); 528 529 /* Set the timestamp into the skb */ 530 shhwtstamps.hwtstamp = ktime_set(ts.tv_sec, ts.tv_nsec); 531 skb_tstamp_tx(skb_match, &shhwtstamps); 532 533 dev_kfree_skb_any(skb_match); 534 } 535 536 return IRQ_HANDLED; 537 } 538 539 irqreturn_t lan966x_ptp_ext_irq_handler(int irq, void *args) 540 { 541 struct lan966x *lan966x = args; 542 struct lan966x_phc *phc; 543 unsigned long flags; 544 u64 time = 0; 545 time64_t s; 546 int pin, i; 547 s64 ns; 548 549 if (!(lan_rd(lan966x, PTP_PIN_INTR))) 550 return IRQ_NONE; 551 552 /* Go through all domains and see which pin generated the interrupt */ 553 for (i = 0; i < LAN966X_PHC_COUNT; ++i) { 554 struct ptp_clock_event ptp_event = {0}; 555 556 phc = &lan966x->phc[i]; 557 pin = ptp_find_pin_unlocked(phc->clock, PTP_PF_EXTTS, 0); 558 if (pin == -1) 559 continue; 560 561 if (!(lan_rd(lan966x, PTP_PIN_INTR) & BIT(pin))) 562 continue; 563 564 spin_lock_irqsave(&lan966x->ptp_clock_lock, flags); 565 566 /* Enable to get the new interrupt. 567 * By writing 1 it clears the bit 568 */ 569 lan_wr(BIT(pin), lan966x, PTP_PIN_INTR); 570 571 /* Get current time */ 572 s = lan_rd(lan966x, PTP_TOD_SEC_MSB(pin)); 573 s <<= 32; 574 s |= lan_rd(lan966x, PTP_TOD_SEC_LSB(pin)); 575 ns = lan_rd(lan966x, PTP_TOD_NSEC(pin)); 576 ns &= PTP_TOD_NSEC_TOD_NSEC; 577 578 spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags); 579 580 if ((ns & 0xFFFFFFF0) == 0x3FFFFFF0) { 581 s--; 582 ns &= 0xf; 583 ns += 999999984; 584 } 585 time = ktime_set(s, ns); 586 587 ptp_event.index = pin; 588 ptp_event.timestamp = time; 589 ptp_event.type = PTP_CLOCK_EXTTS; 590 ptp_clock_event(phc->clock, &ptp_event); 591 } 592 593 return IRQ_HANDLED; 594 } 595 596 static int lan966x_ptp_adjfine(struct ptp_clock_info *ptp, long scaled_ppm) 597 { 598 struct lan966x_phc *phc = container_of(ptp, struct lan966x_phc, info); 599 struct lan966x *lan966x = phc->lan966x; 600 unsigned long flags; 601 bool neg_adj = 0; 602 u64 tod_inc; 603 u64 ref; 604 605 if (!scaled_ppm) 606 return 0; 607 608 if (scaled_ppm < 0) { 609 neg_adj = 1; 610 scaled_ppm = -scaled_ppm; 611 } 612 613 tod_inc = lan966x_ptp_get_nominal_value(); 614 615 /* The multiplication is split in 2 separate additions because of 616 * overflow issues. If scaled_ppm with 16bit fractional part was bigger 617 * than 20ppm then we got overflow. 618 */ 619 ref = LAN966X_1PPM_FORMAT * (scaled_ppm >> 16); 620 ref += (LAN966X_1PPM_FORMAT * (0xffff & scaled_ppm)) >> 16; 621 tod_inc = neg_adj ? tod_inc - ref : tod_inc + ref; 622 623 spin_lock_irqsave(&lan966x->ptp_clock_lock, flags); 624 625 lan_rmw(PTP_DOM_CFG_CLKCFG_DIS_SET(1 << BIT(phc->index)), 626 PTP_DOM_CFG_CLKCFG_DIS, 627 lan966x, PTP_DOM_CFG); 628 629 lan_wr((u32)tod_inc & 0xFFFFFFFF, lan966x, 630 PTP_CLK_PER_CFG(phc->index, 0)); 631 lan_wr((u32)(tod_inc >> 32), lan966x, 632 PTP_CLK_PER_CFG(phc->index, 1)); 633 634 lan_rmw(PTP_DOM_CFG_CLKCFG_DIS_SET(0), 635 PTP_DOM_CFG_CLKCFG_DIS, 636 lan966x, PTP_DOM_CFG); 637 638 spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags); 639 640 return 0; 641 } 642 643 static int lan966x_ptp_settime64(struct ptp_clock_info *ptp, 644 const struct timespec64 *ts) 645 { 646 struct lan966x_phc *phc = container_of(ptp, struct lan966x_phc, info); 647 struct lan966x *lan966x = phc->lan966x; 648 unsigned long flags; 649 650 spin_lock_irqsave(&lan966x->ptp_clock_lock, flags); 651 652 /* Must be in IDLE mode before the time can be loaded */ 653 lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_IDLE) | 654 PTP_PIN_CFG_PIN_DOM_SET(phc->index) | 655 PTP_PIN_CFG_PIN_SYNC_SET(0), 656 PTP_PIN_CFG_PIN_ACTION | 657 PTP_PIN_CFG_PIN_DOM | 658 PTP_PIN_CFG_PIN_SYNC, 659 lan966x, PTP_PIN_CFG(TOD_ACC_PIN)); 660 661 /* Set new value */ 662 lan_wr(PTP_TOD_SEC_MSB_TOD_SEC_MSB_SET(upper_32_bits(ts->tv_sec)), 663 lan966x, PTP_TOD_SEC_MSB(TOD_ACC_PIN)); 664 lan_wr(lower_32_bits(ts->tv_sec), 665 lan966x, PTP_TOD_SEC_LSB(TOD_ACC_PIN)); 666 lan_wr(ts->tv_nsec, lan966x, PTP_TOD_NSEC(TOD_ACC_PIN)); 667 668 /* Apply new values */ 669 lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_LOAD) | 670 PTP_PIN_CFG_PIN_DOM_SET(phc->index) | 671 PTP_PIN_CFG_PIN_SYNC_SET(0), 672 PTP_PIN_CFG_PIN_ACTION | 673 PTP_PIN_CFG_PIN_DOM | 674 PTP_PIN_CFG_PIN_SYNC, 675 lan966x, PTP_PIN_CFG(TOD_ACC_PIN)); 676 677 spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags); 678 679 return 0; 680 } 681 682 int lan966x_ptp_gettime64(struct ptp_clock_info *ptp, struct timespec64 *ts) 683 { 684 struct lan966x_phc *phc = container_of(ptp, struct lan966x_phc, info); 685 struct lan966x *lan966x = phc->lan966x; 686 unsigned long flags; 687 time64_t s; 688 s64 ns; 689 690 spin_lock_irqsave(&lan966x->ptp_clock_lock, flags); 691 692 lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_SAVE) | 693 PTP_PIN_CFG_PIN_DOM_SET(phc->index) | 694 PTP_PIN_CFG_PIN_SYNC_SET(0), 695 PTP_PIN_CFG_PIN_ACTION | 696 PTP_PIN_CFG_PIN_DOM | 697 PTP_PIN_CFG_PIN_SYNC, 698 lan966x, PTP_PIN_CFG(TOD_ACC_PIN)); 699 700 s = lan_rd(lan966x, PTP_TOD_SEC_MSB(TOD_ACC_PIN)); 701 s <<= 32; 702 s |= lan_rd(lan966x, PTP_TOD_SEC_LSB(TOD_ACC_PIN)); 703 ns = lan_rd(lan966x, PTP_TOD_NSEC(TOD_ACC_PIN)); 704 ns &= PTP_TOD_NSEC_TOD_NSEC; 705 706 spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags); 707 708 /* Deal with negative values */ 709 if ((ns & 0xFFFFFFF0) == 0x3FFFFFF0) { 710 s--; 711 ns &= 0xf; 712 ns += 999999984; 713 } 714 715 set_normalized_timespec64(ts, s, ns); 716 return 0; 717 } 718 719 static int lan966x_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta) 720 { 721 struct lan966x_phc *phc = container_of(ptp, struct lan966x_phc, info); 722 struct lan966x *lan966x = phc->lan966x; 723 724 if (delta > -(NSEC_PER_SEC / 2) && delta < (NSEC_PER_SEC / 2)) { 725 unsigned long flags; 726 727 spin_lock_irqsave(&lan966x->ptp_clock_lock, flags); 728 729 /* Must be in IDLE mode before the time can be loaded */ 730 lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_IDLE) | 731 PTP_PIN_CFG_PIN_DOM_SET(phc->index) | 732 PTP_PIN_CFG_PIN_SYNC_SET(0), 733 PTP_PIN_CFG_PIN_ACTION | 734 PTP_PIN_CFG_PIN_DOM | 735 PTP_PIN_CFG_PIN_SYNC, 736 lan966x, PTP_PIN_CFG(TOD_ACC_PIN)); 737 738 lan_wr(PTP_TOD_NSEC_TOD_NSEC_SET(delta), 739 lan966x, PTP_TOD_NSEC(TOD_ACC_PIN)); 740 741 /* Adjust time with the value of PTP_TOD_NSEC */ 742 lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_DELTA) | 743 PTP_PIN_CFG_PIN_DOM_SET(phc->index) | 744 PTP_PIN_CFG_PIN_SYNC_SET(0), 745 PTP_PIN_CFG_PIN_ACTION | 746 PTP_PIN_CFG_PIN_DOM | 747 PTP_PIN_CFG_PIN_SYNC, 748 lan966x, PTP_PIN_CFG(TOD_ACC_PIN)); 749 750 spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags); 751 } else { 752 /* Fall back using lan966x_ptp_settime64 which is not exact */ 753 struct timespec64 ts; 754 u64 now; 755 756 lan966x_ptp_gettime64(ptp, &ts); 757 758 now = ktime_to_ns(timespec64_to_ktime(ts)); 759 ts = ns_to_timespec64(now + delta); 760 761 lan966x_ptp_settime64(ptp, &ts); 762 } 763 764 return 0; 765 } 766 767 static int lan966x_ptp_verify(struct ptp_clock_info *ptp, unsigned int pin, 768 enum ptp_pin_function func, unsigned int chan) 769 { 770 struct lan966x_phc *phc = container_of(ptp, struct lan966x_phc, info); 771 struct lan966x *lan966x = phc->lan966x; 772 struct ptp_clock_info *info; 773 int i; 774 775 /* Currently support only 1 channel */ 776 if (chan != 0) 777 return -1; 778 779 switch (func) { 780 case PTP_PF_NONE: 781 case PTP_PF_PEROUT: 782 case PTP_PF_EXTTS: 783 break; 784 default: 785 return -1; 786 } 787 788 /* The PTP pins are shared by all the PHC. So it is required to see if 789 * the pin is connected to another PHC. The pin is connected to another 790 * PHC if that pin already has a function on that PHC. 791 */ 792 for (i = 0; i < LAN966X_PHC_COUNT; ++i) { 793 info = &lan966x->phc[i].info; 794 795 /* Ignore the check with ourself */ 796 if (ptp == info) 797 continue; 798 799 if (info->pin_config[pin].func == PTP_PF_PEROUT || 800 info->pin_config[pin].func == PTP_PF_EXTTS) 801 return -1; 802 } 803 804 return 0; 805 } 806 807 static int lan966x_ptp_perout(struct ptp_clock_info *ptp, 808 struct ptp_clock_request *rq, int on) 809 { 810 struct lan966x_phc *phc = container_of(ptp, struct lan966x_phc, info); 811 struct lan966x *lan966x = phc->lan966x; 812 struct timespec64 ts_phase, ts_period; 813 unsigned long flags; 814 s64 wf_high, wf_low; 815 bool pps = false; 816 int pin; 817 818 pin = ptp_find_pin(phc->clock, PTP_PF_PEROUT, rq->perout.index); 819 if (pin == -1 || pin >= LAN966X_PHC_PINS_NUM) 820 return -EINVAL; 821 822 if (!on) { 823 spin_lock_irqsave(&lan966x->ptp_clock_lock, flags); 824 lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_IDLE) | 825 PTP_PIN_CFG_PIN_DOM_SET(phc->index) | 826 PTP_PIN_CFG_PIN_SYNC_SET(0), 827 PTP_PIN_CFG_PIN_ACTION | 828 PTP_PIN_CFG_PIN_DOM | 829 PTP_PIN_CFG_PIN_SYNC, 830 lan966x, PTP_PIN_CFG(pin)); 831 spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags); 832 return 0; 833 } 834 835 if (rq->perout.period.sec == 1 && 836 rq->perout.period.nsec == 0) 837 pps = true; 838 839 if (rq->perout.flags & PTP_PEROUT_PHASE) { 840 ts_phase.tv_sec = rq->perout.phase.sec; 841 ts_phase.tv_nsec = rq->perout.phase.nsec; 842 } else { 843 ts_phase.tv_sec = rq->perout.start.sec; 844 ts_phase.tv_nsec = rq->perout.start.nsec; 845 } 846 847 if (ts_phase.tv_sec || (ts_phase.tv_nsec && !pps)) { 848 dev_warn(lan966x->dev, 849 "Absolute time not supported!\n"); 850 return -EINVAL; 851 } 852 853 if (rq->perout.flags & PTP_PEROUT_DUTY_CYCLE) { 854 struct timespec64 ts_on; 855 856 ts_on.tv_sec = rq->perout.on.sec; 857 ts_on.tv_nsec = rq->perout.on.nsec; 858 859 wf_high = timespec64_to_ns(&ts_on); 860 } else { 861 wf_high = 5000; 862 } 863 864 if (pps) { 865 spin_lock_irqsave(&lan966x->ptp_clock_lock, flags); 866 lan_wr(PTP_WF_LOW_PERIOD_PIN_WFL(ts_phase.tv_nsec), 867 lan966x, PTP_WF_LOW_PERIOD(pin)); 868 lan_wr(PTP_WF_HIGH_PERIOD_PIN_WFH(wf_high), 869 lan966x, PTP_WF_HIGH_PERIOD(pin)); 870 lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_CLOCK) | 871 PTP_PIN_CFG_PIN_DOM_SET(phc->index) | 872 PTP_PIN_CFG_PIN_SYNC_SET(3), 873 PTP_PIN_CFG_PIN_ACTION | 874 PTP_PIN_CFG_PIN_DOM | 875 PTP_PIN_CFG_PIN_SYNC, 876 lan966x, PTP_PIN_CFG(pin)); 877 spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags); 878 return 0; 879 } 880 881 ts_period.tv_sec = rq->perout.period.sec; 882 ts_period.tv_nsec = rq->perout.period.nsec; 883 884 wf_low = timespec64_to_ns(&ts_period); 885 wf_low -= wf_high; 886 887 spin_lock_irqsave(&lan966x->ptp_clock_lock, flags); 888 lan_wr(PTP_WF_LOW_PERIOD_PIN_WFL(wf_low), 889 lan966x, PTP_WF_LOW_PERIOD(pin)); 890 lan_wr(PTP_WF_HIGH_PERIOD_PIN_WFH(wf_high), 891 lan966x, PTP_WF_HIGH_PERIOD(pin)); 892 lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_CLOCK) | 893 PTP_PIN_CFG_PIN_DOM_SET(phc->index) | 894 PTP_PIN_CFG_PIN_SYNC_SET(0), 895 PTP_PIN_CFG_PIN_ACTION | 896 PTP_PIN_CFG_PIN_DOM | 897 PTP_PIN_CFG_PIN_SYNC, 898 lan966x, PTP_PIN_CFG(pin)); 899 spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags); 900 901 return 0; 902 } 903 904 static int lan966x_ptp_extts(struct ptp_clock_info *ptp, 905 struct ptp_clock_request *rq, int on) 906 { 907 struct lan966x_phc *phc = container_of(ptp, struct lan966x_phc, info); 908 struct lan966x *lan966x = phc->lan966x; 909 unsigned long flags; 910 int pin; 911 u32 val; 912 913 if (lan966x->ptp_ext_irq <= 0) 914 return -EOPNOTSUPP; 915 916 pin = ptp_find_pin(phc->clock, PTP_PF_EXTTS, rq->extts.index); 917 if (pin == -1 || pin >= LAN966X_PHC_PINS_NUM) 918 return -EINVAL; 919 920 spin_lock_irqsave(&lan966x->ptp_clock_lock, flags); 921 lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_SAVE) | 922 PTP_PIN_CFG_PIN_SYNC_SET(on ? 3 : 0) | 923 PTP_PIN_CFG_PIN_DOM_SET(phc->index) | 924 PTP_PIN_CFG_PIN_SELECT_SET(pin), 925 PTP_PIN_CFG_PIN_ACTION | 926 PTP_PIN_CFG_PIN_SYNC | 927 PTP_PIN_CFG_PIN_DOM | 928 PTP_PIN_CFG_PIN_SELECT, 929 lan966x, PTP_PIN_CFG(pin)); 930 931 val = lan_rd(lan966x, PTP_PIN_INTR_ENA); 932 if (on) 933 val |= BIT(pin); 934 else 935 val &= ~BIT(pin); 936 lan_wr(val, lan966x, PTP_PIN_INTR_ENA); 937 938 spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags); 939 940 return 0; 941 } 942 943 static int lan966x_ptp_enable(struct ptp_clock_info *ptp, 944 struct ptp_clock_request *rq, int on) 945 { 946 switch (rq->type) { 947 case PTP_CLK_REQ_PEROUT: 948 return lan966x_ptp_perout(ptp, rq, on); 949 case PTP_CLK_REQ_EXTTS: 950 return lan966x_ptp_extts(ptp, rq, on); 951 default: 952 return -EOPNOTSUPP; 953 } 954 955 return 0; 956 } 957 958 static struct ptp_clock_info lan966x_ptp_clock_info = { 959 .owner = THIS_MODULE, 960 .name = "lan966x ptp", 961 .max_adj = 200000, 962 .gettime64 = lan966x_ptp_gettime64, 963 .settime64 = lan966x_ptp_settime64, 964 .adjtime = lan966x_ptp_adjtime, 965 .adjfine = lan966x_ptp_adjfine, 966 .verify = lan966x_ptp_verify, 967 .enable = lan966x_ptp_enable, 968 .n_per_out = LAN966X_PHC_PINS_NUM, 969 .n_ext_ts = LAN966X_PHC_PINS_NUM, 970 .n_pins = LAN966X_PHC_PINS_NUM, 971 .supported_extts_flags = PTP_RISING_EDGE | 972 PTP_STRICT_FLAGS, 973 .supported_perout_flags = PTP_PEROUT_DUTY_CYCLE | 974 PTP_PEROUT_PHASE, 975 }; 976 977 static int lan966x_ptp_phc_init(struct lan966x *lan966x, 978 int index, 979 struct ptp_clock_info *clock_info) 980 { 981 struct lan966x_phc *phc = &lan966x->phc[index]; 982 struct ptp_pin_desc *p; 983 int i; 984 985 for (i = 0; i < LAN966X_PHC_PINS_NUM; i++) { 986 p = &phc->pins[i]; 987 988 snprintf(p->name, sizeof(p->name), "pin%d", i); 989 p->index = i; 990 p->func = PTP_PF_NONE; 991 } 992 993 phc->info = *clock_info; 994 phc->info.pin_config = &phc->pins[0]; 995 phc->clock = ptp_clock_register(&phc->info, lan966x->dev); 996 if (IS_ERR(phc->clock)) 997 return PTR_ERR(phc->clock); 998 999 phc->index = index; 1000 phc->lan966x = lan966x; 1001 1002 return 0; 1003 } 1004 1005 int lan966x_ptp_init(struct lan966x *lan966x) 1006 { 1007 u64 tod_adj = lan966x_ptp_get_nominal_value(); 1008 struct lan966x_port *port; 1009 int err, i; 1010 1011 if (!lan966x->ptp) 1012 return 0; 1013 1014 for (i = 0; i < LAN966X_PHC_COUNT; ++i) { 1015 err = lan966x_ptp_phc_init(lan966x, i, &lan966x_ptp_clock_info); 1016 if (err) 1017 return err; 1018 } 1019 1020 spin_lock_init(&lan966x->ptp_clock_lock); 1021 spin_lock_init(&lan966x->ptp_ts_id_lock); 1022 mutex_init(&lan966x->ptp_lock); 1023 1024 /* Disable master counters */ 1025 lan_wr(PTP_DOM_CFG_ENA_SET(0), lan966x, PTP_DOM_CFG); 1026 1027 /* Configure the nominal TOD increment per clock cycle */ 1028 lan_rmw(PTP_DOM_CFG_CLKCFG_DIS_SET(0x7), 1029 PTP_DOM_CFG_CLKCFG_DIS, 1030 lan966x, PTP_DOM_CFG); 1031 1032 for (i = 0; i < LAN966X_PHC_COUNT; ++i) { 1033 lan_wr((u32)tod_adj & 0xFFFFFFFF, lan966x, 1034 PTP_CLK_PER_CFG(i, 0)); 1035 lan_wr((u32)(tod_adj >> 32), lan966x, 1036 PTP_CLK_PER_CFG(i, 1)); 1037 } 1038 1039 lan_rmw(PTP_DOM_CFG_CLKCFG_DIS_SET(0), 1040 PTP_DOM_CFG_CLKCFG_DIS, 1041 lan966x, PTP_DOM_CFG); 1042 1043 /* Enable master counters */ 1044 lan_wr(PTP_DOM_CFG_ENA_SET(0x7), lan966x, PTP_DOM_CFG); 1045 1046 for (i = 0; i < lan966x->num_phys_ports; i++) { 1047 port = lan966x->ports[i]; 1048 if (!port) 1049 continue; 1050 1051 skb_queue_head_init(&port->tx_skbs); 1052 } 1053 1054 return 0; 1055 } 1056 1057 void lan966x_ptp_deinit(struct lan966x *lan966x) 1058 { 1059 struct lan966x_port *port; 1060 int i; 1061 1062 if (!lan966x->ptp) 1063 return; 1064 1065 for (i = 0; i < lan966x->num_phys_ports; i++) { 1066 port = lan966x->ports[i]; 1067 if (!port) 1068 continue; 1069 1070 skb_queue_purge(&port->tx_skbs); 1071 } 1072 1073 for (i = 0; i < LAN966X_PHC_COUNT; ++i) 1074 ptp_clock_unregister(lan966x->phc[i].clock); 1075 } 1076 1077 void lan966x_ptp_rxtstamp(struct lan966x *lan966x, struct sk_buff *skb, 1078 u64 src_port, u64 timestamp) 1079 { 1080 struct skb_shared_hwtstamps *shhwtstamps; 1081 struct lan966x_phc *phc; 1082 struct timespec64 ts; 1083 u64 full_ts_in_ns; 1084 1085 if (!lan966x->ptp || 1086 !lan966x->ports[src_port]->ptp_rx_cmd) 1087 return; 1088 1089 phc = &lan966x->phc[LAN966X_PHC_PORT]; 1090 lan966x_ptp_gettime64(&phc->info, &ts); 1091 1092 /* Drop the sub-ns precision */ 1093 timestamp = timestamp >> 2; 1094 if (ts.tv_nsec < timestamp) 1095 ts.tv_sec--; 1096 ts.tv_nsec = timestamp; 1097 full_ts_in_ns = ktime_set(ts.tv_sec, ts.tv_nsec); 1098 1099 shhwtstamps = skb_hwtstamps(skb); 1100 shhwtstamps->hwtstamp = full_ts_in_ns; 1101 } 1102 1103 u32 lan966x_ptp_get_period_ps(void) 1104 { 1105 /* This represents the system clock period in picoseconds */ 1106 return 15125; 1107 } 1108