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