drivers/net/ethernet/microchip/sparx5/sparx5_ptp.c

   1 // SPDX-License-Identifier: GPL-2.0+
   2 /* Microchip Sparx5 Switch driver
   3  *
   4  * Copyright (c) 2021 Microchip Technology Inc. and its subsidiaries.
   5  *
   6  * The Sparx5 Chip Register Model can be browsed at this location:
   7  * https://github.com/microchip-ung/sparx-5_reginfo
   8  */
   9 #include <linux/ptp_classify.h>
  10
  11 #include "sparx5_main_regs.h"
  12 #include "sparx5_main.h"
  13
  14 #define SPARX5_MAX_PTP_ID       512
  15
  16 #define TOD_ACC_PIN             0x4
  17
  18 enum {
  19         PTP_PIN_ACTION_IDLE = 0,
  20         PTP_PIN_ACTION_LOAD,
  21         PTP_PIN_ACTION_SAVE,
  22         PTP_PIN_ACTION_CLOCK,
  23         PTP_PIN_ACTION_DELTA,
  24         PTP_PIN_ACTION_TOD
  25 };
  26
  27 static u64 sparx5_ptp_get_1ppm(struct sparx5 *sparx5)
  28 {
  29         /* Represents 1ppm adjustment in 2^59 format with 1.59687500000(625)
  30          * 1.99609375000(500), 3.99218750000(250) as reference
  31          * The value is calculated as following:
  32          * (1/1000000)/((2^-59)/X)
  33          */
  34
  35         u64 res = 0;
  36
  37         switch (sparx5->coreclock) {
  38         case SPX5_CORE_CLOCK_250MHZ:
  39                 res = 2301339409586;
  40                 break;
  41         case SPX5_CORE_CLOCK_500MHZ:
  42                 res = 1150669704793;
  43                 break;
  44         case SPX5_CORE_CLOCK_625MHZ:
  45                 res =  920535763834;
  46                 break;
  47         default:
  48                 WARN(1, "Invalid core clock");
  49                 break;
  50         }
  51
  52         return res;
  53 }
  54
  55 static u64 sparx5_ptp_get_nominal_value(struct sparx5 *sparx5)
  56 {
  57         u64 res = 0;
  58
  59         switch (sparx5->coreclock) {
  60         case SPX5_CORE_CLOCK_250MHZ:
  61                 res = 0x1FF0000000000000;
  62                 break;
  63         case SPX5_CORE_CLOCK_500MHZ:
  64                 res = 0x0FF8000000000000;
  65                 break;
  66         case SPX5_CORE_CLOCK_625MHZ:
  67                 res = 0x0CC6666666666666;
  68                 break;
  69         default:
  70                 WARN(1, "Invalid core clock");
  71                 break;
  72         }
  73
  74         return res;
  75 }
  76
  77 int sparx5_ptp_hwtstamp_set(struct sparx5_port *port,
  78                             struct kernel_hwtstamp_config *cfg,
  79                             struct netlink_ext_ack *extack)
  80 {
  81         struct sparx5 *sparx5 = port->sparx5;
  82         struct sparx5_phc *phc;
  83
  84         /* For now don't allow to run ptp on ports that are part of a bridge,
  85          * because in case of transparent clock the HW will still forward the
  86          * frames, so there would be duplicate frames
  87          */
  88
  89         if (test_bit(port->portno, sparx5->bridge_mask))
  90                 return -EINVAL;
  91
  92         switch (cfg->tx_type) {
  93         case HWTSTAMP_TX_ON:
  94                 port->ptp_cmd = IFH_REW_OP_TWO_STEP_PTP;
  95                 break;
  96         case HWTSTAMP_TX_ONESTEP_SYNC:
  97                 port->ptp_cmd = IFH_REW_OP_ONE_STEP_PTP;
  98                 break;
  99         case HWTSTAMP_TX_OFF:
 100                 port->ptp_cmd = IFH_REW_OP_NOOP;
 101                 break;
 102         default:
 103                 return -ERANGE;
 104         }
 105
 106         switch (cfg->rx_filter) {
 107         case HWTSTAMP_FILTER_NONE:
 108                 break;
 109         case HWTSTAMP_FILTER_ALL:
 110         case HWTSTAMP_FILTER_PTP_V1_L4_EVENT:
 111         case HWTSTAMP_FILTER_PTP_V1_L4_SYNC:
 112         case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ:
 113         case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
 114         case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
 115         case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
 116         case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
 117         case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
 118         case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
 119         case HWTSTAMP_FILTER_PTP_V2_EVENT:
 120         case HWTSTAMP_FILTER_PTP_V2_SYNC:
 121         case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
 122         case HWTSTAMP_FILTER_NTP_ALL:
 123                 cfg->rx_filter = HWTSTAMP_FILTER_ALL;
 124                 break;
 125         default:
 126                 return -ERANGE;
 127         }
 128
 129         /* Commit back the result & save it */
 130         mutex_lock(&sparx5->ptp_lock);
 131         phc = &sparx5->phc[SPARX5_PHC_PORT];
 132         phc->hwtstamp_config = *cfg;
 133         mutex_unlock(&sparx5->ptp_lock);
 134
 135         return 0;
 136 }
 137
 138 void sparx5_ptp_hwtstamp_get(struct sparx5_port *port,
 139                              struct kernel_hwtstamp_config *cfg)
 140 {
 141         struct sparx5 *sparx5 = port->sparx5;
 142         struct sparx5_phc *phc;
 143
 144         phc = &sparx5->phc[SPARX5_PHC_PORT];
 145         *cfg = phc->hwtstamp_config;
 146 }
 147
 148 static void sparx5_ptp_classify(struct sparx5_port *port, struct sk_buff *skb,
 149                                 u8 *rew_op, u8 *pdu_type, u8 *pdu_w16_offset)
 150 {
 151         struct ptp_header *header;
 152         u8 msgtype;
 153         int type;
 154
 155         if (port->ptp_cmd == IFH_REW_OP_NOOP) {
 156                 *rew_op = IFH_REW_OP_NOOP;
 157                 *pdu_type = IFH_PDU_TYPE_NONE;
 158                 *pdu_w16_offset = 0;
 159                 return;
 160         }
 161
 162         type = ptp_classify_raw(skb);
 163         if (type == PTP_CLASS_NONE) {
 164                 *rew_op = IFH_REW_OP_NOOP;
 165                 *pdu_type = IFH_PDU_TYPE_NONE;
 166                 *pdu_w16_offset = 0;
 167                 return;
 168         }
 169
 170         header = ptp_parse_header(skb, type);
 171         if (!header) {
 172                 *rew_op = IFH_REW_OP_NOOP;
 173                 *pdu_type = IFH_PDU_TYPE_NONE;
 174                 *pdu_w16_offset = 0;
 175                 return;
 176         }
 177
 178         *pdu_w16_offset = 7;
 179         if (type & PTP_CLASS_L2)
 180                 *pdu_type = IFH_PDU_TYPE_PTP;
 181         if (type & PTP_CLASS_IPV4)
 182                 *pdu_type = IFH_PDU_TYPE_IPV4_UDP_PTP;
 183         if (type & PTP_CLASS_IPV6)
 184                 *pdu_type = IFH_PDU_TYPE_IPV6_UDP_PTP;
 185
 186         if (port->ptp_cmd == IFH_REW_OP_TWO_STEP_PTP) {
 187                 *rew_op = IFH_REW_OP_TWO_STEP_PTP;
 188                 return;
 189         }
 190
 191         /* If it is sync and run 1 step then set the correct operation,
 192          * otherwise run as 2 step
 193          */
 194         msgtype = ptp_get_msgtype(header, type);
 195         if ((msgtype & 0xf) == 0) {
 196                 *rew_op = IFH_REW_OP_ONE_STEP_PTP;
 197                 return;
 198         }
 199
 200         *rew_op = IFH_REW_OP_TWO_STEP_PTP;
 201 }
 202
 203 static void sparx5_ptp_txtstamp_old_release(struct sparx5_port *port)
 204 {
 205         struct sk_buff *skb, *skb_tmp;
 206         unsigned long flags;
 207
 208         spin_lock_irqsave(&port->tx_skbs.lock, flags);
 209         skb_queue_walk_safe(&port->tx_skbs, skb, skb_tmp) {
 210                 if time_after(SPARX5_SKB_CB(skb)->jiffies + SPARX5_PTP_TIMEOUT,
 211                               jiffies)
 212                         break;
 213
 214                 __skb_unlink(skb, &port->tx_skbs);
 215                 dev_kfree_skb_any(skb);
 216         }
 217         spin_unlock_irqrestore(&port->tx_skbs.lock, flags);
 218 }
 219
 220 int sparx5_ptp_txtstamp_request(struct sparx5_port *port,
 221                                 struct sk_buff *skb)
 222 {
 223         struct sparx5 *sparx5 = port->sparx5;
 224         u8 rew_op, pdu_type, pdu_w16_offset;
 225         unsigned long flags;
 226
 227         sparx5_ptp_classify(port, skb, &rew_op, &pdu_type, &pdu_w16_offset);
 228         SPARX5_SKB_CB(skb)->rew_op = rew_op;
 229         SPARX5_SKB_CB(skb)->pdu_type = pdu_type;
 230         SPARX5_SKB_CB(skb)->pdu_w16_offset = pdu_w16_offset;
 231
 232         if (rew_op != IFH_REW_OP_TWO_STEP_PTP)
 233                 return 0;
 234
 235         sparx5_ptp_txtstamp_old_release(port);
 236
 237         spin_lock_irqsave(&sparx5->ptp_ts_id_lock, flags);
 238         if (sparx5->ptp_skbs == SPARX5_MAX_PTP_ID) {
 239                 spin_unlock_irqrestore(&sparx5->ptp_ts_id_lock, flags);
 240                 return -EBUSY;
 241         }
 242
 243         skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
 244
 245         skb_queue_tail(&port->tx_skbs, skb);
 246         SPARX5_SKB_CB(skb)->ts_id = port->ts_id;
 247         SPARX5_SKB_CB(skb)->jiffies = jiffies;
 248
 249         sparx5->ptp_skbs++;
 250         port->ts_id++;
 251         if (port->ts_id == SPARX5_MAX_PTP_ID)
 252                 port->ts_id = 0;
 253
 254         spin_unlock_irqrestore(&sparx5->ptp_ts_id_lock, flags);
 255
 256         return 0;
 257 }
 258
 259 void sparx5_ptp_txtstamp_release(struct sparx5_port *port,
 260                                  struct sk_buff *skb)
 261 {
 262         struct sparx5 *sparx5 = port->sparx5;
 263         unsigned long flags;
 264
 265         spin_lock_irqsave(&sparx5->ptp_ts_id_lock, flags);
 266         port->ts_id--;
 267         sparx5->ptp_skbs--;
 268         skb_unlink(skb, &port->tx_skbs);
 269         spin_unlock_irqrestore(&sparx5->ptp_ts_id_lock, flags);
 270 }
 271
 272 static void sparx5_get_hwtimestamp(struct sparx5 *sparx5,
 273                                    struct timespec64 *ts,
 274                                    u32 nsec)
 275 {
 276         /* Read current PTP time to get seconds */
 277         unsigned long flags;
 278         u32 curr_nsec;
 279
 280         spin_lock_irqsave(&sparx5->ptp_clock_lock, flags);
 281
 282         spx5_rmw(PTP_PTP_PIN_CFG_PTP_PIN_ACTION_SET(PTP_PIN_ACTION_SAVE) |
 283                  PTP_PTP_PIN_CFG_PTP_PIN_DOM_SET(SPARX5_PHC_PORT) |
 284                  PTP_PTP_PIN_CFG_PTP_PIN_SYNC_SET(0),
 285                  PTP_PTP_PIN_CFG_PTP_PIN_ACTION |
 286                  PTP_PTP_PIN_CFG_PTP_PIN_DOM |
 287                  PTP_PTP_PIN_CFG_PTP_PIN_SYNC,
 288                  sparx5, PTP_PTP_PIN_CFG(TOD_ACC_PIN));
 289
 290         ts->tv_sec = spx5_rd(sparx5, PTP_PTP_TOD_SEC_LSB(TOD_ACC_PIN));
 291         curr_nsec = spx5_rd(sparx5, PTP_PTP_TOD_NSEC(TOD_ACC_PIN));
 292
 293         ts->tv_nsec = nsec;
 294
 295         /* Sec has incremented since the ts was registered */
 296         if (curr_nsec < nsec)
 297                 ts->tv_sec--;
 298
 299         spin_unlock_irqrestore(&sparx5->ptp_clock_lock, flags);
 300 }
 301
 302 irqreturn_t sparx5_ptp_irq_handler(int irq, void *args)
 303 {
 304         int budget = SPARX5_MAX_PTP_ID;
 305         struct sparx5 *sparx5 = args;
 306
 307         while (budget--) {
 308                 struct sk_buff *skb, *skb_tmp, *skb_match = NULL;
 309                 struct skb_shared_hwtstamps shhwtstamps;
 310                 struct sparx5_port *port;
 311                 struct timespec64 ts;
 312                 unsigned long flags;
 313                 u32 val, id, txport;
 314                 u32 delay;
 315
 316                 val = spx5_rd(sparx5, REW_PTP_TWOSTEP_CTRL);
 317
 318                 /* Check if a timestamp can be retrieved */
 319                 if (!(val & REW_PTP_TWOSTEP_CTRL_PTP_VLD))
 320                         break;
 321
 322                 WARN_ON(val & REW_PTP_TWOSTEP_CTRL_PTP_OVFL);
 323
 324                 if (!(val & REW_PTP_TWOSTEP_CTRL_STAMP_TX))
 325                         continue;
 326
 327                 /* Retrieve the ts Tx port */
 328                 txport = REW_PTP_TWOSTEP_CTRL_STAMP_PORT_GET(val);
 329
 330                 /* Retrieve its associated skb */
 331                 port = sparx5->ports[txport];
 332
 333                 /* Retrieve the delay */
 334                 delay = spx5_rd(sparx5, REW_PTP_TWOSTEP_STAMP);
 335                 delay = REW_PTP_TWOSTEP_STAMP_STAMP_NSEC_GET(delay);
 336
 337                 /* Get next timestamp from fifo, which needs to be the
 338                  * rx timestamp which represents the id of the frame
 339                  */
 340                 spx5_rmw(REW_PTP_TWOSTEP_CTRL_PTP_NXT_SET(1),
 341                          REW_PTP_TWOSTEP_CTRL_PTP_NXT,
 342                          sparx5, REW_PTP_TWOSTEP_CTRL);
 343
 344                 val = spx5_rd(sparx5, REW_PTP_TWOSTEP_CTRL);
 345
 346                 /* Check if a timestamp can be retried */
 347                 if (!(val & REW_PTP_TWOSTEP_CTRL_PTP_VLD))
 348                         break;
 349
 350                 /* Read RX timestamping to get the ID */
 351                 id = spx5_rd(sparx5, REW_PTP_TWOSTEP_STAMP);
 352                 id <<= 8;
 353                 id |= spx5_rd(sparx5, REW_PTP_TWOSTEP_STAMP_SUBNS);
 354
 355                 spin_lock_irqsave(&port->tx_skbs.lock, flags);
 356                 skb_queue_walk_safe(&port->tx_skbs, skb, skb_tmp) {
 357                         if (SPARX5_SKB_CB(skb)->ts_id != id)
 358                                 continue;
 359
 360                         __skb_unlink(skb, &port->tx_skbs);
 361                         skb_match = skb;
 362                         break;
 363                 }
 364                 spin_unlock_irqrestore(&port->tx_skbs.lock, flags);
 365
 366                 /* Next ts */
 367                 spx5_rmw(REW_PTP_TWOSTEP_CTRL_PTP_NXT_SET(1),
 368                          REW_PTP_TWOSTEP_CTRL_PTP_NXT,
 369                          sparx5, REW_PTP_TWOSTEP_CTRL);
 370
 371                 if (WARN_ON(!skb_match))
 372                         continue;
 373
 374                 spin_lock(&sparx5->ptp_ts_id_lock);
 375                 sparx5->ptp_skbs--;
 376                 spin_unlock(&sparx5->ptp_ts_id_lock);
 377
 378                 /* Get the h/w timestamp */
 379                 sparx5_get_hwtimestamp(sparx5, &ts, delay);
 380
 381                 /* Set the timestamp into the skb */
 382                 shhwtstamps.hwtstamp = ktime_set(ts.tv_sec, ts.tv_nsec);
 383                 skb_tstamp_tx(skb_match, &shhwtstamps);
 384
 385                 dev_kfree_skb_any(skb_match);
 386         }
 387
 388         return IRQ_HANDLED;
 389 }
 390
 391 static int sparx5_ptp_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)
 392 {
 393         struct sparx5_phc *phc = container_of(ptp, struct sparx5_phc, info);
 394         struct sparx5 *sparx5 = phc->sparx5;
 395         unsigned long flags;
 396         bool neg_adj = 0;
 397         u64 tod_inc;
 398         u64 ref;
 399
 400         if (!scaled_ppm)
 401                 return 0;
 402
 403         if (scaled_ppm < 0) {
 404                 neg_adj = 1;
 405                 scaled_ppm = -scaled_ppm;
 406         }
 407
 408         tod_inc = sparx5_ptp_get_nominal_value(sparx5);
 409
 410         /* The multiplication is split in 2 separate additions because of
 411          * overflow issues. If scaled_ppm with 16bit fractional part was bigger
 412          * than 20ppm then we got overflow.
 413          */
 414         ref = sparx5_ptp_get_1ppm(sparx5) * (scaled_ppm >> 16);
 415         ref += (sparx5_ptp_get_1ppm(sparx5) * (0xffff & scaled_ppm)) >> 16;
 416         tod_inc = neg_adj ? tod_inc - ref : tod_inc + ref;
 417
 418         spin_lock_irqsave(&sparx5->ptp_clock_lock, flags);
 419
 420         spx5_rmw(PTP_PTP_DOM_CFG_PTP_CLKCFG_DIS_SET(1 << BIT(phc->index)),
 421                  PTP_PTP_DOM_CFG_PTP_CLKCFG_DIS,
 422                  sparx5, PTP_PTP_DOM_CFG);
 423
 424         spx5_wr((u32)tod_inc & 0xFFFFFFFF, sparx5,
 425                 PTP_CLK_PER_CFG(phc->index, 0));
 426         spx5_wr((u32)(tod_inc >> 32), sparx5,
 427                 PTP_CLK_PER_CFG(phc->index, 1));
 428
 429         spx5_rmw(PTP_PTP_DOM_CFG_PTP_CLKCFG_DIS_SET(0),
 430                  PTP_PTP_DOM_CFG_PTP_CLKCFG_DIS, sparx5,
 431                  PTP_PTP_DOM_CFG);
 432
 433         spin_unlock_irqrestore(&sparx5->ptp_clock_lock, flags);
 434
 435         return 0;
 436 }
 437
 438 static int sparx5_ptp_settime64(struct ptp_clock_info *ptp,
 439                                 const struct timespec64 *ts)
 440 {
 441         struct sparx5_phc *phc = container_of(ptp, struct sparx5_phc, info);
 442         struct sparx5 *sparx5 = phc->sparx5;
 443         unsigned long flags;
 444
 445         spin_lock_irqsave(&sparx5->ptp_clock_lock, flags);
 446
 447         /* Must be in IDLE mode before the time can be loaded */
 448         spx5_rmw(PTP_PTP_PIN_CFG_PTP_PIN_ACTION_SET(PTP_PIN_ACTION_IDLE) |
 449                  PTP_PTP_PIN_CFG_PTP_PIN_DOM_SET(phc->index) |
 450                  PTP_PTP_PIN_CFG_PTP_PIN_SYNC_SET(0),
 451                  PTP_PTP_PIN_CFG_PTP_PIN_ACTION |
 452                  PTP_PTP_PIN_CFG_PTP_PIN_DOM |
 453                  PTP_PTP_PIN_CFG_PTP_PIN_SYNC,
 454                  sparx5, PTP_PTP_PIN_CFG(TOD_ACC_PIN));
 455
 456         /* Set new value */
 457         spx5_wr(PTP_PTP_TOD_SEC_MSB_PTP_TOD_SEC_MSB_SET(upper_32_bits(ts->tv_sec)),
 458                 sparx5, PTP_PTP_TOD_SEC_MSB(TOD_ACC_PIN));
 459         spx5_wr(lower_32_bits(ts->tv_sec),
 460                 sparx5, PTP_PTP_TOD_SEC_LSB(TOD_ACC_PIN));
 461         spx5_wr(ts->tv_nsec, sparx5, PTP_PTP_TOD_NSEC(TOD_ACC_PIN));
 462
 463         /* Apply new values */
 464         spx5_rmw(PTP_PTP_PIN_CFG_PTP_PIN_ACTION_SET(PTP_PIN_ACTION_LOAD) |
 465                  PTP_PTP_PIN_CFG_PTP_PIN_DOM_SET(phc->index) |
 466                  PTP_PTP_PIN_CFG_PTP_PIN_SYNC_SET(0),
 467                  PTP_PTP_PIN_CFG_PTP_PIN_ACTION |
 468                  PTP_PTP_PIN_CFG_PTP_PIN_DOM |
 469                  PTP_PTP_PIN_CFG_PTP_PIN_SYNC,
 470                  sparx5, PTP_PTP_PIN_CFG(TOD_ACC_PIN));
 471
 472         spin_unlock_irqrestore(&sparx5->ptp_clock_lock, flags);
 473
 474         return 0;
 475 }
 476
 477 int sparx5_ptp_gettime64(struct ptp_clock_info *ptp, struct timespec64 *ts)
 478 {
 479         struct sparx5_phc *phc = container_of(ptp, struct sparx5_phc, info);
 480         struct sparx5 *sparx5 = phc->sparx5;
 481         unsigned long flags;
 482         time64_t s;
 483         s64 ns;
 484
 485         spin_lock_irqsave(&sparx5->ptp_clock_lock, flags);
 486
 487         spx5_rmw(PTP_PTP_PIN_CFG_PTP_PIN_ACTION_SET(PTP_PIN_ACTION_SAVE) |
 488                  PTP_PTP_PIN_CFG_PTP_PIN_DOM_SET(phc->index) |
 489                  PTP_PTP_PIN_CFG_PTP_PIN_SYNC_SET(0),
 490                  PTP_PTP_PIN_CFG_PTP_PIN_ACTION |
 491                  PTP_PTP_PIN_CFG_PTP_PIN_DOM |
 492                  PTP_PTP_PIN_CFG_PTP_PIN_SYNC,
 493                  sparx5, PTP_PTP_PIN_CFG(TOD_ACC_PIN));
 494
 495         s = spx5_rd(sparx5, PTP_PTP_TOD_SEC_MSB(TOD_ACC_PIN));
 496         s <<= 32;
 497         s |= spx5_rd(sparx5, PTP_PTP_TOD_SEC_LSB(TOD_ACC_PIN));
 498         ns = spx5_rd(sparx5, PTP_PTP_TOD_NSEC(TOD_ACC_PIN));
 499         ns &= PTP_PTP_TOD_NSEC_PTP_TOD_NSEC;
 500
 501         spin_unlock_irqrestore(&sparx5->ptp_clock_lock, flags);
 502
 503         /* Deal with negative values */
 504         if ((ns & 0xFFFFFFF0) == 0x3FFFFFF0) {
 505                 s--;
 506                 ns &= 0xf;
 507                 ns += 999999984;
 508         }
 509
 510         set_normalized_timespec64(ts, s, ns);
 511         return 0;
 512 }
 513
 514 static int sparx5_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
 515 {
 516         struct sparx5_phc *phc = container_of(ptp, struct sparx5_phc, info);
 517         struct sparx5 *sparx5 = phc->sparx5;
 518
 519         if (delta > -(NSEC_PER_SEC / 2) && delta < (NSEC_PER_SEC / 2)) {
 520                 unsigned long flags;
 521
 522                 spin_lock_irqsave(&sparx5->ptp_clock_lock, flags);
 523
 524                 /* Must be in IDLE mode before the time can be loaded */
 525                 spx5_rmw(PTP_PTP_PIN_CFG_PTP_PIN_ACTION_SET(PTP_PIN_ACTION_IDLE) |
 526                          PTP_PTP_PIN_CFG_PTP_PIN_DOM_SET(phc->index) |
 527                          PTP_PTP_PIN_CFG_PTP_PIN_SYNC_SET(0),
 528                          PTP_PTP_PIN_CFG_PTP_PIN_ACTION |
 529                          PTP_PTP_PIN_CFG_PTP_PIN_DOM |
 530                          PTP_PTP_PIN_CFG_PTP_PIN_SYNC,
 531                          sparx5, PTP_PTP_PIN_CFG(TOD_ACC_PIN));
 532
 533                 spx5_wr(PTP_PTP_TOD_NSEC_PTP_TOD_NSEC_SET(delta),
 534                         sparx5, PTP_PTP_TOD_NSEC(TOD_ACC_PIN));
 535
 536                 /* Adjust time with the value of PTP_TOD_NSEC */
 537                 spx5_rmw(PTP_PTP_PIN_CFG_PTP_PIN_ACTION_SET(PTP_PIN_ACTION_DELTA) |
 538                          PTP_PTP_PIN_CFG_PTP_PIN_DOM_SET(phc->index) |
 539                          PTP_PTP_PIN_CFG_PTP_PIN_SYNC_SET(0),
 540                          PTP_PTP_PIN_CFG_PTP_PIN_ACTION |
 541                          PTP_PTP_PIN_CFG_PTP_PIN_DOM |
 542                          PTP_PTP_PIN_CFG_PTP_PIN_SYNC,
 543                          sparx5, PTP_PTP_PIN_CFG(TOD_ACC_PIN));
 544
 545                 spin_unlock_irqrestore(&sparx5->ptp_clock_lock, flags);
 546         } else {
 547                 /* Fall back using sparx5_ptp_settime64 which is not exact */
 548                 struct timespec64 ts;
 549                 u64 now;
 550
 551                 sparx5_ptp_gettime64(ptp, &ts);
 552
 553                 now = ktime_to_ns(timespec64_to_ktime(ts));
 554                 ts = ns_to_timespec64(now + delta);
 555
 556                 sparx5_ptp_settime64(ptp, &ts);
 557         }
 558
 559         return 0;
 560 }
 561
 562 static struct ptp_clock_info sparx5_ptp_clock_info = {
 563         .owner          = THIS_MODULE,
 564         .name           = "sparx5 ptp",
 565         .max_adj        = 200000,
 566         .gettime64      = sparx5_ptp_gettime64,
 567         .settime64      = sparx5_ptp_settime64,
 568         .adjtime        = sparx5_ptp_adjtime,
 569         .adjfine        = sparx5_ptp_adjfine,
 570 };
 571
 572 static int sparx5_ptp_phc_init(struct sparx5 *sparx5,
 573                                int index,
 574                                struct ptp_clock_info *clock_info)
 575 {
 576         struct sparx5_phc *phc = &sparx5->phc[index];
 577
 578         phc->info = *clock_info;
 579         phc->clock = ptp_clock_register(&phc->info, sparx5->dev);
 580         if (IS_ERR(phc->clock))
 581                 return PTR_ERR(phc->clock);
 582
 583         phc->index = index;
 584         phc->sparx5 = sparx5;
 585
 586         /* PTP Rx stamping is always enabled.  */
 587         phc->hwtstamp_config.rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
 588
 589         return 0;
 590 }
 591
 592 int sparx5_ptp_init(struct sparx5 *sparx5)
 593 {
 594         u64 tod_adj = sparx5_ptp_get_nominal_value(sparx5);
 595         struct sparx5_port *port;
 596         int err, i;
 597
 598         if (!sparx5->ptp)
 599                 return 0;
 600
 601         for (i = 0; i < SPARX5_PHC_COUNT; ++i) {
 602                 err = sparx5_ptp_phc_init(sparx5, i, &sparx5_ptp_clock_info);
 603                 if (err)
 604                         return err;
 605         }
 606
 607         spin_lock_init(&sparx5->ptp_clock_lock);
 608         spin_lock_init(&sparx5->ptp_ts_id_lock);
 609         mutex_init(&sparx5->ptp_lock);
 610
 611         /* Disable master counters */
 612         spx5_wr(PTP_PTP_DOM_CFG_PTP_ENA_SET(0), sparx5, PTP_PTP_DOM_CFG);
 613
 614         /* Configure the nominal TOD increment per clock cycle */
 615         spx5_rmw(PTP_PTP_DOM_CFG_PTP_CLKCFG_DIS_SET(0x7),
 616                  PTP_PTP_DOM_CFG_PTP_CLKCFG_DIS,
 617                  sparx5, PTP_PTP_DOM_CFG);
 618
 619         for (i = 0; i < SPARX5_PHC_COUNT; ++i) {
 620                 spx5_wr((u32)tod_adj & 0xFFFFFFFF, sparx5,
 621                         PTP_CLK_PER_CFG(i, 0));
 622                 spx5_wr((u32)(tod_adj >> 32), sparx5,
 623                         PTP_CLK_PER_CFG(i, 1));
 624         }
 625
 626         spx5_rmw(PTP_PTP_DOM_CFG_PTP_CLKCFG_DIS_SET(0),
 627                  PTP_PTP_DOM_CFG_PTP_CLKCFG_DIS,
 628                  sparx5, PTP_PTP_DOM_CFG);
 629
 630         /* Enable master counters */
 631         spx5_wr(PTP_PTP_DOM_CFG_PTP_ENA_SET(0x7), sparx5, PTP_PTP_DOM_CFG);
 632
 633         for (i = 0; i < SPX5_PORTS; i++) {
 634                 port = sparx5->ports[i];
 635                 if (!port)
 636                         continue;
 637
 638                 skb_queue_head_init(&port->tx_skbs);
 639         }
 640
 641         return 0;
 642 }
 643
 644 void sparx5_ptp_deinit(struct sparx5 *sparx5)
 645 {
 646         struct sparx5_port *port;
 647         int i;
 648
 649         for (i = 0; i < SPX5_PORTS; i++) {
 650                 port = sparx5->ports[i];
 651                 if (!port)
 652                         continue;
 653
 654                 skb_queue_purge(&port->tx_skbs);
 655         }
 656
 657         for (i = 0; i < SPARX5_PHC_COUNT; ++i)
 658                 ptp_clock_unregister(sparx5->phc[i].clock);
 659 }
 660
 661 void sparx5_ptp_rxtstamp(struct sparx5 *sparx5, struct sk_buff *skb,
 662                          u64 timestamp)
 663 {
 664         struct skb_shared_hwtstamps *shhwtstamps;
 665         struct sparx5_phc *phc;
 666         struct timespec64 ts;
 667         u64 full_ts_in_ns;
 668
 669         if (!sparx5->ptp)
 670                 return;
 671
 672         phc = &sparx5->phc[SPARX5_PHC_PORT];
 673         sparx5_ptp_gettime64(&phc->info, &ts);
 674
 675         if (ts.tv_nsec < timestamp)
 676                 ts.tv_sec--;
 677         ts.tv_nsec = timestamp;
 678         full_ts_in_ns = ktime_set(ts.tv_sec, ts.tv_nsec);
 679
 680         shhwtstamps = skb_hwtstamps(skb);
 681         shhwtstamps->hwtstamp = full_ts_in_ns;
 682 }