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 (!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
  94 free_rule:
  95         /* Free the local copy of the rule */
  96         vcap_free_rule(vrule);
  97         return err;
  98 }
  99
 100 static int lan966x_ptp_del_trap(struct lan966x_port *port,
 101                                 u32 rule_id)
 102 {
 103         struct lan966x *lan966x = port->lan966x;
 104         struct vcap_rule *vrule;
 105         u32 value, mask;
 106         int err;
 107
 108         vrule = vcap_get_rule(lan966x->vcap_ctrl, rule_id);
 109         if (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
 124 free_rule:
 125         vcap_free_rule(vrule);
 126         return err;
 127 }
 128
 129 static int lan966x_ptp_add_l2_key(struct vcap_rule *vrule,
 130                                   struct lan966x_port *port)
 131 {
 132         return vcap_rule_add_key_u32(vrule, VCAP_KF_ETYPE, ETH_P_1588, ~0);
 133 }
 134
 135 static int lan966x_ptp_add_ip_event_key(struct vcap_rule *vrule,
 136                                         struct lan966x_port *port)
 137 {
 138         return vcap_rule_add_key_u32(vrule, VCAP_KF_L4_DPORT, PTP_EV_PORT, ~0) ||
 139                vcap_rule_add_key_bit(vrule, VCAP_KF_TCP_IS, VCAP_BIT_0);
 140 }
 141
 142 static int lan966x_ptp_add_ip_general_key(struct vcap_rule *vrule,
 143                                           struct lan966x_port *port)
 144 {
 145         return vcap_rule_add_key_u32(vrule, VCAP_KF_L4_DPORT, PTP_GEN_PORT, ~0) ||
 146                vcap_rule_add_key_bit(vrule, VCAP_KF_TCP_IS, VCAP_BIT_0);
 147 }
 148
 149 static int lan966x_ptp_add_l2_rule(struct lan966x_port *port)
 150 {
 151         return lan966x_ptp_add_trap(port, lan966x_ptp_add_l2_key,
 152                                     LAN966X_VCAP_L2_PTP_TRAP, ETH_P_ALL);
 153 }
 154
 155 static int lan966x_ptp_add_ipv4_rules(struct lan966x_port *port)
 156 {
 157         int err;
 158
 159         err = lan966x_ptp_add_trap(port, lan966x_ptp_add_ip_event_key,
 160                                    LAN966X_VCAP_IPV4_EV_PTP_TRAP, ETH_P_IP);
 161         if (err)
 162                 return err;
 163
 164         err = lan966x_ptp_add_trap(port, lan966x_ptp_add_ip_general_key,
 165                                    LAN966X_VCAP_IPV4_GEN_PTP_TRAP, ETH_P_IP);
 166         if (err)
 167                 lan966x_ptp_del_trap(port, LAN966X_VCAP_IPV4_EV_PTP_TRAP);
 168
 169         return err;
 170 }
 171
 172 static int lan966x_ptp_add_ipv6_rules(struct lan966x_port *port)
 173 {
 174         int err;
 175
 176         err = lan966x_ptp_add_trap(port, lan966x_ptp_add_ip_event_key,
 177                                    LAN966X_VCAP_IPV6_EV_PTP_TRAP, ETH_P_IPV6);
 178         if (err)
 179                 return err;
 180
 181         err = lan966x_ptp_add_trap(port, lan966x_ptp_add_ip_general_key,
 182                                    LAN966X_VCAP_IPV6_GEN_PTP_TRAP, ETH_P_IPV6);
 183         if (err)
 184                 lan966x_ptp_del_trap(port, LAN966X_VCAP_IPV6_EV_PTP_TRAP);
 185
 186         return err;
 187 }
 188
 189 static int lan966x_ptp_del_l2_rule(struct lan966x_port *port)
 190 {
 191         return lan966x_ptp_del_trap(port, LAN966X_VCAP_L2_PTP_TRAP);
 192 }
 193
 194 static int lan966x_ptp_del_ipv4_rules(struct lan966x_port *port)
 195 {
 196         int err;
 197
 198         err = lan966x_ptp_del_trap(port, LAN966X_VCAP_IPV4_EV_PTP_TRAP);
 199         err |= lan966x_ptp_del_trap(port, LAN966X_VCAP_IPV4_GEN_PTP_TRAP);
 200
 201         return err;
 202 }
 203
 204 static int lan966x_ptp_del_ipv6_rules(struct lan966x_port *port)
 205 {
 206         int err;
 207
 208         err = lan966x_ptp_del_trap(port, LAN966X_VCAP_IPV6_EV_PTP_TRAP);
 209         err |= lan966x_ptp_del_trap(port, LAN966X_VCAP_IPV6_GEN_PTP_TRAP);
 210
 211         return err;
 212 }
 213
 214 static int lan966x_ptp_add_traps(struct lan966x_port *port)
 215 {
 216         int err;
 217
 218         err = lan966x_ptp_add_l2_rule(port);
 219         if (err)
 220                 goto err_l2;
 221
 222         err = lan966x_ptp_add_ipv4_rules(port);
 223         if (err)
 224                 goto err_ipv4;
 225
 226         err = lan966x_ptp_add_ipv6_rules(port);
 227         if (err)
 228                 goto err_ipv6;
 229
 230         return err;
 231
 232 err_ipv6:
 233         lan966x_ptp_del_ipv4_rules(port);
 234 err_ipv4:
 235         lan966x_ptp_del_l2_rule(port);
 236 err_l2:
 237         return err;
 238 }
 239
 240 int lan966x_ptp_del_traps(struct lan966x_port *port)
 241 {
 242         int err;
 243
 244         err = lan966x_ptp_del_l2_rule(port);
 245         err |= lan966x_ptp_del_ipv4_rules(port);
 246         err |= lan966x_ptp_del_ipv6_rules(port);
 247
 248         return err;
 249 }
 250
 251 int lan966x_ptp_setup_traps(struct lan966x_port *port,
 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
 260 int 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
 315 void 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
 325 static 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
 355 static 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
 372 int 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
 409 void 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
 422 static 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
 452 irqreturn_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
 539 irqreturn_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
 596 static 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
 643 static 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
 682 int 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
 719 static 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
 767 static 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
 807 static 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
 908 static 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
 953 static 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
 968 static 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
 983 static 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
1011 int 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
1063 void 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
1083 void 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
1109 u32 lan966x_ptp_get_period_ps(void)
1110 {
1111         /* This represents the system clock period in picoseconds */
1112         return 15125;
1113 }