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