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
   7 #define LAN966X_MAX_PTP_ID      512
   8
   9 /* Represents 1ppm adjustment in 2^59 format with 6.037735849ns as reference
  10  * The value is calculated as following: (1/1000000)/((2^-59)/6.037735849)
  11  */
  12 #define LAN966X_1PPM_FORMAT             3480517749723LL
  13
  14 /* Represents 1ppb adjustment in 2^29 format with 6.037735849ns as reference
  15  * The value is calculated as following: (1/1000000000)/((2^59)/6.037735849)
  16  */
  17 #define LAN966X_1PPB_FORMAT             3480517749LL
  18
  19 #define TOD_ACC_PIN             0x5
  20
  21 enum {
  22         PTP_PIN_ACTION_IDLE = 0,
  23         PTP_PIN_ACTION_LOAD,
  24         PTP_PIN_ACTION_SAVE,
  25         PTP_PIN_ACTION_CLOCK,
  26         PTP_PIN_ACTION_DELTA,
  27         PTP_PIN_ACTION_TOD
  28 };
  29
  30 static u64 lan966x_ptp_get_nominal_value(void)
  31 {
  32         u64 res = 0x304d2df1;
  33
  34         res <<= 32;
  35         return res;
  36 }
  37
  38 int lan966x_ptp_hwtstamp_set(struct lan966x_port *port, struct ifreq *ifr)
  39 {
  40         struct lan966x *lan966x = port->lan966x;
  41         struct hwtstamp_config cfg;
  42         struct lan966x_phc *phc;
  43
  44         /* For now don't allow to run ptp on ports that are part of a bridge,
  45          * because in case of transparent clock the HW will still forward the
  46          * frames, so there would be duplicate frames
  47          */
  48         if (lan966x->bridge_mask & BIT(port->chip_port))
  49                 return -EINVAL;
  50
  51         if (copy_from_user(&cfg, ifr->ifr_data, sizeof(cfg)))
  52                 return -EFAULT;
  53
  54         switch (cfg.tx_type) {
  55         case HWTSTAMP_TX_ON:
  56                 port->ptp_cmd = IFH_REW_OP_TWO_STEP_PTP;
  57                 break;
  58         case HWTSTAMP_TX_ONESTEP_SYNC:
  59                 port->ptp_cmd = IFH_REW_OP_ONE_STEP_PTP;
  60                 break;
  61         case HWTSTAMP_TX_OFF:
  62                 port->ptp_cmd = IFH_REW_OP_NOOP;
  63                 break;
  64         default:
  65                 return -ERANGE;
  66         }
  67
  68         switch (cfg.rx_filter) {
  69         case HWTSTAMP_FILTER_NONE:
  70                 break;
  71         case HWTSTAMP_FILTER_ALL:
  72         case HWTSTAMP_FILTER_PTP_V1_L4_EVENT:
  73         case HWTSTAMP_FILTER_PTP_V1_L4_SYNC:
  74         case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ:
  75         case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
  76         case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
  77         case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
  78         case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
  79         case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
  80         case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
  81         case HWTSTAMP_FILTER_PTP_V2_EVENT:
  82         case HWTSTAMP_FILTER_PTP_V2_SYNC:
  83         case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
  84         case HWTSTAMP_FILTER_NTP_ALL:
  85                 cfg.rx_filter = HWTSTAMP_FILTER_ALL;
  86                 break;
  87         default:
  88                 return -ERANGE;
  89         }
  90
  91         /* Commit back the result & save it */
  92         mutex_lock(&lan966x->ptp_lock);
  93         phc = &lan966x->phc[LAN966X_PHC_PORT];
  94         memcpy(&phc->hwtstamp_config, &cfg, sizeof(cfg));
  95         mutex_unlock(&lan966x->ptp_lock);
  96
  97         return copy_to_user(ifr->ifr_data, &cfg, sizeof(cfg)) ? -EFAULT : 0;
  98 }
  99
 100 int lan966x_ptp_hwtstamp_get(struct lan966x_port *port, struct ifreq *ifr)
 101 {
 102         struct lan966x *lan966x = port->lan966x;
 103         struct lan966x_phc *phc;
 104
 105         phc = &lan966x->phc[LAN966X_PHC_PORT];
 106         return copy_to_user(ifr->ifr_data, &phc->hwtstamp_config,
 107                             sizeof(phc->hwtstamp_config)) ? -EFAULT : 0;
 108 }
 109
 110 static int lan966x_ptp_classify(struct lan966x_port *port, struct sk_buff *skb)
 111 {
 112         struct ptp_header *header;
 113         u8 msgtype;
 114         int type;
 115
 116         if (port->ptp_cmd == IFH_REW_OP_NOOP)
 117                 return IFH_REW_OP_NOOP;
 118
 119         type = ptp_classify_raw(skb);
 120         if (type == PTP_CLASS_NONE)
 121                 return IFH_REW_OP_NOOP;
 122
 123         header = ptp_parse_header(skb, type);
 124         if (!header)
 125                 return IFH_REW_OP_NOOP;
 126
 127         if (port->ptp_cmd == IFH_REW_OP_TWO_STEP_PTP)
 128                 return IFH_REW_OP_TWO_STEP_PTP;
 129
 130         /* If it is sync and run 1 step then set the correct operation,
 131          * otherwise run as 2 step
 132          */
 133         msgtype = ptp_get_msgtype(header, type);
 134         if ((msgtype & 0xf) == 0)
 135                 return IFH_REW_OP_ONE_STEP_PTP;
 136
 137         return IFH_REW_OP_TWO_STEP_PTP;
 138 }
 139
 140 static void lan966x_ptp_txtstamp_old_release(struct lan966x_port *port)
 141 {
 142         struct sk_buff *skb, *skb_tmp;
 143         unsigned long flags;
 144
 145         spin_lock_irqsave(&port->tx_skbs.lock, flags);
 146         skb_queue_walk_safe(&port->tx_skbs, skb, skb_tmp) {
 147                 if time_after(LAN966X_SKB_CB(skb)->jiffies + LAN966X_PTP_TIMEOUT,
 148                               jiffies)
 149                         break;
 150
 151                 __skb_unlink(skb, &port->tx_skbs);
 152                 dev_kfree_skb_any(skb);
 153         }
 154         spin_unlock_irqrestore(&port->tx_skbs.lock, flags);
 155 }
 156
 157 int lan966x_ptp_txtstamp_request(struct lan966x_port *port,
 158                                  struct sk_buff *skb)
 159 {
 160         struct lan966x *lan966x = port->lan966x;
 161         unsigned long flags;
 162         u8 rew_op;
 163
 164         rew_op = lan966x_ptp_classify(port, skb);
 165         LAN966X_SKB_CB(skb)->rew_op = rew_op;
 166
 167         if (rew_op != IFH_REW_OP_TWO_STEP_PTP)
 168                 return 0;
 169
 170         lan966x_ptp_txtstamp_old_release(port);
 171
 172         spin_lock_irqsave(&lan966x->ptp_ts_id_lock, flags);
 173         if (lan966x->ptp_skbs == LAN966X_MAX_PTP_ID) {
 174                 spin_unlock_irqrestore(&lan966x->ptp_ts_id_lock, flags);
 175                 return -EBUSY;
 176         }
 177
 178         skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
 179
 180         skb_queue_tail(&port->tx_skbs, skb);
 181         LAN966X_SKB_CB(skb)->ts_id = port->ts_id;
 182         LAN966X_SKB_CB(skb)->jiffies = jiffies;
 183
 184         lan966x->ptp_skbs++;
 185         port->ts_id++;
 186         if (port->ts_id == LAN966X_MAX_PTP_ID)
 187                 port->ts_id = 0;
 188
 189         spin_unlock_irqrestore(&lan966x->ptp_ts_id_lock, flags);
 190
 191         return 0;
 192 }
 193
 194 void lan966x_ptp_txtstamp_release(struct lan966x_port *port,
 195                                   struct sk_buff *skb)
 196 {
 197         struct lan966x *lan966x = port->lan966x;
 198         unsigned long flags;
 199
 200         spin_lock_irqsave(&lan966x->ptp_ts_id_lock, flags);
 201         port->ts_id--;
 202         lan966x->ptp_skbs--;
 203         skb_unlink(skb, &port->tx_skbs);
 204         spin_unlock_irqrestore(&lan966x->ptp_ts_id_lock, flags);
 205 }
 206
 207 static void lan966x_get_hwtimestamp(struct lan966x *lan966x,
 208                                     struct timespec64 *ts,
 209                                     u32 nsec)
 210 {
 211         /* Read current PTP time to get seconds */
 212         unsigned long flags;
 213         u32 curr_nsec;
 214
 215         spin_lock_irqsave(&lan966x->ptp_clock_lock, flags);
 216
 217         lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_SAVE) |
 218                 PTP_PIN_CFG_PIN_DOM_SET(LAN966X_PHC_PORT) |
 219                 PTP_PIN_CFG_PIN_SYNC_SET(0),
 220                 PTP_PIN_CFG_PIN_ACTION |
 221                 PTP_PIN_CFG_PIN_DOM |
 222                 PTP_PIN_CFG_PIN_SYNC,
 223                 lan966x, PTP_PIN_CFG(TOD_ACC_PIN));
 224
 225         ts->tv_sec = lan_rd(lan966x, PTP_TOD_SEC_LSB(TOD_ACC_PIN));
 226         curr_nsec = lan_rd(lan966x, PTP_TOD_NSEC(TOD_ACC_PIN));
 227
 228         ts->tv_nsec = nsec;
 229
 230         /* Sec has incremented since the ts was registered */
 231         if (curr_nsec < nsec)
 232                 ts->tv_sec--;
 233
 234         spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags);
 235 }
 236
 237 irqreturn_t lan966x_ptp_irq_handler(int irq, void *args)
 238 {
 239         int budget = LAN966X_MAX_PTP_ID;
 240         struct lan966x *lan966x = args;
 241
 242         while (budget--) {
 243                 struct sk_buff *skb, *skb_tmp, *skb_match = NULL;
 244                 struct skb_shared_hwtstamps shhwtstamps;
 245                 struct lan966x_port *port;
 246                 struct timespec64 ts;
 247                 unsigned long flags;
 248                 u32 val, id, txport;
 249                 u32 delay;
 250
 251                 val = lan_rd(lan966x, PTP_TWOSTEP_CTRL);
 252
 253                 /* Check if a timestamp can be retrieved */
 254                 if (!(val & PTP_TWOSTEP_CTRL_VLD))
 255                         break;
 256
 257                 WARN_ON(val & PTP_TWOSTEP_CTRL_OVFL);
 258
 259                 if (!(val & PTP_TWOSTEP_CTRL_STAMP_TX))
 260                         continue;
 261
 262                 /* Retrieve the ts Tx port */
 263                 txport = PTP_TWOSTEP_CTRL_STAMP_PORT_GET(val);
 264
 265                 /* Retrieve its associated skb */
 266                 port = lan966x->ports[txport];
 267
 268                 /* Retrieve the delay */
 269                 delay = lan_rd(lan966x, PTP_TWOSTEP_STAMP);
 270                 delay = PTP_TWOSTEP_STAMP_STAMP_NSEC_GET(delay);
 271
 272                 /* Get next timestamp from fifo, which needs to be the
 273                  * rx timestamp which represents the id of the frame
 274                  */
 275                 lan_rmw(PTP_TWOSTEP_CTRL_NXT_SET(1),
 276                         PTP_TWOSTEP_CTRL_NXT,
 277                         lan966x, PTP_TWOSTEP_CTRL);
 278
 279                 val = lan_rd(lan966x, PTP_TWOSTEP_CTRL);
 280
 281                 /* Check if a timestamp can be retried */
 282                 if (!(val & PTP_TWOSTEP_CTRL_VLD))
 283                         break;
 284
 285                 /* Read RX timestamping to get the ID */
 286                 id = lan_rd(lan966x, PTP_TWOSTEP_STAMP);
 287
 288                 spin_lock_irqsave(&port->tx_skbs.lock, flags);
 289                 skb_queue_walk_safe(&port->tx_skbs, skb, skb_tmp) {
 290                         if (LAN966X_SKB_CB(skb)->ts_id != id)
 291                                 continue;
 292
 293                         __skb_unlink(skb, &port->tx_skbs);
 294                         skb_match = skb;
 295                         break;
 296                 }
 297                 spin_unlock_irqrestore(&port->tx_skbs.lock, flags);
 298
 299                 /* Next ts */
 300                 lan_rmw(PTP_TWOSTEP_CTRL_NXT_SET(1),
 301                         PTP_TWOSTEP_CTRL_NXT,
 302                         lan966x, PTP_TWOSTEP_CTRL);
 303
 304                 if (WARN_ON(!skb_match))
 305                         continue;
 306
 307                 spin_lock(&lan966x->ptp_ts_id_lock);
 308                 lan966x->ptp_skbs--;
 309                 spin_unlock(&lan966x->ptp_ts_id_lock);
 310
 311                 /* Get the h/w timestamp */
 312                 lan966x_get_hwtimestamp(lan966x, &ts, delay);
 313
 314                 /* Set the timestamp into the skb */
 315                 shhwtstamps.hwtstamp = ktime_set(ts.tv_sec, ts.tv_nsec);
 316                 skb_tstamp_tx(skb_match, &shhwtstamps);
 317
 318                 dev_kfree_skb_any(skb_match);
 319         }
 320
 321         return IRQ_HANDLED;
 322 }
 323
 324 static int lan966x_ptp_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)
 325 {
 326         struct lan966x_phc *phc = container_of(ptp, struct lan966x_phc, info);
 327         struct lan966x *lan966x = phc->lan966x;
 328         unsigned long flags;
 329         bool neg_adj = 0;
 330         u64 tod_inc;
 331         u64 ref;
 332
 333         if (!scaled_ppm)
 334                 return 0;
 335
 336         if (scaled_ppm < 0) {
 337                 neg_adj = 1;
 338                 scaled_ppm = -scaled_ppm;
 339         }
 340
 341         tod_inc = lan966x_ptp_get_nominal_value();
 342
 343         /* The multiplication is split in 2 separate additions because of
 344          * overflow issues. If scaled_ppm with 16bit fractional part was bigger
 345          * than 20ppm then we got overflow.
 346          */
 347         ref = LAN966X_1PPM_FORMAT * (scaled_ppm >> 16);
 348         ref += (LAN966X_1PPM_FORMAT * (0xffff & scaled_ppm)) >> 16;
 349         tod_inc = neg_adj ? tod_inc - ref : tod_inc + ref;
 350
 351         spin_lock_irqsave(&lan966x->ptp_clock_lock, flags);
 352
 353         lan_rmw(PTP_DOM_CFG_CLKCFG_DIS_SET(1 << BIT(phc->index)),
 354                 PTP_DOM_CFG_CLKCFG_DIS,
 355                 lan966x, PTP_DOM_CFG);
 356
 357         lan_wr((u32)tod_inc & 0xFFFFFFFF, lan966x,
 358                PTP_CLK_PER_CFG(phc->index, 0));
 359         lan_wr((u32)(tod_inc >> 32), lan966x,
 360                PTP_CLK_PER_CFG(phc->index, 1));
 361
 362         lan_rmw(PTP_DOM_CFG_CLKCFG_DIS_SET(0),
 363                 PTP_DOM_CFG_CLKCFG_DIS,
 364                 lan966x, PTP_DOM_CFG);
 365
 366         spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags);
 367
 368         return 0;
 369 }
 370
 371 static int lan966x_ptp_settime64(struct ptp_clock_info *ptp,
 372                                  const struct timespec64 *ts)
 373 {
 374         struct lan966x_phc *phc = container_of(ptp, struct lan966x_phc, info);
 375         struct lan966x *lan966x = phc->lan966x;
 376         unsigned long flags;
 377
 378         spin_lock_irqsave(&lan966x->ptp_clock_lock, flags);
 379
 380         /* Must be in IDLE mode before the time can be loaded */
 381         lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_IDLE) |
 382                 PTP_PIN_CFG_PIN_DOM_SET(phc->index) |
 383                 PTP_PIN_CFG_PIN_SYNC_SET(0),
 384                 PTP_PIN_CFG_PIN_ACTION |
 385                 PTP_PIN_CFG_PIN_DOM |
 386                 PTP_PIN_CFG_PIN_SYNC,
 387                 lan966x, PTP_PIN_CFG(TOD_ACC_PIN));
 388
 389         /* Set new value */
 390         lan_wr(PTP_TOD_SEC_MSB_TOD_SEC_MSB_SET(upper_32_bits(ts->tv_sec)),
 391                lan966x, PTP_TOD_SEC_MSB(TOD_ACC_PIN));
 392         lan_wr(lower_32_bits(ts->tv_sec),
 393                lan966x, PTP_TOD_SEC_LSB(TOD_ACC_PIN));
 394         lan_wr(ts->tv_nsec, lan966x, PTP_TOD_NSEC(TOD_ACC_PIN));
 395
 396         /* Apply new values */
 397         lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_LOAD) |
 398                 PTP_PIN_CFG_PIN_DOM_SET(phc->index) |
 399                 PTP_PIN_CFG_PIN_SYNC_SET(0),
 400                 PTP_PIN_CFG_PIN_ACTION |
 401                 PTP_PIN_CFG_PIN_DOM |
 402                 PTP_PIN_CFG_PIN_SYNC,
 403                 lan966x, PTP_PIN_CFG(TOD_ACC_PIN));
 404
 405         spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags);
 406
 407         return 0;
 408 }
 409
 410 static int lan966x_ptp_gettime64(struct ptp_clock_info *ptp,
 411                                  struct timespec64 *ts)
 412 {
 413         struct lan966x_phc *phc = container_of(ptp, struct lan966x_phc, info);
 414         struct lan966x *lan966x = phc->lan966x;
 415         unsigned long flags;
 416         time64_t s;
 417         s64 ns;
 418
 419         spin_lock_irqsave(&lan966x->ptp_clock_lock, flags);
 420
 421         lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_SAVE) |
 422                 PTP_PIN_CFG_PIN_DOM_SET(phc->index) |
 423                 PTP_PIN_CFG_PIN_SYNC_SET(0),
 424                 PTP_PIN_CFG_PIN_ACTION |
 425                 PTP_PIN_CFG_PIN_DOM |
 426                 PTP_PIN_CFG_PIN_SYNC,
 427                 lan966x, PTP_PIN_CFG(TOD_ACC_PIN));
 428
 429         s = lan_rd(lan966x, PTP_TOD_SEC_MSB(TOD_ACC_PIN));
 430         s <<= 32;
 431         s |= lan_rd(lan966x, PTP_TOD_SEC_LSB(TOD_ACC_PIN));
 432         ns = lan_rd(lan966x, PTP_TOD_NSEC(TOD_ACC_PIN));
 433         ns &= PTP_TOD_NSEC_TOD_NSEC;
 434
 435         spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags);
 436
 437         /* Deal with negative values */
 438         if ((ns & 0xFFFFFFF0) == 0x3FFFFFF0) {
 439                 s--;
 440                 ns &= 0xf;
 441                 ns += 999999984;
 442         }
 443
 444         set_normalized_timespec64(ts, s, ns);
 445         return 0;
 446 }
 447
 448 static int lan966x_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
 449 {
 450         struct lan966x_phc *phc = container_of(ptp, struct lan966x_phc, info);
 451         struct lan966x *lan966x = phc->lan966x;
 452
 453         if (delta > -(NSEC_PER_SEC / 2) && delta < (NSEC_PER_SEC / 2)) {
 454                 unsigned long flags;
 455
 456                 spin_lock_irqsave(&lan966x->ptp_clock_lock, flags);
 457
 458                 /* Must be in IDLE mode before the time can be loaded */
 459                 lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_IDLE) |
 460                         PTP_PIN_CFG_PIN_DOM_SET(phc->index) |
 461                         PTP_PIN_CFG_PIN_SYNC_SET(0),
 462                         PTP_PIN_CFG_PIN_ACTION |
 463                         PTP_PIN_CFG_PIN_DOM |
 464                         PTP_PIN_CFG_PIN_SYNC,
 465                         lan966x, PTP_PIN_CFG(TOD_ACC_PIN));
 466
 467                 lan_wr(PTP_TOD_NSEC_TOD_NSEC_SET(delta),
 468                        lan966x, PTP_TOD_NSEC(TOD_ACC_PIN));
 469
 470                 /* Adjust time with the value of PTP_TOD_NSEC */
 471                 lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_DELTA) |
 472                         PTP_PIN_CFG_PIN_DOM_SET(phc->index) |
 473                         PTP_PIN_CFG_PIN_SYNC_SET(0),
 474                         PTP_PIN_CFG_PIN_ACTION |
 475                         PTP_PIN_CFG_PIN_DOM |
 476                         PTP_PIN_CFG_PIN_SYNC,
 477                         lan966x, PTP_PIN_CFG(TOD_ACC_PIN));
 478
 479                 spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags);
 480         } else {
 481                 /* Fall back using lan966x_ptp_settime64 which is not exact */
 482                 struct timespec64 ts;
 483                 u64 now;
 484
 485                 lan966x_ptp_gettime64(ptp, &ts);
 486
 487                 now = ktime_to_ns(timespec64_to_ktime(ts));
 488                 ts = ns_to_timespec64(now + delta);
 489
 490                 lan966x_ptp_settime64(ptp, &ts);
 491         }
 492
 493         return 0;
 494 }
 495
 496 static struct ptp_clock_info lan966x_ptp_clock_info = {
 497         .owner          = THIS_MODULE,
 498         .name           = "lan966x ptp",
 499         .max_adj        = 200000,
 500         .gettime64      = lan966x_ptp_gettime64,
 501         .settime64      = lan966x_ptp_settime64,
 502         .adjtime        = lan966x_ptp_adjtime,
 503         .adjfine        = lan966x_ptp_adjfine,
 504 };
 505
 506 static int lan966x_ptp_phc_init(struct lan966x *lan966x,
 507                                 int index,
 508                                 struct ptp_clock_info *clock_info)
 509 {
 510         struct lan966x_phc *phc = &lan966x->phc[index];
 511
 512         phc->info = *clock_info;
 513         phc->clock = ptp_clock_register(&phc->info, lan966x->dev);
 514         if (IS_ERR(phc->clock))
 515                 return PTR_ERR(phc->clock);
 516
 517         phc->index = index;
 518         phc->lan966x = lan966x;
 519
 520         /* PTP Rx stamping is always enabled.  */
 521         phc->hwtstamp_config.rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
 522
 523         return 0;
 524 }
 525
 526 int lan966x_ptp_init(struct lan966x *lan966x)
 527 {
 528         u64 tod_adj = lan966x_ptp_get_nominal_value();
 529         struct lan966x_port *port;
 530         int err, i;
 531
 532         if (!lan966x->ptp)
 533                 return 0;
 534
 535         for (i = 0; i < LAN966X_PHC_COUNT; ++i) {
 536                 err = lan966x_ptp_phc_init(lan966x, i, &lan966x_ptp_clock_info);
 537                 if (err)
 538                         return err;
 539         }
 540
 541         spin_lock_init(&lan966x->ptp_clock_lock);
 542         spin_lock_init(&lan966x->ptp_ts_id_lock);
 543         mutex_init(&lan966x->ptp_lock);
 544
 545         /* Disable master counters */
 546         lan_wr(PTP_DOM_CFG_ENA_SET(0), lan966x, PTP_DOM_CFG);
 547
 548         /* Configure the nominal TOD increment per clock cycle */
 549         lan_rmw(PTP_DOM_CFG_CLKCFG_DIS_SET(0x7),
 550                 PTP_DOM_CFG_CLKCFG_DIS,
 551                 lan966x, PTP_DOM_CFG);
 552
 553         for (i = 0; i < LAN966X_PHC_COUNT; ++i) {
 554                 lan_wr((u32)tod_adj & 0xFFFFFFFF, lan966x,
 555                        PTP_CLK_PER_CFG(i, 0));
 556                 lan_wr((u32)(tod_adj >> 32), lan966x,
 557                        PTP_CLK_PER_CFG(i, 1));
 558         }
 559
 560         lan_rmw(PTP_DOM_CFG_CLKCFG_DIS_SET(0),
 561                 PTP_DOM_CFG_CLKCFG_DIS,
 562                 lan966x, PTP_DOM_CFG);
 563
 564         /* Enable master counters */
 565         lan_wr(PTP_DOM_CFG_ENA_SET(0x7), lan966x, PTP_DOM_CFG);
 566
 567         for (i = 0; i < lan966x->num_phys_ports; i++) {
 568                 port = lan966x->ports[i];
 569                 if (!port)
 570                         continue;
 571
 572                 skb_queue_head_init(&port->tx_skbs);
 573         }
 574
 575         return 0;
 576 }
 577
 578 void lan966x_ptp_deinit(struct lan966x *lan966x)
 579 {
 580         struct lan966x_port *port;
 581         int i;
 582
 583         for (i = 0; i < lan966x->num_phys_ports; i++) {
 584                 port = lan966x->ports[i];
 585                 if (!port)
 586                         continue;
 587
 588                 skb_queue_purge(&port->tx_skbs);
 589         }
 590
 591         for (i = 0; i < LAN966X_PHC_COUNT; ++i)
 592                 ptp_clock_unregister(lan966x->phc[i].clock);
 593 }
 594
 595 void lan966x_ptp_rxtstamp(struct lan966x *lan966x, struct sk_buff *skb,
 596                           u64 timestamp)
 597 {
 598         struct skb_shared_hwtstamps *shhwtstamps;
 599         struct lan966x_phc *phc;
 600         struct timespec64 ts;
 601         u64 full_ts_in_ns;
 602
 603         if (!lan966x->ptp)
 604                 return;
 605
 606         phc = &lan966x->phc[LAN966X_PHC_PORT];
 607         lan966x_ptp_gettime64(&phc->info, &ts);
 608
 609         /* Drop the sub-ns precision */
 610         timestamp = timestamp >> 2;
 611         if (ts.tv_nsec < timestamp)
 612                 ts.tv_sec--;
 613         ts.tv_nsec = timestamp;
 614         full_ts_in_ns = ktime_set(ts.tv_sec, ts.tv_nsec);
 615
 616         shhwtstamps = skb_hwtstamps(skb);
 617         shhwtstamps->hwtstamp = full_ts_in_ns;
 618 }