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             0x7
  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         /* This is the default value that for each system clock, the time of day
  33          * is increased. It has the format 5.59 nanosecond.
  34          */
  35         return 0x304d4873ecade305;
  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 irqreturn_t lan966x_ptp_ext_irq_handler(int irq, void *args)
 325 {
 326         struct lan966x *lan966x = args;
 327         struct lan966x_phc *phc;
 328         unsigned long flags;
 329         u64 time = 0;
 330         time64_t s;
 331         int pin, i;
 332         s64 ns;
 333
 334         if (!(lan_rd(lan966x, PTP_PIN_INTR)))
 335                 return IRQ_NONE;
 336
 337         /* Go through all domains and see which pin generated the interrupt */
 338         for (i = 0; i < LAN966X_PHC_COUNT; ++i) {
 339                 struct ptp_clock_event ptp_event = {0};
 340
 341                 phc = &lan966x->phc[i];
 342                 pin = ptp_find_pin_unlocked(phc->clock, PTP_PF_EXTTS, 0);
 343                 if (pin == -1)
 344                         continue;
 345
 346                 if (!(lan_rd(lan966x, PTP_PIN_INTR) & BIT(pin)))
 347                         continue;
 348
 349                 spin_lock_irqsave(&lan966x->ptp_clock_lock, flags);
 350
 351                 /* Enable to get the new interrupt.
 352                  * By writing 1 it clears the bit
 353                  */
 354                 lan_wr(BIT(pin), lan966x, PTP_PIN_INTR);
 355
 356                 /* Get current time */
 357                 s = lan_rd(lan966x, PTP_TOD_SEC_MSB(pin));
 358                 s <<= 32;
 359                 s |= lan_rd(lan966x, PTP_TOD_SEC_LSB(pin));
 360                 ns = lan_rd(lan966x, PTP_TOD_NSEC(pin));
 361                 ns &= PTP_TOD_NSEC_TOD_NSEC;
 362
 363                 spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags);
 364
 365                 if ((ns & 0xFFFFFFF0) == 0x3FFFFFF0) {
 366                         s--;
 367                         ns &= 0xf;
 368                         ns += 999999984;
 369                 }
 370                 time = ktime_set(s, ns);
 371
 372                 ptp_event.index = pin;
 373                 ptp_event.timestamp = time;
 374                 ptp_event.type = PTP_CLOCK_EXTTS;
 375                 ptp_clock_event(phc->clock, &ptp_event);
 376         }
 377
 378         return IRQ_HANDLED;
 379 }
 380
 381 static int lan966x_ptp_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)
 382 {
 383         struct lan966x_phc *phc = container_of(ptp, struct lan966x_phc, info);
 384         struct lan966x *lan966x = phc->lan966x;
 385         unsigned long flags;
 386         bool neg_adj = 0;
 387         u64 tod_inc;
 388         u64 ref;
 389
 390         if (!scaled_ppm)
 391                 return 0;
 392
 393         if (scaled_ppm < 0) {
 394                 neg_adj = 1;
 395                 scaled_ppm = -scaled_ppm;
 396         }
 397
 398         tod_inc = lan966x_ptp_get_nominal_value();
 399
 400         /* The multiplication is split in 2 separate additions because of
 401          * overflow issues. If scaled_ppm with 16bit fractional part was bigger
 402          * than 20ppm then we got overflow.
 403          */
 404         ref = LAN966X_1PPM_FORMAT * (scaled_ppm >> 16);
 405         ref += (LAN966X_1PPM_FORMAT * (0xffff & scaled_ppm)) >> 16;
 406         tod_inc = neg_adj ? tod_inc - ref : tod_inc + ref;
 407
 408         spin_lock_irqsave(&lan966x->ptp_clock_lock, flags);
 409
 410         lan_rmw(PTP_DOM_CFG_CLKCFG_DIS_SET(1 << BIT(phc->index)),
 411                 PTP_DOM_CFG_CLKCFG_DIS,
 412                 lan966x, PTP_DOM_CFG);
 413
 414         lan_wr((u32)tod_inc & 0xFFFFFFFF, lan966x,
 415                PTP_CLK_PER_CFG(phc->index, 0));
 416         lan_wr((u32)(tod_inc >> 32), lan966x,
 417                PTP_CLK_PER_CFG(phc->index, 1));
 418
 419         lan_rmw(PTP_DOM_CFG_CLKCFG_DIS_SET(0),
 420                 PTP_DOM_CFG_CLKCFG_DIS,
 421                 lan966x, PTP_DOM_CFG);
 422
 423         spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags);
 424
 425         return 0;
 426 }
 427
 428 static int lan966x_ptp_settime64(struct ptp_clock_info *ptp,
 429                                  const struct timespec64 *ts)
 430 {
 431         struct lan966x_phc *phc = container_of(ptp, struct lan966x_phc, info);
 432         struct lan966x *lan966x = phc->lan966x;
 433         unsigned long flags;
 434
 435         spin_lock_irqsave(&lan966x->ptp_clock_lock, flags);
 436
 437         /* Must be in IDLE mode before the time can be loaded */
 438         lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_IDLE) |
 439                 PTP_PIN_CFG_PIN_DOM_SET(phc->index) |
 440                 PTP_PIN_CFG_PIN_SYNC_SET(0),
 441                 PTP_PIN_CFG_PIN_ACTION |
 442                 PTP_PIN_CFG_PIN_DOM |
 443                 PTP_PIN_CFG_PIN_SYNC,
 444                 lan966x, PTP_PIN_CFG(TOD_ACC_PIN));
 445
 446         /* Set new value */
 447         lan_wr(PTP_TOD_SEC_MSB_TOD_SEC_MSB_SET(upper_32_bits(ts->tv_sec)),
 448                lan966x, PTP_TOD_SEC_MSB(TOD_ACC_PIN));
 449         lan_wr(lower_32_bits(ts->tv_sec),
 450                lan966x, PTP_TOD_SEC_LSB(TOD_ACC_PIN));
 451         lan_wr(ts->tv_nsec, lan966x, PTP_TOD_NSEC(TOD_ACC_PIN));
 452
 453         /* Apply new values */
 454         lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_LOAD) |
 455                 PTP_PIN_CFG_PIN_DOM_SET(phc->index) |
 456                 PTP_PIN_CFG_PIN_SYNC_SET(0),
 457                 PTP_PIN_CFG_PIN_ACTION |
 458                 PTP_PIN_CFG_PIN_DOM |
 459                 PTP_PIN_CFG_PIN_SYNC,
 460                 lan966x, PTP_PIN_CFG(TOD_ACC_PIN));
 461
 462         spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags);
 463
 464         return 0;
 465 }
 466
 467 int lan966x_ptp_gettime64(struct ptp_clock_info *ptp, struct timespec64 *ts)
 468 {
 469         struct lan966x_phc *phc = container_of(ptp, struct lan966x_phc, info);
 470         struct lan966x *lan966x = phc->lan966x;
 471         unsigned long flags;
 472         time64_t s;
 473         s64 ns;
 474
 475         spin_lock_irqsave(&lan966x->ptp_clock_lock, flags);
 476
 477         lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_SAVE) |
 478                 PTP_PIN_CFG_PIN_DOM_SET(phc->index) |
 479                 PTP_PIN_CFG_PIN_SYNC_SET(0),
 480                 PTP_PIN_CFG_PIN_ACTION |
 481                 PTP_PIN_CFG_PIN_DOM |
 482                 PTP_PIN_CFG_PIN_SYNC,
 483                 lan966x, PTP_PIN_CFG(TOD_ACC_PIN));
 484
 485         s = lan_rd(lan966x, PTP_TOD_SEC_MSB(TOD_ACC_PIN));
 486         s <<= 32;
 487         s |= lan_rd(lan966x, PTP_TOD_SEC_LSB(TOD_ACC_PIN));
 488         ns = lan_rd(lan966x, PTP_TOD_NSEC(TOD_ACC_PIN));
 489         ns &= PTP_TOD_NSEC_TOD_NSEC;
 490
 491         spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags);
 492
 493         /* Deal with negative values */
 494         if ((ns & 0xFFFFFFF0) == 0x3FFFFFF0) {
 495                 s--;
 496                 ns &= 0xf;
 497                 ns += 999999984;
 498         }
 499
 500         set_normalized_timespec64(ts, s, ns);
 501         return 0;
 502 }
 503
 504 static int lan966x_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
 505 {
 506         struct lan966x_phc *phc = container_of(ptp, struct lan966x_phc, info);
 507         struct lan966x *lan966x = phc->lan966x;
 508
 509         if (delta > -(NSEC_PER_SEC / 2) && delta < (NSEC_PER_SEC / 2)) {
 510                 unsigned long flags;
 511
 512                 spin_lock_irqsave(&lan966x->ptp_clock_lock, flags);
 513
 514                 /* Must be in IDLE mode before the time can be loaded */
 515                 lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_IDLE) |
 516                         PTP_PIN_CFG_PIN_DOM_SET(phc->index) |
 517                         PTP_PIN_CFG_PIN_SYNC_SET(0),
 518                         PTP_PIN_CFG_PIN_ACTION |
 519                         PTP_PIN_CFG_PIN_DOM |
 520                         PTP_PIN_CFG_PIN_SYNC,
 521                         lan966x, PTP_PIN_CFG(TOD_ACC_PIN));
 522
 523                 lan_wr(PTP_TOD_NSEC_TOD_NSEC_SET(delta),
 524                        lan966x, PTP_TOD_NSEC(TOD_ACC_PIN));
 525
 526                 /* Adjust time with the value of PTP_TOD_NSEC */
 527                 lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_DELTA) |
 528                         PTP_PIN_CFG_PIN_DOM_SET(phc->index) |
 529                         PTP_PIN_CFG_PIN_SYNC_SET(0),
 530                         PTP_PIN_CFG_PIN_ACTION |
 531                         PTP_PIN_CFG_PIN_DOM |
 532                         PTP_PIN_CFG_PIN_SYNC,
 533                         lan966x, PTP_PIN_CFG(TOD_ACC_PIN));
 534
 535                 spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags);
 536         } else {
 537                 /* Fall back using lan966x_ptp_settime64 which is not exact */
 538                 struct timespec64 ts;
 539                 u64 now;
 540
 541                 lan966x_ptp_gettime64(ptp, &ts);
 542
 543                 now = ktime_to_ns(timespec64_to_ktime(ts));
 544                 ts = ns_to_timespec64(now + delta);
 545
 546                 lan966x_ptp_settime64(ptp, &ts);
 547         }
 548
 549         return 0;
 550 }
 551
 552 static int lan966x_ptp_verify(struct ptp_clock_info *ptp, unsigned int pin,
 553                               enum ptp_pin_function func, unsigned int chan)
 554 {
 555         struct lan966x_phc *phc = container_of(ptp, struct lan966x_phc, info);
 556         struct lan966x *lan966x = phc->lan966x;
 557         struct ptp_clock_info *info;
 558         int i;
 559
 560         /* Currently support only 1 channel */
 561         if (chan != 0)
 562                 return -1;
 563
 564         switch (func) {
 565         case PTP_PF_NONE:
 566         case PTP_PF_PEROUT:
 567         case PTP_PF_EXTTS:
 568                 break;
 569         default:
 570                 return -1;
 571         }
 572
 573         /* The PTP pins are shared by all the PHC. So it is required to see if
 574          * the pin is connected to another PHC. The pin is connected to another
 575          * PHC if that pin already has a function on that PHC.
 576          */
 577         for (i = 0; i < LAN966X_PHC_COUNT; ++i) {
 578                 info = &lan966x->phc[i].info;
 579
 580                 /* Ignore the check with ourself */
 581                 if (ptp == info)
 582                         continue;
 583
 584                 if (info->pin_config[pin].func == PTP_PF_PEROUT ||
 585                     info->pin_config[pin].func == PTP_PF_EXTTS)
 586                         return -1;
 587         }
 588
 589         return 0;
 590 }
 591
 592 static int lan966x_ptp_perout(struct ptp_clock_info *ptp,
 593                               struct ptp_clock_request *rq, int on)
 594 {
 595         struct lan966x_phc *phc = container_of(ptp, struct lan966x_phc, info);
 596         struct lan966x *lan966x = phc->lan966x;
 597         struct timespec64 ts_phase, ts_period;
 598         unsigned long flags;
 599         s64 wf_high, wf_low;
 600         bool pps = false;
 601         int pin;
 602
 603         if (rq->perout.flags & ~(PTP_PEROUT_DUTY_CYCLE |
 604                                  PTP_PEROUT_PHASE))
 605                 return -EOPNOTSUPP;
 606
 607         pin = ptp_find_pin(phc->clock, PTP_PF_PEROUT, rq->perout.index);
 608         if (pin == -1 || pin >= LAN966X_PHC_PINS_NUM)
 609                 return -EINVAL;
 610
 611         if (!on) {
 612                 spin_lock_irqsave(&lan966x->ptp_clock_lock, flags);
 613                 lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_IDLE) |
 614                         PTP_PIN_CFG_PIN_DOM_SET(phc->index) |
 615                         PTP_PIN_CFG_PIN_SYNC_SET(0),
 616                         PTP_PIN_CFG_PIN_ACTION |
 617                         PTP_PIN_CFG_PIN_DOM |
 618                         PTP_PIN_CFG_PIN_SYNC,
 619                         lan966x, PTP_PIN_CFG(pin));
 620                 spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags);
 621                 return 0;
 622         }
 623
 624         if (rq->perout.period.sec == 1 &&
 625             rq->perout.period.nsec == 0)
 626                 pps = true;
 627
 628         if (rq->perout.flags & PTP_PEROUT_PHASE) {
 629                 ts_phase.tv_sec = rq->perout.phase.sec;
 630                 ts_phase.tv_nsec = rq->perout.phase.nsec;
 631         } else {
 632                 ts_phase.tv_sec = rq->perout.start.sec;
 633                 ts_phase.tv_nsec = rq->perout.start.nsec;
 634         }
 635
 636         if (ts_phase.tv_sec || (ts_phase.tv_nsec && !pps)) {
 637                 dev_warn(lan966x->dev,
 638                          "Absolute time not supported!\n");
 639                 return -EINVAL;
 640         }
 641
 642         if (rq->perout.flags & PTP_PEROUT_DUTY_CYCLE) {
 643                 struct timespec64 ts_on;
 644
 645                 ts_on.tv_sec = rq->perout.on.sec;
 646                 ts_on.tv_nsec = rq->perout.on.nsec;
 647
 648                 wf_high = timespec64_to_ns(&ts_on);
 649         } else {
 650                 wf_high = 5000;
 651         }
 652
 653         if (pps) {
 654                 spin_lock_irqsave(&lan966x->ptp_clock_lock, flags);
 655                 lan_wr(PTP_WF_LOW_PERIOD_PIN_WFL(ts_phase.tv_nsec),
 656                        lan966x, PTP_WF_LOW_PERIOD(pin));
 657                 lan_wr(PTP_WF_HIGH_PERIOD_PIN_WFH(wf_high),
 658                        lan966x, PTP_WF_HIGH_PERIOD(pin));
 659                 lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_CLOCK) |
 660                         PTP_PIN_CFG_PIN_DOM_SET(phc->index) |
 661                         PTP_PIN_CFG_PIN_SYNC_SET(3),
 662                         PTP_PIN_CFG_PIN_ACTION |
 663                         PTP_PIN_CFG_PIN_DOM |
 664                         PTP_PIN_CFG_PIN_SYNC,
 665                         lan966x, PTP_PIN_CFG(pin));
 666                 spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags);
 667                 return 0;
 668         }
 669
 670         ts_period.tv_sec = rq->perout.period.sec;
 671         ts_period.tv_nsec = rq->perout.period.nsec;
 672
 673         wf_low = timespec64_to_ns(&ts_period);
 674         wf_low -= wf_high;
 675
 676         spin_lock_irqsave(&lan966x->ptp_clock_lock, flags);
 677         lan_wr(PTP_WF_LOW_PERIOD_PIN_WFL(wf_low),
 678                lan966x, PTP_WF_LOW_PERIOD(pin));
 679         lan_wr(PTP_WF_HIGH_PERIOD_PIN_WFH(wf_high),
 680                lan966x, PTP_WF_HIGH_PERIOD(pin));
 681         lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_CLOCK) |
 682                 PTP_PIN_CFG_PIN_DOM_SET(phc->index) |
 683                 PTP_PIN_CFG_PIN_SYNC_SET(0),
 684                 PTP_PIN_CFG_PIN_ACTION |
 685                 PTP_PIN_CFG_PIN_DOM |
 686                 PTP_PIN_CFG_PIN_SYNC,
 687                 lan966x, PTP_PIN_CFG(pin));
 688         spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags);
 689
 690         return 0;
 691 }
 692
 693 static int lan966x_ptp_extts(struct ptp_clock_info *ptp,
 694                              struct ptp_clock_request *rq, int on)
 695 {
 696         struct lan966x_phc *phc = container_of(ptp, struct lan966x_phc, info);
 697         struct lan966x *lan966x = phc->lan966x;
 698         unsigned long flags;
 699         int pin;
 700         u32 val;
 701
 702         if (lan966x->ptp_ext_irq <= 0)
 703                 return -EOPNOTSUPP;
 704
 705         /* Reject requests with unsupported flags */
 706         if (rq->extts.flags & ~(PTP_ENABLE_FEATURE |
 707                                 PTP_RISING_EDGE |
 708                                 PTP_STRICT_FLAGS))
 709                 return -EOPNOTSUPP;
 710
 711         pin = ptp_find_pin(phc->clock, PTP_PF_EXTTS, rq->extts.index);
 712         if (pin == -1 || pin >= LAN966X_PHC_PINS_NUM)
 713                 return -EINVAL;
 714
 715         spin_lock_irqsave(&lan966x->ptp_clock_lock, flags);
 716         lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_SAVE) |
 717                 PTP_PIN_CFG_PIN_SYNC_SET(on ? 3 : 0) |
 718                 PTP_PIN_CFG_PIN_DOM_SET(phc->index) |
 719                 PTP_PIN_CFG_PIN_SELECT_SET(pin),
 720                 PTP_PIN_CFG_PIN_ACTION |
 721                 PTP_PIN_CFG_PIN_SYNC |
 722                 PTP_PIN_CFG_PIN_DOM |
 723                 PTP_PIN_CFG_PIN_SELECT,
 724                 lan966x, PTP_PIN_CFG(pin));
 725
 726         val = lan_rd(lan966x, PTP_PIN_INTR_ENA);
 727         if (on)
 728                 val |= BIT(pin);
 729         else
 730                 val &= ~BIT(pin);
 731         lan_wr(val, lan966x, PTP_PIN_INTR_ENA);
 732
 733         spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags);
 734
 735         return 0;
 736 }
 737
 738 static int lan966x_ptp_enable(struct ptp_clock_info *ptp,
 739                               struct ptp_clock_request *rq, int on)
 740 {
 741         switch (rq->type) {
 742         case PTP_CLK_REQ_PEROUT:
 743                 return lan966x_ptp_perout(ptp, rq, on);
 744         case PTP_CLK_REQ_EXTTS:
 745                 return lan966x_ptp_extts(ptp, rq, on);
 746         default:
 747                 return -EOPNOTSUPP;
 748         }
 749
 750         return 0;
 751 }
 752
 753 static struct ptp_clock_info lan966x_ptp_clock_info = {
 754         .owner          = THIS_MODULE,
 755         .name           = "lan966x ptp",
 756         .max_adj        = 200000,
 757         .gettime64      = lan966x_ptp_gettime64,
 758         .settime64      = lan966x_ptp_settime64,
 759         .adjtime        = lan966x_ptp_adjtime,
 760         .adjfine        = lan966x_ptp_adjfine,
 761         .verify         = lan966x_ptp_verify,
 762         .enable         = lan966x_ptp_enable,
 763         .n_per_out      = LAN966X_PHC_PINS_NUM,
 764         .n_ext_ts       = LAN966X_PHC_PINS_NUM,
 765         .n_pins         = LAN966X_PHC_PINS_NUM,
 766 };
 767
 768 static int lan966x_ptp_phc_init(struct lan966x *lan966x,
 769                                 int index,
 770                                 struct ptp_clock_info *clock_info)
 771 {
 772         struct lan966x_phc *phc = &lan966x->phc[index];
 773         struct ptp_pin_desc *p;
 774         int i;
 775
 776         for (i = 0; i < LAN966X_PHC_PINS_NUM; i++) {
 777                 p = &phc->pins[i];
 778
 779                 snprintf(p->name, sizeof(p->name), "pin%d", i);
 780                 p->index = i;
 781                 p->func = PTP_PF_NONE;
 782         }
 783
 784         phc->info = *clock_info;
 785         phc->info.pin_config = &phc->pins[0];
 786         phc->clock = ptp_clock_register(&phc->info, lan966x->dev);
 787         if (IS_ERR(phc->clock))
 788                 return PTR_ERR(phc->clock);
 789
 790         phc->index = index;
 791         phc->lan966x = lan966x;
 792
 793         /* PTP Rx stamping is always enabled.  */
 794         phc->hwtstamp_config.rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
 795
 796         return 0;
 797 }
 798
 799 int lan966x_ptp_init(struct lan966x *lan966x)
 800 {
 801         u64 tod_adj = lan966x_ptp_get_nominal_value();
 802         struct lan966x_port *port;
 803         int err, i;
 804
 805         if (!lan966x->ptp)
 806                 return 0;
 807
 808         for (i = 0; i < LAN966X_PHC_COUNT; ++i) {
 809                 err = lan966x_ptp_phc_init(lan966x, i, &lan966x_ptp_clock_info);
 810                 if (err)
 811                         return err;
 812         }
 813
 814         spin_lock_init(&lan966x->ptp_clock_lock);
 815         spin_lock_init(&lan966x->ptp_ts_id_lock);
 816         mutex_init(&lan966x->ptp_lock);
 817
 818         /* Disable master counters */
 819         lan_wr(PTP_DOM_CFG_ENA_SET(0), lan966x, PTP_DOM_CFG);
 820
 821         /* Configure the nominal TOD increment per clock cycle */
 822         lan_rmw(PTP_DOM_CFG_CLKCFG_DIS_SET(0x7),
 823                 PTP_DOM_CFG_CLKCFG_DIS,
 824                 lan966x, PTP_DOM_CFG);
 825
 826         for (i = 0; i < LAN966X_PHC_COUNT; ++i) {
 827                 lan_wr((u32)tod_adj & 0xFFFFFFFF, lan966x,
 828                        PTP_CLK_PER_CFG(i, 0));
 829                 lan_wr((u32)(tod_adj >> 32), lan966x,
 830                        PTP_CLK_PER_CFG(i, 1));
 831         }
 832
 833         lan_rmw(PTP_DOM_CFG_CLKCFG_DIS_SET(0),
 834                 PTP_DOM_CFG_CLKCFG_DIS,
 835                 lan966x, PTP_DOM_CFG);
 836
 837         /* Enable master counters */
 838         lan_wr(PTP_DOM_CFG_ENA_SET(0x7), lan966x, PTP_DOM_CFG);
 839
 840         for (i = 0; i < lan966x->num_phys_ports; i++) {
 841                 port = lan966x->ports[i];
 842                 if (!port)
 843                         continue;
 844
 845                 skb_queue_head_init(&port->tx_skbs);
 846         }
 847
 848         return 0;
 849 }
 850
 851 void lan966x_ptp_deinit(struct lan966x *lan966x)
 852 {
 853         struct lan966x_port *port;
 854         int i;
 855
 856         for (i = 0; i < lan966x->num_phys_ports; i++) {
 857                 port = lan966x->ports[i];
 858                 if (!port)
 859                         continue;
 860
 861                 skb_queue_purge(&port->tx_skbs);
 862         }
 863
 864         for (i = 0; i < LAN966X_PHC_COUNT; ++i)
 865                 ptp_clock_unregister(lan966x->phc[i].clock);
 866 }
 867
 868 void lan966x_ptp_rxtstamp(struct lan966x *lan966x, struct sk_buff *skb,
 869                           u64 timestamp)
 870 {
 871         struct skb_shared_hwtstamps *shhwtstamps;
 872         struct lan966x_phc *phc;
 873         struct timespec64 ts;
 874         u64 full_ts_in_ns;
 875
 876         if (!lan966x->ptp)
 877                 return;
 878
 879         phc = &lan966x->phc[LAN966X_PHC_PORT];
 880         lan966x_ptp_gettime64(&phc->info, &ts);
 881
 882         /* Drop the sub-ns precision */
 883         timestamp = timestamp >> 2;
 884         if (ts.tv_nsec < timestamp)
 885                 ts.tv_sec--;
 886         ts.tv_nsec = timestamp;
 887         full_ts_in_ns = ktime_set(ts.tv_sec, ts.tv_nsec);
 888
 889         shhwtstamps = skb_hwtstamps(skb);
 890         shhwtstamps->hwtstamp = full_ts_in_ns;
 891 }
 892
 893 u32 lan966x_ptp_get_period_ps(void)
 894 {
 895         /* This represents the system clock period in picoseconds */
 896         return 15125;
 897 }