1114 lines · c
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 51210 11/* Represents 1ppm adjustment in 2^59 format with 6.037735849ns as reference12 * The value is calculated as following: (1/1000000)/((2^-59)/6.037735849)13 */14#define LAN966X_1PPM_FORMAT 3480517749723LL15 16/* Represents 1ppb adjustment in 2^29 format with 6.037735849ns as reference17 * The value is calculated as following: (1/1000000000)/((2^59)/6.037735849)18 */19#define LAN966X_1PPB_FORMAT 3480517749LL20 21#define TOD_ACC_PIN 0x722 23/* This represents the base rule ID for the PTP rules that are added in the24 * VCAP to trap frames to CPU. This number needs to be bigger than the maximum25 * number of entries that can exist in the VCAP.26 */27#define LAN966X_VCAP_PTP_RULE_ID 100000028#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_TOD41};42 43static u64 lan966x_ptp_get_nominal_value(void)44{45 /* This is the default value that for each system clock, the time of day46 * 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 else257 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 step347 */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 the488 * rx timestamp which represents the id of the frame489 */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 bit568 */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 of616 * overflow issues. If scaled_ppm with 16bit fractional part was bigger617 * 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 if789 * the pin is connected to another PHC. The pin is connected to another790 * 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 else945 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