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