Back to home page

OSCL-LXR
 
 

     

0001 // SPDX-License-Identifier: GPL-2.0+
0002 
0003 #include <linux/ptp_classify.h>
0004 
0005 #include "lan966x_main.h"
0006 
0007 #define LAN966X_MAX_PTP_ID  512
0008 
0009 /* Represents 1ppm adjustment in 2^59 format with 6.037735849ns as reference
0010  * The value is calculated as following: (1/1000000)/((2^-59)/6.037735849)
0011  */
0012 #define LAN966X_1PPM_FORMAT     3480517749723LL
0013 
0014 /* Represents 1ppb adjustment in 2^29 format with 6.037735849ns as reference
0015  * The value is calculated as following: (1/1000000000)/((2^59)/6.037735849)
0016  */
0017 #define LAN966X_1PPB_FORMAT     3480517749LL
0018 
0019 #define TOD_ACC_PIN     0x7
0020 
0021 enum {
0022     PTP_PIN_ACTION_IDLE = 0,
0023     PTP_PIN_ACTION_LOAD,
0024     PTP_PIN_ACTION_SAVE,
0025     PTP_PIN_ACTION_CLOCK,
0026     PTP_PIN_ACTION_DELTA,
0027     PTP_PIN_ACTION_TOD
0028 };
0029 
0030 static u64 lan966x_ptp_get_nominal_value(void)
0031 {
0032     /* This is the default value that for each system clock, the time of day
0033      * is increased. It has the format 5.59 nanosecond.
0034      */
0035     return 0x304d4873ecade305;
0036 }
0037 
0038 int lan966x_ptp_hwtstamp_set(struct lan966x_port *port, struct ifreq *ifr)
0039 {
0040     struct lan966x *lan966x = port->lan966x;
0041     struct hwtstamp_config cfg;
0042     struct lan966x_phc *phc;
0043 
0044     /* For now don't allow to run ptp on ports that are part of a bridge,
0045      * because in case of transparent clock the HW will still forward the
0046      * frames, so there would be duplicate frames
0047      */
0048     if (lan966x->bridge_mask & BIT(port->chip_port))
0049         return -EINVAL;
0050 
0051     if (copy_from_user(&cfg, ifr->ifr_data, sizeof(cfg)))
0052         return -EFAULT;
0053 
0054     switch (cfg.tx_type) {
0055     case HWTSTAMP_TX_ON:
0056         port->ptp_cmd = IFH_REW_OP_TWO_STEP_PTP;
0057         break;
0058     case HWTSTAMP_TX_ONESTEP_SYNC:
0059         port->ptp_cmd = IFH_REW_OP_ONE_STEP_PTP;
0060         break;
0061     case HWTSTAMP_TX_OFF:
0062         port->ptp_cmd = IFH_REW_OP_NOOP;
0063         break;
0064     default:
0065         return -ERANGE;
0066     }
0067 
0068     switch (cfg.rx_filter) {
0069     case HWTSTAMP_FILTER_NONE:
0070         break;
0071     case HWTSTAMP_FILTER_ALL:
0072     case HWTSTAMP_FILTER_PTP_V1_L4_EVENT:
0073     case HWTSTAMP_FILTER_PTP_V1_L4_SYNC:
0074     case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ:
0075     case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
0076     case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
0077     case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
0078     case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
0079     case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
0080     case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
0081     case HWTSTAMP_FILTER_PTP_V2_EVENT:
0082     case HWTSTAMP_FILTER_PTP_V2_SYNC:
0083     case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
0084     case HWTSTAMP_FILTER_NTP_ALL:
0085         cfg.rx_filter = HWTSTAMP_FILTER_ALL;
0086         break;
0087     default:
0088         return -ERANGE;
0089     }
0090 
0091     /* Commit back the result & save it */
0092     mutex_lock(&lan966x->ptp_lock);
0093     phc = &lan966x->phc[LAN966X_PHC_PORT];
0094     memcpy(&phc->hwtstamp_config, &cfg, sizeof(cfg));
0095     mutex_unlock(&lan966x->ptp_lock);
0096 
0097     return copy_to_user(ifr->ifr_data, &cfg, sizeof(cfg)) ? -EFAULT : 0;
0098 }
0099 
0100 int lan966x_ptp_hwtstamp_get(struct lan966x_port *port, struct ifreq *ifr)
0101 {
0102     struct lan966x *lan966x = port->lan966x;
0103     struct lan966x_phc *phc;
0104 
0105     phc = &lan966x->phc[LAN966X_PHC_PORT];
0106     return copy_to_user(ifr->ifr_data, &phc->hwtstamp_config,
0107                 sizeof(phc->hwtstamp_config)) ? -EFAULT : 0;
0108 }
0109 
0110 static int lan966x_ptp_classify(struct lan966x_port *port, struct sk_buff *skb)
0111 {
0112     struct ptp_header *header;
0113     u8 msgtype;
0114     int type;
0115 
0116     if (port->ptp_cmd == IFH_REW_OP_NOOP)
0117         return IFH_REW_OP_NOOP;
0118 
0119     type = ptp_classify_raw(skb);
0120     if (type == PTP_CLASS_NONE)
0121         return IFH_REW_OP_NOOP;
0122 
0123     header = ptp_parse_header(skb, type);
0124     if (!header)
0125         return IFH_REW_OP_NOOP;
0126 
0127     if (port->ptp_cmd == IFH_REW_OP_TWO_STEP_PTP)
0128         return IFH_REW_OP_TWO_STEP_PTP;
0129 
0130     /* If it is sync and run 1 step then set the correct operation,
0131      * otherwise run as 2 step
0132      */
0133     msgtype = ptp_get_msgtype(header, type);
0134     if ((msgtype & 0xf) == 0)
0135         return IFH_REW_OP_ONE_STEP_PTP;
0136 
0137     return IFH_REW_OP_TWO_STEP_PTP;
0138 }
0139 
0140 static void lan966x_ptp_txtstamp_old_release(struct lan966x_port *port)
0141 {
0142     struct sk_buff *skb, *skb_tmp;
0143     unsigned long flags;
0144 
0145     spin_lock_irqsave(&port->tx_skbs.lock, flags);
0146     skb_queue_walk_safe(&port->tx_skbs, skb, skb_tmp) {
0147         if time_after(LAN966X_SKB_CB(skb)->jiffies + LAN966X_PTP_TIMEOUT,
0148                   jiffies)
0149             break;
0150 
0151         __skb_unlink(skb, &port->tx_skbs);
0152         dev_kfree_skb_any(skb);
0153     }
0154     spin_unlock_irqrestore(&port->tx_skbs.lock, flags);
0155 }
0156 
0157 int lan966x_ptp_txtstamp_request(struct lan966x_port *port,
0158                  struct sk_buff *skb)
0159 {
0160     struct lan966x *lan966x = port->lan966x;
0161     unsigned long flags;
0162     u8 rew_op;
0163 
0164     rew_op = lan966x_ptp_classify(port, skb);
0165     LAN966X_SKB_CB(skb)->rew_op = rew_op;
0166 
0167     if (rew_op != IFH_REW_OP_TWO_STEP_PTP)
0168         return 0;
0169 
0170     lan966x_ptp_txtstamp_old_release(port);
0171 
0172     spin_lock_irqsave(&lan966x->ptp_ts_id_lock, flags);
0173     if (lan966x->ptp_skbs == LAN966X_MAX_PTP_ID) {
0174         spin_unlock_irqrestore(&lan966x->ptp_ts_id_lock, flags);
0175         return -EBUSY;
0176     }
0177 
0178     skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
0179 
0180     skb_queue_tail(&port->tx_skbs, skb);
0181     LAN966X_SKB_CB(skb)->ts_id = port->ts_id;
0182     LAN966X_SKB_CB(skb)->jiffies = jiffies;
0183 
0184     lan966x->ptp_skbs++;
0185     port->ts_id++;
0186     if (port->ts_id == LAN966X_MAX_PTP_ID)
0187         port->ts_id = 0;
0188 
0189     spin_unlock_irqrestore(&lan966x->ptp_ts_id_lock, flags);
0190 
0191     return 0;
0192 }
0193 
0194 void lan966x_ptp_txtstamp_release(struct lan966x_port *port,
0195                   struct sk_buff *skb)
0196 {
0197     struct lan966x *lan966x = port->lan966x;
0198     unsigned long flags;
0199 
0200     spin_lock_irqsave(&lan966x->ptp_ts_id_lock, flags);
0201     port->ts_id--;
0202     lan966x->ptp_skbs--;
0203     skb_unlink(skb, &port->tx_skbs);
0204     spin_unlock_irqrestore(&lan966x->ptp_ts_id_lock, flags);
0205 }
0206 
0207 static void lan966x_get_hwtimestamp(struct lan966x *lan966x,
0208                     struct timespec64 *ts,
0209                     u32 nsec)
0210 {
0211     /* Read current PTP time to get seconds */
0212     unsigned long flags;
0213     u32 curr_nsec;
0214 
0215     spin_lock_irqsave(&lan966x->ptp_clock_lock, flags);
0216 
0217     lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_SAVE) |
0218         PTP_PIN_CFG_PIN_DOM_SET(LAN966X_PHC_PORT) |
0219         PTP_PIN_CFG_PIN_SYNC_SET(0),
0220         PTP_PIN_CFG_PIN_ACTION |
0221         PTP_PIN_CFG_PIN_DOM |
0222         PTP_PIN_CFG_PIN_SYNC,
0223         lan966x, PTP_PIN_CFG(TOD_ACC_PIN));
0224 
0225     ts->tv_sec = lan_rd(lan966x, PTP_TOD_SEC_LSB(TOD_ACC_PIN));
0226     curr_nsec = lan_rd(lan966x, PTP_TOD_NSEC(TOD_ACC_PIN));
0227 
0228     ts->tv_nsec = nsec;
0229 
0230     /* Sec has incremented since the ts was registered */
0231     if (curr_nsec < nsec)
0232         ts->tv_sec--;
0233 
0234     spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags);
0235 }
0236 
0237 irqreturn_t lan966x_ptp_irq_handler(int irq, void *args)
0238 {
0239     int budget = LAN966X_MAX_PTP_ID;
0240     struct lan966x *lan966x = args;
0241 
0242     while (budget--) {
0243         struct sk_buff *skb, *skb_tmp, *skb_match = NULL;
0244         struct skb_shared_hwtstamps shhwtstamps;
0245         struct lan966x_port *port;
0246         struct timespec64 ts;
0247         unsigned long flags;
0248         u32 val, id, txport;
0249         u32 delay;
0250 
0251         val = lan_rd(lan966x, PTP_TWOSTEP_CTRL);
0252 
0253         /* Check if a timestamp can be retrieved */
0254         if (!(val & PTP_TWOSTEP_CTRL_VLD))
0255             break;
0256 
0257         WARN_ON(val & PTP_TWOSTEP_CTRL_OVFL);
0258 
0259         if (!(val & PTP_TWOSTEP_CTRL_STAMP_TX))
0260             continue;
0261 
0262         /* Retrieve the ts Tx port */
0263         txport = PTP_TWOSTEP_CTRL_STAMP_PORT_GET(val);
0264 
0265         /* Retrieve its associated skb */
0266         port = lan966x->ports[txport];
0267 
0268         /* Retrieve the delay */
0269         delay = lan_rd(lan966x, PTP_TWOSTEP_STAMP);
0270         delay = PTP_TWOSTEP_STAMP_STAMP_NSEC_GET(delay);
0271 
0272         /* Get next timestamp from fifo, which needs to be the
0273          * rx timestamp which represents the id of the frame
0274          */
0275         lan_rmw(PTP_TWOSTEP_CTRL_NXT_SET(1),
0276             PTP_TWOSTEP_CTRL_NXT,
0277             lan966x, PTP_TWOSTEP_CTRL);
0278 
0279         val = lan_rd(lan966x, PTP_TWOSTEP_CTRL);
0280 
0281         /* Check if a timestamp can be retried */
0282         if (!(val & PTP_TWOSTEP_CTRL_VLD))
0283             break;
0284 
0285         /* Read RX timestamping to get the ID */
0286         id = lan_rd(lan966x, PTP_TWOSTEP_STAMP);
0287 
0288         spin_lock_irqsave(&port->tx_skbs.lock, flags);
0289         skb_queue_walk_safe(&port->tx_skbs, skb, skb_tmp) {
0290             if (LAN966X_SKB_CB(skb)->ts_id != id)
0291                 continue;
0292 
0293             __skb_unlink(skb, &port->tx_skbs);
0294             skb_match = skb;
0295             break;
0296         }
0297         spin_unlock_irqrestore(&port->tx_skbs.lock, flags);
0298 
0299         /* Next ts */
0300         lan_rmw(PTP_TWOSTEP_CTRL_NXT_SET(1),
0301             PTP_TWOSTEP_CTRL_NXT,
0302             lan966x, PTP_TWOSTEP_CTRL);
0303 
0304         if (WARN_ON(!skb_match))
0305             continue;
0306 
0307         spin_lock(&lan966x->ptp_ts_id_lock);
0308         lan966x->ptp_skbs--;
0309         spin_unlock(&lan966x->ptp_ts_id_lock);
0310 
0311         /* Get the h/w timestamp */
0312         lan966x_get_hwtimestamp(lan966x, &ts, delay);
0313 
0314         /* Set the timestamp into the skb */
0315         shhwtstamps.hwtstamp = ktime_set(ts.tv_sec, ts.tv_nsec);
0316         skb_tstamp_tx(skb_match, &shhwtstamps);
0317 
0318         dev_kfree_skb_any(skb_match);
0319     }
0320 
0321     return IRQ_HANDLED;
0322 }
0323 
0324 irqreturn_t lan966x_ptp_ext_irq_handler(int irq, void *args)
0325 {
0326     struct lan966x *lan966x = args;
0327     struct lan966x_phc *phc;
0328     unsigned long flags;
0329     u64 time = 0;
0330     time64_t s;
0331     int pin, i;
0332     s64 ns;
0333 
0334     if (!(lan_rd(lan966x, PTP_PIN_INTR)))
0335         return IRQ_NONE;
0336 
0337     /* Go through all domains and see which pin generated the interrupt */
0338     for (i = 0; i < LAN966X_PHC_COUNT; ++i) {
0339         struct ptp_clock_event ptp_event = {0};
0340 
0341         phc = &lan966x->phc[i];
0342         pin = ptp_find_pin_unlocked(phc->clock, PTP_PF_EXTTS, 0);
0343         if (pin == -1)
0344             continue;
0345 
0346         if (!(lan_rd(lan966x, PTP_PIN_INTR) & BIT(pin)))
0347             continue;
0348 
0349         spin_lock_irqsave(&lan966x->ptp_clock_lock, flags);
0350 
0351         /* Enable to get the new interrupt.
0352          * By writing 1 it clears the bit
0353          */
0354         lan_wr(BIT(pin), lan966x, PTP_PIN_INTR);
0355 
0356         /* Get current time */
0357         s = lan_rd(lan966x, PTP_TOD_SEC_MSB(pin));
0358         s <<= 32;
0359         s |= lan_rd(lan966x, PTP_TOD_SEC_LSB(pin));
0360         ns = lan_rd(lan966x, PTP_TOD_NSEC(pin));
0361         ns &= PTP_TOD_NSEC_TOD_NSEC;
0362 
0363         spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags);
0364 
0365         if ((ns & 0xFFFFFFF0) == 0x3FFFFFF0) {
0366             s--;
0367             ns &= 0xf;
0368             ns += 999999984;
0369         }
0370         time = ktime_set(s, ns);
0371 
0372         ptp_event.index = pin;
0373         ptp_event.timestamp = time;
0374         ptp_event.type = PTP_CLOCK_EXTTS;
0375         ptp_clock_event(phc->clock, &ptp_event);
0376     }
0377 
0378     return IRQ_HANDLED;
0379 }
0380 
0381 static int lan966x_ptp_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)
0382 {
0383     struct lan966x_phc *phc = container_of(ptp, struct lan966x_phc, info);
0384     struct lan966x *lan966x = phc->lan966x;
0385     unsigned long flags;
0386     bool neg_adj = 0;
0387     u64 tod_inc;
0388     u64 ref;
0389 
0390     if (!scaled_ppm)
0391         return 0;
0392 
0393     if (scaled_ppm < 0) {
0394         neg_adj = 1;
0395         scaled_ppm = -scaled_ppm;
0396     }
0397 
0398     tod_inc = lan966x_ptp_get_nominal_value();
0399 
0400     /* The multiplication is split in 2 separate additions because of
0401      * overflow issues. If scaled_ppm with 16bit fractional part was bigger
0402      * than 20ppm then we got overflow.
0403      */
0404     ref = LAN966X_1PPM_FORMAT * (scaled_ppm >> 16);
0405     ref += (LAN966X_1PPM_FORMAT * (0xffff & scaled_ppm)) >> 16;
0406     tod_inc = neg_adj ? tod_inc - ref : tod_inc + ref;
0407 
0408     spin_lock_irqsave(&lan966x->ptp_clock_lock, flags);
0409 
0410     lan_rmw(PTP_DOM_CFG_CLKCFG_DIS_SET(1 << BIT(phc->index)),
0411         PTP_DOM_CFG_CLKCFG_DIS,
0412         lan966x, PTP_DOM_CFG);
0413 
0414     lan_wr((u32)tod_inc & 0xFFFFFFFF, lan966x,
0415            PTP_CLK_PER_CFG(phc->index, 0));
0416     lan_wr((u32)(tod_inc >> 32), lan966x,
0417            PTP_CLK_PER_CFG(phc->index, 1));
0418 
0419     lan_rmw(PTP_DOM_CFG_CLKCFG_DIS_SET(0),
0420         PTP_DOM_CFG_CLKCFG_DIS,
0421         lan966x, PTP_DOM_CFG);
0422 
0423     spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags);
0424 
0425     return 0;
0426 }
0427 
0428 static int lan966x_ptp_settime64(struct ptp_clock_info *ptp,
0429                  const struct timespec64 *ts)
0430 {
0431     struct lan966x_phc *phc = container_of(ptp, struct lan966x_phc, info);
0432     struct lan966x *lan966x = phc->lan966x;
0433     unsigned long flags;
0434 
0435     spin_lock_irqsave(&lan966x->ptp_clock_lock, flags);
0436 
0437     /* Must be in IDLE mode before the time can be loaded */
0438     lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_IDLE) |
0439         PTP_PIN_CFG_PIN_DOM_SET(phc->index) |
0440         PTP_PIN_CFG_PIN_SYNC_SET(0),
0441         PTP_PIN_CFG_PIN_ACTION |
0442         PTP_PIN_CFG_PIN_DOM |
0443         PTP_PIN_CFG_PIN_SYNC,
0444         lan966x, PTP_PIN_CFG(TOD_ACC_PIN));
0445 
0446     /* Set new value */
0447     lan_wr(PTP_TOD_SEC_MSB_TOD_SEC_MSB_SET(upper_32_bits(ts->tv_sec)),
0448            lan966x, PTP_TOD_SEC_MSB(TOD_ACC_PIN));
0449     lan_wr(lower_32_bits(ts->tv_sec),
0450            lan966x, PTP_TOD_SEC_LSB(TOD_ACC_PIN));
0451     lan_wr(ts->tv_nsec, lan966x, PTP_TOD_NSEC(TOD_ACC_PIN));
0452 
0453     /* Apply new values */
0454     lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_LOAD) |
0455         PTP_PIN_CFG_PIN_DOM_SET(phc->index) |
0456         PTP_PIN_CFG_PIN_SYNC_SET(0),
0457         PTP_PIN_CFG_PIN_ACTION |
0458         PTP_PIN_CFG_PIN_DOM |
0459         PTP_PIN_CFG_PIN_SYNC,
0460         lan966x, PTP_PIN_CFG(TOD_ACC_PIN));
0461 
0462     spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags);
0463 
0464     return 0;
0465 }
0466 
0467 static int lan966x_ptp_gettime64(struct ptp_clock_info *ptp,
0468                  struct timespec64 *ts)
0469 {
0470     struct lan966x_phc *phc = container_of(ptp, struct lan966x_phc, info);
0471     struct lan966x *lan966x = phc->lan966x;
0472     unsigned long flags;
0473     time64_t s;
0474     s64 ns;
0475 
0476     spin_lock_irqsave(&lan966x->ptp_clock_lock, flags);
0477 
0478     lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_SAVE) |
0479         PTP_PIN_CFG_PIN_DOM_SET(phc->index) |
0480         PTP_PIN_CFG_PIN_SYNC_SET(0),
0481         PTP_PIN_CFG_PIN_ACTION |
0482         PTP_PIN_CFG_PIN_DOM |
0483         PTP_PIN_CFG_PIN_SYNC,
0484         lan966x, PTP_PIN_CFG(TOD_ACC_PIN));
0485 
0486     s = lan_rd(lan966x, PTP_TOD_SEC_MSB(TOD_ACC_PIN));
0487     s <<= 32;
0488     s |= lan_rd(lan966x, PTP_TOD_SEC_LSB(TOD_ACC_PIN));
0489     ns = lan_rd(lan966x, PTP_TOD_NSEC(TOD_ACC_PIN));
0490     ns &= PTP_TOD_NSEC_TOD_NSEC;
0491 
0492     spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags);
0493 
0494     /* Deal with negative values */
0495     if ((ns & 0xFFFFFFF0) == 0x3FFFFFF0) {
0496         s--;
0497         ns &= 0xf;
0498         ns += 999999984;
0499     }
0500 
0501     set_normalized_timespec64(ts, s, ns);
0502     return 0;
0503 }
0504 
0505 static int lan966x_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
0506 {
0507     struct lan966x_phc *phc = container_of(ptp, struct lan966x_phc, info);
0508     struct lan966x *lan966x = phc->lan966x;
0509 
0510     if (delta > -(NSEC_PER_SEC / 2) && delta < (NSEC_PER_SEC / 2)) {
0511         unsigned long flags;
0512 
0513         spin_lock_irqsave(&lan966x->ptp_clock_lock, flags);
0514 
0515         /* Must be in IDLE mode before the time can be loaded */
0516         lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_IDLE) |
0517             PTP_PIN_CFG_PIN_DOM_SET(phc->index) |
0518             PTP_PIN_CFG_PIN_SYNC_SET(0),
0519             PTP_PIN_CFG_PIN_ACTION |
0520             PTP_PIN_CFG_PIN_DOM |
0521             PTP_PIN_CFG_PIN_SYNC,
0522             lan966x, PTP_PIN_CFG(TOD_ACC_PIN));
0523 
0524         lan_wr(PTP_TOD_NSEC_TOD_NSEC_SET(delta),
0525                lan966x, PTP_TOD_NSEC(TOD_ACC_PIN));
0526 
0527         /* Adjust time with the value of PTP_TOD_NSEC */
0528         lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_DELTA) |
0529             PTP_PIN_CFG_PIN_DOM_SET(phc->index) |
0530             PTP_PIN_CFG_PIN_SYNC_SET(0),
0531             PTP_PIN_CFG_PIN_ACTION |
0532             PTP_PIN_CFG_PIN_DOM |
0533             PTP_PIN_CFG_PIN_SYNC,
0534             lan966x, PTP_PIN_CFG(TOD_ACC_PIN));
0535 
0536         spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags);
0537     } else {
0538         /* Fall back using lan966x_ptp_settime64 which is not exact */
0539         struct timespec64 ts;
0540         u64 now;
0541 
0542         lan966x_ptp_gettime64(ptp, &ts);
0543 
0544         now = ktime_to_ns(timespec64_to_ktime(ts));
0545         ts = ns_to_timespec64(now + delta);
0546 
0547         lan966x_ptp_settime64(ptp, &ts);
0548     }
0549 
0550     return 0;
0551 }
0552 
0553 static int lan966x_ptp_verify(struct ptp_clock_info *ptp, unsigned int pin,
0554                   enum ptp_pin_function func, unsigned int chan)
0555 {
0556     struct lan966x_phc *phc = container_of(ptp, struct lan966x_phc, info);
0557     struct lan966x *lan966x = phc->lan966x;
0558     struct ptp_clock_info *info;
0559     int i;
0560 
0561     /* Currently support only 1 channel */
0562     if (chan != 0)
0563         return -1;
0564 
0565     switch (func) {
0566     case PTP_PF_NONE:
0567     case PTP_PF_PEROUT:
0568     case PTP_PF_EXTTS:
0569         break;
0570     default:
0571         return -1;
0572     }
0573 
0574     /* The PTP pins are shared by all the PHC. So it is required to see if
0575      * the pin is connected to another PHC. The pin is connected to another
0576      * PHC if that pin already has a function on that PHC.
0577      */
0578     for (i = 0; i < LAN966X_PHC_COUNT; ++i) {
0579         info = &lan966x->phc[i].info;
0580 
0581         /* Ignore the check with ourself */
0582         if (ptp == info)
0583             continue;
0584 
0585         if (info->pin_config[pin].func == PTP_PF_PEROUT ||
0586             info->pin_config[pin].func == PTP_PF_EXTTS)
0587             return -1;
0588     }
0589 
0590     return 0;
0591 }
0592 
0593 static int lan966x_ptp_perout(struct ptp_clock_info *ptp,
0594                   struct ptp_clock_request *rq, int on)
0595 {
0596     struct lan966x_phc *phc = container_of(ptp, struct lan966x_phc, info);
0597     struct lan966x *lan966x = phc->lan966x;
0598     struct timespec64 ts_phase, ts_period;
0599     unsigned long flags;
0600     s64 wf_high, wf_low;
0601     bool pps = false;
0602     int pin;
0603 
0604     if (rq->perout.flags & ~(PTP_PEROUT_DUTY_CYCLE |
0605                  PTP_PEROUT_PHASE))
0606         return -EOPNOTSUPP;
0607 
0608     pin = ptp_find_pin(phc->clock, PTP_PF_PEROUT, rq->perout.index);
0609     if (pin == -1 || pin >= LAN966X_PHC_PINS_NUM)
0610         return -EINVAL;
0611 
0612     if (!on) {
0613         spin_lock_irqsave(&lan966x->ptp_clock_lock, flags);
0614         lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_IDLE) |
0615             PTP_PIN_CFG_PIN_DOM_SET(phc->index) |
0616             PTP_PIN_CFG_PIN_SYNC_SET(0),
0617             PTP_PIN_CFG_PIN_ACTION |
0618             PTP_PIN_CFG_PIN_DOM |
0619             PTP_PIN_CFG_PIN_SYNC,
0620             lan966x, PTP_PIN_CFG(pin));
0621         spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags);
0622         return 0;
0623     }
0624 
0625     if (rq->perout.period.sec == 1 &&
0626         rq->perout.period.nsec == 0)
0627         pps = true;
0628 
0629     if (rq->perout.flags & PTP_PEROUT_PHASE) {
0630         ts_phase.tv_sec = rq->perout.phase.sec;
0631         ts_phase.tv_nsec = rq->perout.phase.nsec;
0632     } else {
0633         ts_phase.tv_sec = rq->perout.start.sec;
0634         ts_phase.tv_nsec = rq->perout.start.nsec;
0635     }
0636 
0637     if (ts_phase.tv_sec || (ts_phase.tv_nsec && !pps)) {
0638         dev_warn(lan966x->dev,
0639              "Absolute time not supported!\n");
0640         return -EINVAL;
0641     }
0642 
0643     if (rq->perout.flags & PTP_PEROUT_DUTY_CYCLE) {
0644         struct timespec64 ts_on;
0645 
0646         ts_on.tv_sec = rq->perout.on.sec;
0647         ts_on.tv_nsec = rq->perout.on.nsec;
0648 
0649         wf_high = timespec64_to_ns(&ts_on);
0650     } else {
0651         wf_high = 5000;
0652     }
0653 
0654     if (pps) {
0655         spin_lock_irqsave(&lan966x->ptp_clock_lock, flags);
0656         lan_wr(PTP_WF_LOW_PERIOD_PIN_WFL(ts_phase.tv_nsec),
0657                lan966x, PTP_WF_LOW_PERIOD(pin));
0658         lan_wr(PTP_WF_HIGH_PERIOD_PIN_WFH(wf_high),
0659                lan966x, PTP_WF_HIGH_PERIOD(pin));
0660         lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_CLOCK) |
0661             PTP_PIN_CFG_PIN_DOM_SET(phc->index) |
0662             PTP_PIN_CFG_PIN_SYNC_SET(3),
0663             PTP_PIN_CFG_PIN_ACTION |
0664             PTP_PIN_CFG_PIN_DOM |
0665             PTP_PIN_CFG_PIN_SYNC,
0666             lan966x, PTP_PIN_CFG(pin));
0667         spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags);
0668         return 0;
0669     }
0670 
0671     ts_period.tv_sec = rq->perout.period.sec;
0672     ts_period.tv_nsec = rq->perout.period.nsec;
0673 
0674     wf_low = timespec64_to_ns(&ts_period);
0675     wf_low -= wf_high;
0676 
0677     spin_lock_irqsave(&lan966x->ptp_clock_lock, flags);
0678     lan_wr(PTP_WF_LOW_PERIOD_PIN_WFL(wf_low),
0679            lan966x, PTP_WF_LOW_PERIOD(pin));
0680     lan_wr(PTP_WF_HIGH_PERIOD_PIN_WFH(wf_high),
0681            lan966x, PTP_WF_HIGH_PERIOD(pin));
0682     lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_CLOCK) |
0683         PTP_PIN_CFG_PIN_DOM_SET(phc->index) |
0684         PTP_PIN_CFG_PIN_SYNC_SET(0),
0685         PTP_PIN_CFG_PIN_ACTION |
0686         PTP_PIN_CFG_PIN_DOM |
0687         PTP_PIN_CFG_PIN_SYNC,
0688         lan966x, PTP_PIN_CFG(pin));
0689     spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags);
0690 
0691     return 0;
0692 }
0693 
0694 static int lan966x_ptp_extts(struct ptp_clock_info *ptp,
0695                  struct ptp_clock_request *rq, int on)
0696 {
0697     struct lan966x_phc *phc = container_of(ptp, struct lan966x_phc, info);
0698     struct lan966x *lan966x = phc->lan966x;
0699     unsigned long flags;
0700     int pin;
0701     u32 val;
0702 
0703     if (lan966x->ptp_ext_irq <= 0)
0704         return -EOPNOTSUPP;
0705 
0706     /* Reject requests with unsupported flags */
0707     if (rq->extts.flags & ~(PTP_ENABLE_FEATURE |
0708                 PTP_RISING_EDGE |
0709                 PTP_STRICT_FLAGS))
0710         return -EOPNOTSUPP;
0711 
0712     pin = ptp_find_pin(phc->clock, PTP_PF_EXTTS, rq->extts.index);
0713     if (pin == -1 || pin >= LAN966X_PHC_PINS_NUM)
0714         return -EINVAL;
0715 
0716     spin_lock_irqsave(&lan966x->ptp_clock_lock, flags);
0717     lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_SAVE) |
0718         PTP_PIN_CFG_PIN_SYNC_SET(on ? 3 : 0) |
0719         PTP_PIN_CFG_PIN_DOM_SET(phc->index) |
0720         PTP_PIN_CFG_PIN_SELECT_SET(pin),
0721         PTP_PIN_CFG_PIN_ACTION |
0722         PTP_PIN_CFG_PIN_SYNC |
0723         PTP_PIN_CFG_PIN_DOM |
0724         PTP_PIN_CFG_PIN_SELECT,
0725         lan966x, PTP_PIN_CFG(pin));
0726 
0727     val = lan_rd(lan966x, PTP_PIN_INTR_ENA);
0728     if (on)
0729         val |= BIT(pin);
0730     else
0731         val &= ~BIT(pin);
0732     lan_wr(val, lan966x, PTP_PIN_INTR_ENA);
0733 
0734     spin_unlock_irqrestore(&lan966x->ptp_clock_lock, flags);
0735 
0736     return 0;
0737 }
0738 
0739 static int lan966x_ptp_enable(struct ptp_clock_info *ptp,
0740                   struct ptp_clock_request *rq, int on)
0741 {
0742     switch (rq->type) {
0743     case PTP_CLK_REQ_PEROUT:
0744         return lan966x_ptp_perout(ptp, rq, on);
0745     case PTP_CLK_REQ_EXTTS:
0746         return lan966x_ptp_extts(ptp, rq, on);
0747     default:
0748         return -EOPNOTSUPP;
0749     }
0750 
0751     return 0;
0752 }
0753 
0754 static struct ptp_clock_info lan966x_ptp_clock_info = {
0755     .owner      = THIS_MODULE,
0756     .name       = "lan966x ptp",
0757     .max_adj    = 200000,
0758     .gettime64  = lan966x_ptp_gettime64,
0759     .settime64  = lan966x_ptp_settime64,
0760     .adjtime    = lan966x_ptp_adjtime,
0761     .adjfine    = lan966x_ptp_adjfine,
0762     .verify     = lan966x_ptp_verify,
0763     .enable     = lan966x_ptp_enable,
0764     .n_per_out  = LAN966X_PHC_PINS_NUM,
0765     .n_ext_ts   = LAN966X_PHC_PINS_NUM,
0766     .n_pins     = LAN966X_PHC_PINS_NUM,
0767 };
0768 
0769 static int lan966x_ptp_phc_init(struct lan966x *lan966x,
0770                 int index,
0771                 struct ptp_clock_info *clock_info)
0772 {
0773     struct lan966x_phc *phc = &lan966x->phc[index];
0774     struct ptp_pin_desc *p;
0775     int i;
0776 
0777     for (i = 0; i < LAN966X_PHC_PINS_NUM; i++) {
0778         p = &phc->pins[i];
0779 
0780         snprintf(p->name, sizeof(p->name), "pin%d", i);
0781         p->index = i;
0782         p->func = PTP_PF_NONE;
0783     }
0784 
0785     phc->info = *clock_info;
0786     phc->info.pin_config = &phc->pins[0];
0787     phc->clock = ptp_clock_register(&phc->info, lan966x->dev);
0788     if (IS_ERR(phc->clock))
0789         return PTR_ERR(phc->clock);
0790 
0791     phc->index = index;
0792     phc->lan966x = lan966x;
0793 
0794     /* PTP Rx stamping is always enabled.  */
0795     phc->hwtstamp_config.rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
0796 
0797     return 0;
0798 }
0799 
0800 int lan966x_ptp_init(struct lan966x *lan966x)
0801 {
0802     u64 tod_adj = lan966x_ptp_get_nominal_value();
0803     struct lan966x_port *port;
0804     int err, i;
0805 
0806     if (!lan966x->ptp)
0807         return 0;
0808 
0809     for (i = 0; i < LAN966X_PHC_COUNT; ++i) {
0810         err = lan966x_ptp_phc_init(lan966x, i, &lan966x_ptp_clock_info);
0811         if (err)
0812             return err;
0813     }
0814 
0815     spin_lock_init(&lan966x->ptp_clock_lock);
0816     spin_lock_init(&lan966x->ptp_ts_id_lock);
0817     mutex_init(&lan966x->ptp_lock);
0818 
0819     /* Disable master counters */
0820     lan_wr(PTP_DOM_CFG_ENA_SET(0), lan966x, PTP_DOM_CFG);
0821 
0822     /* Configure the nominal TOD increment per clock cycle */
0823     lan_rmw(PTP_DOM_CFG_CLKCFG_DIS_SET(0x7),
0824         PTP_DOM_CFG_CLKCFG_DIS,
0825         lan966x, PTP_DOM_CFG);
0826 
0827     for (i = 0; i < LAN966X_PHC_COUNT; ++i) {
0828         lan_wr((u32)tod_adj & 0xFFFFFFFF, lan966x,
0829                PTP_CLK_PER_CFG(i, 0));
0830         lan_wr((u32)(tod_adj >> 32), lan966x,
0831                PTP_CLK_PER_CFG(i, 1));
0832     }
0833 
0834     lan_rmw(PTP_DOM_CFG_CLKCFG_DIS_SET(0),
0835         PTP_DOM_CFG_CLKCFG_DIS,
0836         lan966x, PTP_DOM_CFG);
0837 
0838     /* Enable master counters */
0839     lan_wr(PTP_DOM_CFG_ENA_SET(0x7), lan966x, PTP_DOM_CFG);
0840 
0841     for (i = 0; i < lan966x->num_phys_ports; i++) {
0842         port = lan966x->ports[i];
0843         if (!port)
0844             continue;
0845 
0846         skb_queue_head_init(&port->tx_skbs);
0847     }
0848 
0849     return 0;
0850 }
0851 
0852 void lan966x_ptp_deinit(struct lan966x *lan966x)
0853 {
0854     struct lan966x_port *port;
0855     int i;
0856 
0857     for (i = 0; i < lan966x->num_phys_ports; i++) {
0858         port = lan966x->ports[i];
0859         if (!port)
0860             continue;
0861 
0862         skb_queue_purge(&port->tx_skbs);
0863     }
0864 
0865     for (i = 0; i < LAN966X_PHC_COUNT; ++i)
0866         ptp_clock_unregister(lan966x->phc[i].clock);
0867 }
0868 
0869 void lan966x_ptp_rxtstamp(struct lan966x *lan966x, struct sk_buff *skb,
0870               u64 timestamp)
0871 {
0872     struct skb_shared_hwtstamps *shhwtstamps;
0873     struct lan966x_phc *phc;
0874     struct timespec64 ts;
0875     u64 full_ts_in_ns;
0876 
0877     if (!lan966x->ptp)
0878         return;
0879 
0880     phc = &lan966x->phc[LAN966X_PHC_PORT];
0881     lan966x_ptp_gettime64(&phc->info, &ts);
0882 
0883     /* Drop the sub-ns precision */
0884     timestamp = timestamp >> 2;
0885     if (ts.tv_nsec < timestamp)
0886         ts.tv_sec--;
0887     ts.tv_nsec = timestamp;
0888     full_ts_in_ns = ktime_set(ts.tv_sec, ts.tv_nsec);
0889 
0890     shhwtstamps = skb_hwtstamps(skb);
0891     shhwtstamps->hwtstamp = full_ts_in_ns;
0892 }
[ Source navigation ] [ Diff markup ] [ Identifier search ] [ general search ]

This page was automatically generated by the 2.1.0 LXR engine.

The LXR team
Valid CSS 2.1! Valid HTML 4.01!