microchip/sparx5/sparx5_ptp.c

0001 // SPDX-License-Identifier: GPL-2.0+
0002 /* Microchip Sparx5 Switch driver
0003  *
0004  * Copyright (c) 2021 Microchip Technology Inc. and its subsidiaries.
0005  *
0006  * The Sparx5 Chip Register Model can be browsed at this location:
0007  * https://github.com/microchip-ung/sparx-5_reginfo
0008  */
0009 #include <linux/ptp_classify.h>
0010
0011 #include "sparx5_main_regs.h"
0012 #include "sparx5_main.h"
0013
0014 #define SPARX5_MAX_PTP_ID   512
0015
0016 #define TOD_ACC_PIN     0x4
0017
0018 enum {
0019     PTP_PIN_ACTION_IDLE = 0,
0020     PTP_PIN_ACTION_LOAD,
0021     PTP_PIN_ACTION_SAVE,
0022     PTP_PIN_ACTION_CLOCK,
0023     PTP_PIN_ACTION_DELTA,
0024     PTP_PIN_ACTION_TOD
0025 };
0026
0027 static u64 sparx5_ptp_get_1ppm(struct sparx5 *sparx5)
0028 {
0029     /* Represents 1ppm adjustment in 2^59 format with 1.59687500000(625)
0030      * 1.99609375000(500), 3.99218750000(250) as reference
0031      * The value is calculated as following:
0032      * (1/1000000)/((2^-59)/X)
0033      */
0034
0035     u64 res = 0;
0036
0037     switch (sparx5->coreclock) {
0038     case SPX5_CORE_CLOCK_250MHZ:
0039         res = 2301339409586;
0040         break;
0041     case SPX5_CORE_CLOCK_500MHZ:
0042         res = 1150669704793;
0043         break;
0044     case SPX5_CORE_CLOCK_625MHZ:
0045         res =  920535763834;
0046         break;
0047     default:
0048         WARN(1, "Invalid core clock");
0049         break;
0050     }
0051
0052     return res;
0053 }
0054
0055 static u64 sparx5_ptp_get_nominal_value(struct sparx5 *sparx5)
0056 {
0057     u64 res = 0;
0058
0059     switch (sparx5->coreclock) {
0060     case SPX5_CORE_CLOCK_250MHZ:
0061         res = 0x1FF0000000000000;
0062         break;
0063     case SPX5_CORE_CLOCK_500MHZ:
0064         res = 0x0FF8000000000000;
0065         break;
0066     case SPX5_CORE_CLOCK_625MHZ:
0067         res = 0x0CC6666666666666;
0068         break;
0069     default:
0070         WARN(1, "Invalid core clock");
0071         break;
0072     }
0073
0074     return res;
0075 }
0076
0077 int sparx5_ptp_hwtstamp_set(struct sparx5_port *port, struct ifreq *ifr)
0078 {
0079     struct sparx5 *sparx5 = port->sparx5;
0080     struct hwtstamp_config cfg;
0081     struct sparx5_phc *phc;
0082
0083     /* For now don't allow to run ptp on ports that are part of a bridge,
0084      * because in case of transparent clock the HW will still forward the
0085      * frames, so there would be duplicate frames
0086      */
0087
0088     if (test_bit(port->portno, sparx5->bridge_mask))
0089         return -EINVAL;
0090
0091     if (copy_from_user(&cfg, ifr->ifr_data, sizeof(cfg)))
0092         return -EFAULT;
0093
0094     switch (cfg.tx_type) {
0095     case HWTSTAMP_TX_ON:
0096         port->ptp_cmd = IFH_REW_OP_TWO_STEP_PTP;
0097         break;
0098     case HWTSTAMP_TX_ONESTEP_SYNC:
0099         port->ptp_cmd = IFH_REW_OP_ONE_STEP_PTP;
0100         break;
0101     case HWTSTAMP_TX_OFF:
0102         port->ptp_cmd = IFH_REW_OP_NOOP;
0103         break;
0104     default:
0105         return -ERANGE;
0106     }
0107
0108     switch (cfg.rx_filter) {
0109     case HWTSTAMP_FILTER_NONE:
0110         break;
0111     case HWTSTAMP_FILTER_ALL:
0112     case HWTSTAMP_FILTER_PTP_V1_L4_EVENT:
0113     case HWTSTAMP_FILTER_PTP_V1_L4_SYNC:
0114     case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ:
0115     case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
0116     case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
0117     case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
0118     case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
0119     case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
0120     case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
0121     case HWTSTAMP_FILTER_PTP_V2_EVENT:
0122     case HWTSTAMP_FILTER_PTP_V2_SYNC:
0123     case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
0124     case HWTSTAMP_FILTER_NTP_ALL:
0125         cfg.rx_filter = HWTSTAMP_FILTER_ALL;
0126         break;
0127     default:
0128         return -ERANGE;
0129     }
0130
0131     /* Commit back the result & save it */
0132     mutex_lock(&sparx5->ptp_lock);
0133     phc = &sparx5->phc[SPARX5_PHC_PORT];
0134     memcpy(&phc->hwtstamp_config, &cfg, sizeof(cfg));
0135     mutex_unlock(&sparx5->ptp_lock);
0136
0137     return copy_to_user(ifr->ifr_data, &cfg, sizeof(cfg)) ? -EFAULT : 0;
0138 }
0139
0140 int sparx5_ptp_hwtstamp_get(struct sparx5_port *port, struct ifreq *ifr)
0141 {
0142     struct sparx5 *sparx5 = port->sparx5;
0143     struct sparx5_phc *phc;
0144
0145     phc = &sparx5->phc[SPARX5_PHC_PORT];
0146     return copy_to_user(ifr->ifr_data, &phc->hwtstamp_config,
0147                 sizeof(phc->hwtstamp_config)) ? -EFAULT : 0;
0148 }
0149
0150 static void sparx5_ptp_classify(struct sparx5_port *port, struct sk_buff *skb,
0151                 u8 *rew_op, u8 *pdu_type, u8 *pdu_w16_offset)
0152 {
0153     struct ptp_header *header;
0154     u8 msgtype;
0155     int type;
0156
0157     if (port->ptp_cmd == IFH_REW_OP_NOOP) {
0158         *rew_op = IFH_REW_OP_NOOP;
0159         *pdu_type = IFH_PDU_TYPE_NONE;
0160         *pdu_w16_offset = 0;
0161         return;
0162     }
0163
0164     type = ptp_classify_raw(skb);
0165     if (type == PTP_CLASS_NONE) {
0166         *rew_op = IFH_REW_OP_NOOP;
0167         *pdu_type = IFH_PDU_TYPE_NONE;
0168         *pdu_w16_offset = 0;
0169         return;
0170     }
0171
0172     header = ptp_parse_header(skb, type);
0173     if (!header) {
0174         *rew_op = IFH_REW_OP_NOOP;
0175         *pdu_type = IFH_PDU_TYPE_NONE;
0176         *pdu_w16_offset = 0;
0177         return;
0178     }
0179
0180     *pdu_w16_offset = 7;
0181     if (type & PTP_CLASS_L2)
0182         *pdu_type = IFH_PDU_TYPE_PTP;
0183     if (type & PTP_CLASS_IPV4)
0184         *pdu_type = IFH_PDU_TYPE_IPV4_UDP_PTP;
0185     if (type & PTP_CLASS_IPV6)
0186         *pdu_type = IFH_PDU_TYPE_IPV6_UDP_PTP;
0187
0188     if (port->ptp_cmd == IFH_REW_OP_TWO_STEP_PTP) {
0189         *rew_op = IFH_REW_OP_TWO_STEP_PTP;
0190         return;
0191     }
0192
0193     /* If it is sync and run 1 step then set the correct operation,
0194      * otherwise run as 2 step
0195      */
0196     msgtype = ptp_get_msgtype(header, type);
0197     if ((msgtype & 0xf) == 0) {
0198         *rew_op = IFH_REW_OP_ONE_STEP_PTP;
0199         return;
0200     }
0201
0202     *rew_op = IFH_REW_OP_TWO_STEP_PTP;
0203 }
0204
0205 static void sparx5_ptp_txtstamp_old_release(struct sparx5_port *port)
0206 {
0207     struct sk_buff *skb, *skb_tmp;
0208     unsigned long flags;
0209
0210     spin_lock_irqsave(&port->tx_skbs.lock, flags);
0211     skb_queue_walk_safe(&port->tx_skbs, skb, skb_tmp) {
0212         if time_after(SPARX5_SKB_CB(skb)->jiffies + SPARX5_PTP_TIMEOUT,
0213                   jiffies)
0214             break;
0215
0216         __skb_unlink(skb, &port->tx_skbs);
0217         dev_kfree_skb_any(skb);
0218     }
0219     spin_unlock_irqrestore(&port->tx_skbs.lock, flags);
0220 }
0221
0222 int sparx5_ptp_txtstamp_request(struct sparx5_port *port,
0223                 struct sk_buff *skb)
0224 {
0225     struct sparx5 *sparx5 = port->sparx5;
0226     u8 rew_op, pdu_type, pdu_w16_offset;
0227     unsigned long flags;
0228
0229     sparx5_ptp_classify(port, skb, &rew_op, &pdu_type, &pdu_w16_offset);
0230     SPARX5_SKB_CB(skb)->rew_op = rew_op;
0231     SPARX5_SKB_CB(skb)->pdu_type = pdu_type;
0232     SPARX5_SKB_CB(skb)->pdu_w16_offset = pdu_w16_offset;
0233
0234     if (rew_op != IFH_REW_OP_TWO_STEP_PTP)
0235         return 0;
0236
0237     sparx5_ptp_txtstamp_old_release(port);
0238
0239     spin_lock_irqsave(&sparx5->ptp_ts_id_lock, flags);
0240     if (sparx5->ptp_skbs == SPARX5_MAX_PTP_ID) {
0241         spin_unlock_irqrestore(&sparx5->ptp_ts_id_lock, flags);
0242         return -EBUSY;
0243     }
0244
0245     skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
0246
0247     skb_queue_tail(&port->tx_skbs, skb);
0248     SPARX5_SKB_CB(skb)->ts_id = port->ts_id;
0249     SPARX5_SKB_CB(skb)->jiffies = jiffies;
0250
0251     sparx5->ptp_skbs++;
0252     port->ts_id++;
0253     if (port->ts_id == SPARX5_MAX_PTP_ID)
0254         port->ts_id = 0;
0255
0256     spin_unlock_irqrestore(&sparx5->ptp_ts_id_lock, flags);
0257
0258     return 0;
0259 }
0260
0261 void sparx5_ptp_txtstamp_release(struct sparx5_port *port,
0262                  struct sk_buff *skb)
0263 {
0264     struct sparx5 *sparx5 = port->sparx5;
0265     unsigned long flags;
0266
0267     spin_lock_irqsave(&sparx5->ptp_ts_id_lock, flags);
0268     port->ts_id--;
0269     sparx5->ptp_skbs--;
0270     skb_unlink(skb, &port->tx_skbs);
0271     spin_unlock_irqrestore(&sparx5->ptp_ts_id_lock, flags);
0272 }
0273
0274 static void sparx5_get_hwtimestamp(struct sparx5 *sparx5,
0275                    struct timespec64 *ts,
0276                    u32 nsec)
0277 {
0278     /* Read current PTP time to get seconds */
0279     unsigned long flags;
0280     u32 curr_nsec;
0281
0282     spin_lock_irqsave(&sparx5->ptp_clock_lock, flags);
0283
0284     spx5_rmw(PTP_PTP_PIN_CFG_PTP_PIN_ACTION_SET(PTP_PIN_ACTION_SAVE) |
0285          PTP_PTP_PIN_CFG_PTP_PIN_DOM_SET(SPARX5_PHC_PORT) |
0286          PTP_PTP_PIN_CFG_PTP_PIN_SYNC_SET(0),
0287          PTP_PTP_PIN_CFG_PTP_PIN_ACTION |
0288          PTP_PTP_PIN_CFG_PTP_PIN_DOM |
0289          PTP_PTP_PIN_CFG_PTP_PIN_SYNC,
0290          sparx5, PTP_PTP_PIN_CFG(TOD_ACC_PIN));
0291
0292     ts->tv_sec = spx5_rd(sparx5, PTP_PTP_TOD_SEC_LSB(TOD_ACC_PIN));
0293     curr_nsec = spx5_rd(sparx5, PTP_PTP_TOD_NSEC(TOD_ACC_PIN));
0294
0295     ts->tv_nsec = nsec;
0296
0297     /* Sec has incremented since the ts was registered */
0298     if (curr_nsec < nsec)
0299         ts->tv_sec--;
0300
0301     spin_unlock_irqrestore(&sparx5->ptp_clock_lock, flags);
0302 }
0303
0304 irqreturn_t sparx5_ptp_irq_handler(int irq, void *args)
0305 {
0306     int budget = SPARX5_MAX_PTP_ID;
0307     struct sparx5 *sparx5 = args;
0308
0309     while (budget--) {
0310         struct sk_buff *skb, *skb_tmp, *skb_match = NULL;
0311         struct skb_shared_hwtstamps shhwtstamps;
0312         struct sparx5_port *port;
0313         struct timespec64 ts;
0314         unsigned long flags;
0315         u32 val, id, txport;
0316         u32 delay;
0317
0318         val = spx5_rd(sparx5, REW_PTP_TWOSTEP_CTRL);
0319
0320         /* Check if a timestamp can be retrieved */
0321         if (!(val & REW_PTP_TWOSTEP_CTRL_PTP_VLD))
0322             break;
0323
0324         WARN_ON(val & REW_PTP_TWOSTEP_CTRL_PTP_OVFL);
0325
0326         if (!(val & REW_PTP_TWOSTEP_CTRL_STAMP_TX))
0327             continue;
0328
0329         /* Retrieve the ts Tx port */
0330         txport = REW_PTP_TWOSTEP_CTRL_STAMP_PORT_GET(val);
0331
0332         /* Retrieve its associated skb */
0333         port = sparx5->ports[txport];
0334
0335         /* Retrieve the delay */
0336         delay = spx5_rd(sparx5, REW_PTP_TWOSTEP_STAMP);
0337         delay = REW_PTP_TWOSTEP_STAMP_STAMP_NSEC_GET(delay);
0338
0339         /* Get next timestamp from fifo, which needs to be the
0340          * rx timestamp which represents the id of the frame
0341          */
0342         spx5_rmw(REW_PTP_TWOSTEP_CTRL_PTP_NXT_SET(1),
0343              REW_PTP_TWOSTEP_CTRL_PTP_NXT,
0344              sparx5, REW_PTP_TWOSTEP_CTRL);
0345
0346         val = spx5_rd(sparx5, REW_PTP_TWOSTEP_CTRL);
0347
0348         /* Check if a timestamp can be retried */
0349         if (!(val & REW_PTP_TWOSTEP_CTRL_PTP_VLD))
0350             break;
0351
0352         /* Read RX timestamping to get the ID */
0353         id = spx5_rd(sparx5, REW_PTP_TWOSTEP_STAMP);
0354         id <<= 8;
0355         id |= spx5_rd(sparx5, REW_PTP_TWOSTEP_STAMP_SUBNS);
0356
0357         spin_lock_irqsave(&port->tx_skbs.lock, flags);
0358         skb_queue_walk_safe(&port->tx_skbs, skb, skb_tmp) {
0359             if (SPARX5_SKB_CB(skb)->ts_id != id)
0360                 continue;
0361
0362             __skb_unlink(skb, &port->tx_skbs);
0363             skb_match = skb;
0364             break;
0365         }
0366         spin_unlock_irqrestore(&port->tx_skbs.lock, flags);
0367
0368         /* Next ts */
0369         spx5_rmw(REW_PTP_TWOSTEP_CTRL_PTP_NXT_SET(1),
0370              REW_PTP_TWOSTEP_CTRL_PTP_NXT,
0371              sparx5, REW_PTP_TWOSTEP_CTRL);
0372
0373         if (WARN_ON(!skb_match))
0374             continue;
0375
0376         spin_lock(&sparx5->ptp_ts_id_lock);
0377         sparx5->ptp_skbs--;
0378         spin_unlock(&sparx5->ptp_ts_id_lock);
0379
0380         /* Get the h/w timestamp */
0381         sparx5_get_hwtimestamp(sparx5, &ts, delay);
0382
0383         /* Set the timestamp into the skb */
0384         shhwtstamps.hwtstamp = ktime_set(ts.tv_sec, ts.tv_nsec);
0385         skb_tstamp_tx(skb_match, &shhwtstamps);
0386
0387         dev_kfree_skb_any(skb_match);
0388     }
0389
0390     return IRQ_HANDLED;
0391 }
0392
0393 static int sparx5_ptp_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)
0394 {
0395     struct sparx5_phc *phc = container_of(ptp, struct sparx5_phc, info);
0396     struct sparx5 *sparx5 = phc->sparx5;
0397     unsigned long flags;
0398     bool neg_adj = 0;
0399     u64 tod_inc;
0400     u64 ref;
0401
0402     if (!scaled_ppm)
0403         return 0;
0404
0405     if (scaled_ppm < 0) {
0406         neg_adj = 1;
0407         scaled_ppm = -scaled_ppm;
0408     }
0409
0410     tod_inc = sparx5_ptp_get_nominal_value(sparx5);
0411
0412     /* The multiplication is split in 2 separate additions because of
0413      * overflow issues. If scaled_ppm with 16bit fractional part was bigger
0414      * than 20ppm then we got overflow.
0415      */
0416     ref = sparx5_ptp_get_1ppm(sparx5) * (scaled_ppm >> 16);
0417     ref += (sparx5_ptp_get_1ppm(sparx5) * (0xffff & scaled_ppm)) >> 16;
0418     tod_inc = neg_adj ? tod_inc - ref : tod_inc + ref;
0419
0420     spin_lock_irqsave(&sparx5->ptp_clock_lock, flags);
0421
0422     spx5_rmw(PTP_PTP_DOM_CFG_PTP_CLKCFG_DIS_SET(1 << BIT(phc->index)),
0423          PTP_PTP_DOM_CFG_PTP_CLKCFG_DIS,
0424          sparx5, PTP_PTP_DOM_CFG);
0425
0426     spx5_wr((u32)tod_inc & 0xFFFFFFFF, sparx5,
0427         PTP_CLK_PER_CFG(phc->index, 0));
0428     spx5_wr((u32)(tod_inc >> 32), sparx5,
0429         PTP_CLK_PER_CFG(phc->index, 1));
0430
0431     spx5_rmw(PTP_PTP_DOM_CFG_PTP_CLKCFG_DIS_SET(0),
0432          PTP_PTP_DOM_CFG_PTP_CLKCFG_DIS, sparx5,
0433          PTP_PTP_DOM_CFG);
0434
0435     spin_unlock_irqrestore(&sparx5->ptp_clock_lock, flags);
0436
0437     return 0;
0438 }
0439
0440 static int sparx5_ptp_settime64(struct ptp_clock_info *ptp,
0441                 const struct timespec64 *ts)
0442 {
0443     struct sparx5_phc *phc = container_of(ptp, struct sparx5_phc, info);
0444     struct sparx5 *sparx5 = phc->sparx5;
0445     unsigned long flags;
0446
0447     spin_lock_irqsave(&sparx5->ptp_clock_lock, flags);
0448
0449     /* Must be in IDLE mode before the time can be loaded */
0450     spx5_rmw(PTP_PTP_PIN_CFG_PTP_PIN_ACTION_SET(PTP_PIN_ACTION_IDLE) |
0451          PTP_PTP_PIN_CFG_PTP_PIN_DOM_SET(phc->index) |
0452          PTP_PTP_PIN_CFG_PTP_PIN_SYNC_SET(0),
0453          PTP_PTP_PIN_CFG_PTP_PIN_ACTION |
0454          PTP_PTP_PIN_CFG_PTP_PIN_DOM |
0455          PTP_PTP_PIN_CFG_PTP_PIN_SYNC,
0456          sparx5, PTP_PTP_PIN_CFG(TOD_ACC_PIN));
0457
0458     /* Set new value */
0459     spx5_wr(PTP_PTP_TOD_SEC_MSB_PTP_TOD_SEC_MSB_SET(upper_32_bits(ts->tv_sec)),
0460         sparx5, PTP_PTP_TOD_SEC_MSB(TOD_ACC_PIN));
0461     spx5_wr(lower_32_bits(ts->tv_sec),
0462         sparx5, PTP_PTP_TOD_SEC_LSB(TOD_ACC_PIN));
0463     spx5_wr(ts->tv_nsec, sparx5, PTP_PTP_TOD_NSEC(TOD_ACC_PIN));
0464
0465     /* Apply new values */
0466     spx5_rmw(PTP_PTP_PIN_CFG_PTP_PIN_ACTION_SET(PTP_PIN_ACTION_LOAD) |
0467          PTP_PTP_PIN_CFG_PTP_PIN_DOM_SET(phc->index) |
0468          PTP_PTP_PIN_CFG_PTP_PIN_SYNC_SET(0),
0469          PTP_PTP_PIN_CFG_PTP_PIN_ACTION |
0470          PTP_PTP_PIN_CFG_PTP_PIN_DOM |
0471          PTP_PTP_PIN_CFG_PTP_PIN_SYNC,
0472          sparx5, PTP_PTP_PIN_CFG(TOD_ACC_PIN));
0473
0474     spin_unlock_irqrestore(&sparx5->ptp_clock_lock, flags);
0475
0476     return 0;
0477 }
0478
0479 static int sparx5_ptp_gettime64(struct ptp_clock_info *ptp,
0480                 struct timespec64 *ts)
0481 {
0482     struct sparx5_phc *phc = container_of(ptp, struct sparx5_phc, info);
0483     struct sparx5 *sparx5 = phc->sparx5;
0484     unsigned long flags;
0485     time64_t s;
0486     s64 ns;
0487
0488     spin_lock_irqsave(&sparx5->ptp_clock_lock, flags);
0489
0490     spx5_rmw(PTP_PTP_PIN_CFG_PTP_PIN_ACTION_SET(PTP_PIN_ACTION_SAVE) |
0491          PTP_PTP_PIN_CFG_PTP_PIN_DOM_SET(phc->index) |
0492          PTP_PTP_PIN_CFG_PTP_PIN_SYNC_SET(0),
0493          PTP_PTP_PIN_CFG_PTP_PIN_ACTION |
0494          PTP_PTP_PIN_CFG_PTP_PIN_DOM |
0495          PTP_PTP_PIN_CFG_PTP_PIN_SYNC,
0496          sparx5, PTP_PTP_PIN_CFG(TOD_ACC_PIN));
0497
0498     s = spx5_rd(sparx5, PTP_PTP_TOD_SEC_MSB(TOD_ACC_PIN));
0499     s <<= 32;
0500     s |= spx5_rd(sparx5, PTP_PTP_TOD_SEC_LSB(TOD_ACC_PIN));
0501     ns = spx5_rd(sparx5, PTP_PTP_TOD_NSEC(TOD_ACC_PIN));
0502     ns &= PTP_PTP_TOD_NSEC_PTP_TOD_NSEC;
0503
0504     spin_unlock_irqrestore(&sparx5->ptp_clock_lock, flags);
0505
0506     /* Deal with negative values */
0507     if ((ns & 0xFFFFFFF0) == 0x3FFFFFF0) {
0508         s--;
0509         ns &= 0xf;
0510         ns += 999999984;
0511     }
0512
0513     set_normalized_timespec64(ts, s, ns);
0514     return 0;
0515 }
0516
0517 static int sparx5_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
0518 {
0519     struct sparx5_phc *phc = container_of(ptp, struct sparx5_phc, info);
0520     struct sparx5 *sparx5 = phc->sparx5;
0521
0522     if (delta > -(NSEC_PER_SEC / 2) && delta < (NSEC_PER_SEC / 2)) {
0523         unsigned long flags;
0524
0525         spin_lock_irqsave(&sparx5->ptp_clock_lock, flags);
0526
0527         /* Must be in IDLE mode before the time can be loaded */
0528         spx5_rmw(PTP_PTP_PIN_CFG_PTP_PIN_ACTION_SET(PTP_PIN_ACTION_IDLE) |
0529              PTP_PTP_PIN_CFG_PTP_PIN_DOM_SET(phc->index) |
0530              PTP_PTP_PIN_CFG_PTP_PIN_SYNC_SET(0),
0531              PTP_PTP_PIN_CFG_PTP_PIN_ACTION |
0532              PTP_PTP_PIN_CFG_PTP_PIN_DOM |
0533              PTP_PTP_PIN_CFG_PTP_PIN_SYNC,
0534              sparx5, PTP_PTP_PIN_CFG(TOD_ACC_PIN));
0535
0536         spx5_wr(PTP_PTP_TOD_NSEC_PTP_TOD_NSEC_SET(delta),
0537             sparx5, PTP_PTP_TOD_NSEC(TOD_ACC_PIN));
0538
0539         /* Adjust time with the value of PTP_TOD_NSEC */
0540         spx5_rmw(PTP_PTP_PIN_CFG_PTP_PIN_ACTION_SET(PTP_PIN_ACTION_DELTA) |
0541              PTP_PTP_PIN_CFG_PTP_PIN_DOM_SET(phc->index) |
0542              PTP_PTP_PIN_CFG_PTP_PIN_SYNC_SET(0),
0543              PTP_PTP_PIN_CFG_PTP_PIN_ACTION |
0544              PTP_PTP_PIN_CFG_PTP_PIN_DOM |
0545              PTP_PTP_PIN_CFG_PTP_PIN_SYNC,
0546              sparx5, PTP_PTP_PIN_CFG(TOD_ACC_PIN));
0547
0548         spin_unlock_irqrestore(&sparx5->ptp_clock_lock, flags);
0549     } else {
0550         /* Fall back using sparx5_ptp_settime64 which is not exact */
0551         struct timespec64 ts;
0552         u64 now;
0553
0554         sparx5_ptp_gettime64(ptp, &ts);
0555
0556         now = ktime_to_ns(timespec64_to_ktime(ts));
0557         ts = ns_to_timespec64(now + delta);
0558
0559         sparx5_ptp_settime64(ptp, &ts);
0560     }
0561
0562     return 0;
0563 }
0564
0565 static struct ptp_clock_info sparx5_ptp_clock_info = {
0566     .owner      = THIS_MODULE,
0567     .name       = "sparx5 ptp",
0568     .max_adj    = 200000,
0569     .gettime64  = sparx5_ptp_gettime64,
0570     .settime64  = sparx5_ptp_settime64,
0571     .adjtime    = sparx5_ptp_adjtime,
0572     .adjfine    = sparx5_ptp_adjfine,
0573 };
0574
0575 static int sparx5_ptp_phc_init(struct sparx5 *sparx5,
0576                    int index,
0577                    struct ptp_clock_info *clock_info)
0578 {
0579     struct sparx5_phc *phc = &sparx5->phc[index];
0580
0581     phc->info = *clock_info;
0582     phc->clock = ptp_clock_register(&phc->info, sparx5->dev);
0583     if (IS_ERR(phc->clock))
0584         return PTR_ERR(phc->clock);
0585
0586     phc->index = index;
0587     phc->sparx5 = sparx5;
0588
0589     /* PTP Rx stamping is always enabled.  */
0590     phc->hwtstamp_config.rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
0591
0592     return 0;
0593 }
0594
0595 int sparx5_ptp_init(struct sparx5 *sparx5)
0596 {
0597     u64 tod_adj = sparx5_ptp_get_nominal_value(sparx5);
0598     struct sparx5_port *port;
0599     int err, i;
0600
0601     if (!sparx5->ptp)
0602         return 0;
0603
0604     for (i = 0; i < SPARX5_PHC_COUNT; ++i) {
0605         err = sparx5_ptp_phc_init(sparx5, i, &sparx5_ptp_clock_info);
0606         if (err)
0607             return err;
0608     }
0609
0610     spin_lock_init(&sparx5->ptp_clock_lock);
0611     spin_lock_init(&sparx5->ptp_ts_id_lock);
0612     mutex_init(&sparx5->ptp_lock);
0613
0614     /* Disable master counters */
0615     spx5_wr(PTP_PTP_DOM_CFG_PTP_ENA_SET(0), sparx5, PTP_PTP_DOM_CFG);
0616
0617     /* Configure the nominal TOD increment per clock cycle */
0618     spx5_rmw(PTP_PTP_DOM_CFG_PTP_CLKCFG_DIS_SET(0x7),
0619          PTP_PTP_DOM_CFG_PTP_CLKCFG_DIS,
0620          sparx5, PTP_PTP_DOM_CFG);
0621
0622     for (i = 0; i < SPARX5_PHC_COUNT; ++i) {
0623         spx5_wr((u32)tod_adj & 0xFFFFFFFF, sparx5,
0624             PTP_CLK_PER_CFG(i, 0));
0625         spx5_wr((u32)(tod_adj >> 32), sparx5,
0626             PTP_CLK_PER_CFG(i, 1));
0627     }
0628
0629     spx5_rmw(PTP_PTP_DOM_CFG_PTP_CLKCFG_DIS_SET(0),
0630          PTP_PTP_DOM_CFG_PTP_CLKCFG_DIS,
0631          sparx5, PTP_PTP_DOM_CFG);
0632
0633     /* Enable master counters */
0634     spx5_wr(PTP_PTP_DOM_CFG_PTP_ENA_SET(0x7), sparx5, PTP_PTP_DOM_CFG);
0635
0636     for (i = 0; i < sparx5->port_count; i++) {
0637         port = sparx5->ports[i];
0638         if (!port)
0639             continue;
0640
0641         skb_queue_head_init(&port->tx_skbs);
0642     }
0643
0644     return 0;
0645 }
0646
0647 void sparx5_ptp_deinit(struct sparx5 *sparx5)
0648 {
0649     struct sparx5_port *port;
0650     int i;
0651
0652     for (i = 0; i < sparx5->port_count; i++) {
0653         port = sparx5->ports[i];
0654         if (!port)
0655             continue;
0656
0657         skb_queue_purge(&port->tx_skbs);
0658     }
0659
0660     for (i = 0; i < SPARX5_PHC_COUNT; ++i)
0661         ptp_clock_unregister(sparx5->phc[i].clock);
0662 }
0663
0664 void sparx5_ptp_rxtstamp(struct sparx5 *sparx5, struct sk_buff *skb,
0665              u64 timestamp)
0666 {
0667     struct skb_shared_hwtstamps *shhwtstamps;
0668     struct sparx5_phc *phc;
0669     struct timespec64 ts;
0670     u64 full_ts_in_ns;
0671
0672     if (!sparx5->ptp)
0673         return;
0674
0675     phc = &sparx5->phc[SPARX5_PHC_PORT];
0676     sparx5_ptp_gettime64(&phc->info, &ts);
0677
0678     if (ts.tv_nsec < timestamp)
0679         ts.tv_sec--;
0680     ts.tv_nsec = timestamp;
0681     full_ts_in_ns = ktime_set(ts.tv_sec, ts.tv_nsec);
0682
0683     shhwtstamps = skb_hwtstamps(skb);
0684     shhwtstamps->hwtstamp = full_ts_in_ns;
0685 }