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