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