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