Back to home page

OSCL-LXR
 
 

     

0001 // SPDX-License-Identifier: GPL-2.0-only
0002 /*
0003  * 1588 PTP support for Cadence GEM device.
0004  *
0005  * Copyright (C) 2017 Cadence Design Systems - https://www.cadence.com
0006  *
0007  * Authors: Rafal Ozieblo <rafalo@cadence.com>
0008  *          Bartosz Folta <bfolta@cadence.com>
0009  */
0010 #include <linux/kernel.h>
0011 #include <linux/types.h>
0012 #include <linux/clk.h>
0013 #include <linux/device.h>
0014 #include <linux/etherdevice.h>
0015 #include <linux/platform_device.h>
0016 #include <linux/time64.h>
0017 #include <linux/ptp_classify.h>
0018 #include <linux/if_ether.h>
0019 #include <linux/if_vlan.h>
0020 #include <linux/net_tstamp.h>
0021 #include <linux/circ_buf.h>
0022 #include <linux/spinlock.h>
0023 
0024 #include "macb.h"
0025 
0026 #define  GEM_PTP_TIMER_NAME "gem-ptp-timer"
0027 
0028 static struct macb_dma_desc_ptp *macb_ptp_desc(struct macb *bp,
0029                            struct macb_dma_desc *desc)
0030 {
0031     if (bp->hw_dma_cap == HW_DMA_CAP_PTP)
0032         return (struct macb_dma_desc_ptp *)
0033                 ((u8 *)desc + sizeof(struct macb_dma_desc));
0034     if (bp->hw_dma_cap == HW_DMA_CAP_64B_PTP)
0035         return (struct macb_dma_desc_ptp *)
0036                 ((u8 *)desc + sizeof(struct macb_dma_desc)
0037                 + sizeof(struct macb_dma_desc_64));
0038     return NULL;
0039 }
0040 
0041 static int gem_tsu_get_time(struct ptp_clock_info *ptp, struct timespec64 *ts,
0042                 struct ptp_system_timestamp *sts)
0043 {
0044     struct macb *bp = container_of(ptp, struct macb, ptp_clock_info);
0045     unsigned long flags;
0046     long first, second;
0047     u32 secl, sech;
0048 
0049     spin_lock_irqsave(&bp->tsu_clk_lock, flags);
0050     ptp_read_system_prets(sts);
0051     first = gem_readl(bp, TN);
0052     ptp_read_system_postts(sts);
0053     secl = gem_readl(bp, TSL);
0054     sech = gem_readl(bp, TSH);
0055     second = gem_readl(bp, TN);
0056 
0057     /* test for nsec rollover */
0058     if (first > second) {
0059         /* if so, use later read & re-read seconds
0060          * (assume all done within 1s)
0061          */
0062         ptp_read_system_prets(sts);
0063         ts->tv_nsec = gem_readl(bp, TN);
0064         ptp_read_system_postts(sts);
0065         secl = gem_readl(bp, TSL);
0066         sech = gem_readl(bp, TSH);
0067     } else {
0068         ts->tv_nsec = first;
0069     }
0070 
0071     spin_unlock_irqrestore(&bp->tsu_clk_lock, flags);
0072     ts->tv_sec = (((u64)sech << GEM_TSL_SIZE) | secl)
0073             & TSU_SEC_MAX_VAL;
0074     return 0;
0075 }
0076 
0077 static int gem_tsu_set_time(struct ptp_clock_info *ptp,
0078                 const struct timespec64 *ts)
0079 {
0080     struct macb *bp = container_of(ptp, struct macb, ptp_clock_info);
0081     unsigned long flags;
0082     u32 ns, sech, secl;
0083 
0084     secl = (u32)ts->tv_sec;
0085     sech = (ts->tv_sec >> GEM_TSL_SIZE) & ((1 << GEM_TSH_SIZE) - 1);
0086     ns = ts->tv_nsec;
0087 
0088     spin_lock_irqsave(&bp->tsu_clk_lock, flags);
0089 
0090     /* TSH doesn't latch the time and no atomicity! */
0091     gem_writel(bp, TN, 0); /* clear to avoid overflow */
0092     gem_writel(bp, TSH, sech);
0093     /* write lower bits 2nd, for synchronized secs update */
0094     gem_writel(bp, TSL, secl);
0095     gem_writel(bp, TN, ns);
0096 
0097     spin_unlock_irqrestore(&bp->tsu_clk_lock, flags);
0098 
0099     return 0;
0100 }
0101 
0102 static int gem_tsu_incr_set(struct macb *bp, struct tsu_incr *incr_spec)
0103 {
0104     unsigned long flags;
0105 
0106     /* tsu_timer_incr register must be written after
0107      * the tsu_timer_incr_sub_ns register and the write operation
0108      * will cause the value written to the tsu_timer_incr_sub_ns register
0109      * to take effect.
0110      */
0111     spin_lock_irqsave(&bp->tsu_clk_lock, flags);
0112     /* RegBit[15:0] = Subns[23:8]; RegBit[31:24] = Subns[7:0] */
0113     gem_writel(bp, TISUBN, GEM_BF(SUBNSINCRL, incr_spec->sub_ns) |
0114            GEM_BF(SUBNSINCRH, (incr_spec->sub_ns >>
0115               GEM_SUBNSINCRL_SIZE)));
0116     gem_writel(bp, TI, GEM_BF(NSINCR, incr_spec->ns));
0117     spin_unlock_irqrestore(&bp->tsu_clk_lock, flags);
0118 
0119     return 0;
0120 }
0121 
0122 static int gem_ptp_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)
0123 {
0124     struct macb *bp = container_of(ptp, struct macb, ptp_clock_info);
0125     struct tsu_incr incr_spec;
0126     bool neg_adj = false;
0127     u32 word;
0128     u64 adj;
0129 
0130     if (scaled_ppm < 0) {
0131         neg_adj = true;
0132         scaled_ppm = -scaled_ppm;
0133     }
0134 
0135     /* Adjustment is relative to base frequency */
0136     incr_spec.sub_ns = bp->tsu_incr.sub_ns;
0137     incr_spec.ns = bp->tsu_incr.ns;
0138 
0139     /* scaling: unused(8bit) | ns(8bit) | fractions(16bit) */
0140     word = ((u64)incr_spec.ns << GEM_SUBNSINCR_SIZE) + incr_spec.sub_ns;
0141     adj = (u64)scaled_ppm * word;
0142     /* Divide with rounding, equivalent to floating dividing:
0143      * (temp / USEC_PER_SEC) + 0.5
0144      */
0145     adj += (USEC_PER_SEC >> 1);
0146     adj >>= PPM_FRACTION; /* remove fractions */
0147     adj = div_u64(adj, USEC_PER_SEC);
0148     adj = neg_adj ? (word - adj) : (word + adj);
0149 
0150     incr_spec.ns = (adj >> GEM_SUBNSINCR_SIZE)
0151             & ((1 << GEM_NSINCR_SIZE) - 1);
0152     incr_spec.sub_ns = adj & ((1 << GEM_SUBNSINCR_SIZE) - 1);
0153     gem_tsu_incr_set(bp, &incr_spec);
0154     return 0;
0155 }
0156 
0157 static int gem_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
0158 {
0159     struct macb *bp = container_of(ptp, struct macb, ptp_clock_info);
0160     struct timespec64 now, then = ns_to_timespec64(delta);
0161     u32 adj, sign = 0;
0162 
0163     if (delta < 0) {
0164         sign = 1;
0165         delta = -delta;
0166     }
0167 
0168     if (delta > TSU_NSEC_MAX_VAL) {
0169         gem_tsu_get_time(&bp->ptp_clock_info, &now, NULL);
0170         now = timespec64_add(now, then);
0171 
0172         gem_tsu_set_time(&bp->ptp_clock_info,
0173                  (const struct timespec64 *)&now);
0174     } else {
0175         adj = (sign << GEM_ADDSUB_OFFSET) | delta;
0176 
0177         gem_writel(bp, TA, adj);
0178     }
0179 
0180     return 0;
0181 }
0182 
0183 static int gem_ptp_enable(struct ptp_clock_info *ptp,
0184               struct ptp_clock_request *rq, int on)
0185 {
0186     return -EOPNOTSUPP;
0187 }
0188 
0189 static const struct ptp_clock_info gem_ptp_caps_template = {
0190     .owner      = THIS_MODULE,
0191     .name       = GEM_PTP_TIMER_NAME,
0192     .max_adj    = 0,
0193     .n_alarm    = 0,
0194     .n_ext_ts   = 0,
0195     .n_per_out  = 0,
0196     .n_pins     = 0,
0197     .pps        = 1,
0198     .adjfine    = gem_ptp_adjfine,
0199     .adjtime    = gem_ptp_adjtime,
0200     .gettimex64 = gem_tsu_get_time,
0201     .settime64  = gem_tsu_set_time,
0202     .enable     = gem_ptp_enable,
0203 };
0204 
0205 static void gem_ptp_init_timer(struct macb *bp)
0206 {
0207     u32 rem = 0;
0208     u64 adj;
0209 
0210     bp->tsu_incr.ns = div_u64_rem(NSEC_PER_SEC, bp->tsu_rate, &rem);
0211     if (rem) {
0212         adj = rem;
0213         adj <<= GEM_SUBNSINCR_SIZE;
0214         bp->tsu_incr.sub_ns = div_u64(adj, bp->tsu_rate);
0215     } else {
0216         bp->tsu_incr.sub_ns = 0;
0217     }
0218 }
0219 
0220 static void gem_ptp_init_tsu(struct macb *bp)
0221 {
0222     struct timespec64 ts;
0223 
0224     /* 1. get current system time */
0225     ts = ns_to_timespec64(ktime_to_ns(ktime_get_real()));
0226 
0227     /* 2. set ptp timer */
0228     gem_tsu_set_time(&bp->ptp_clock_info, &ts);
0229 
0230     /* 3. set PTP timer increment value to BASE_INCREMENT */
0231     gem_tsu_incr_set(bp, &bp->tsu_incr);
0232 
0233     gem_writel(bp, TA, 0);
0234 }
0235 
0236 static void gem_ptp_clear_timer(struct macb *bp)
0237 {
0238     bp->tsu_incr.sub_ns = 0;
0239     bp->tsu_incr.ns = 0;
0240 
0241     gem_writel(bp, TISUBN, GEM_BF(SUBNSINCR, 0));
0242     gem_writel(bp, TI, GEM_BF(NSINCR, 0));
0243     gem_writel(bp, TA, 0);
0244 }
0245 
0246 static int gem_hw_timestamp(struct macb *bp, u32 dma_desc_ts_1,
0247                 u32 dma_desc_ts_2, struct timespec64 *ts)
0248 {
0249     struct timespec64 tsu;
0250 
0251     ts->tv_sec = (GEM_BFEXT(DMA_SECH, dma_desc_ts_2) << GEM_DMA_SECL_SIZE) |
0252             GEM_BFEXT(DMA_SECL, dma_desc_ts_1);
0253     ts->tv_nsec = GEM_BFEXT(DMA_NSEC, dma_desc_ts_1);
0254 
0255     /* TSU overlapping workaround
0256      * The timestamp only contains lower few bits of seconds,
0257      * so add value from 1588 timer
0258      */
0259     gem_tsu_get_time(&bp->ptp_clock_info, &tsu, NULL);
0260 
0261     /* If the top bit is set in the timestamp,
0262      * but not in 1588 timer, it has rolled over,
0263      * so subtract max size
0264      */
0265     if ((ts->tv_sec & (GEM_DMA_SEC_TOP >> 1)) &&
0266         !(tsu.tv_sec & (GEM_DMA_SEC_TOP >> 1)))
0267         ts->tv_sec -= GEM_DMA_SEC_TOP;
0268 
0269     ts->tv_sec += ((~GEM_DMA_SEC_MASK) & tsu.tv_sec);
0270 
0271     return 0;
0272 }
0273 
0274 void gem_ptp_rxstamp(struct macb *bp, struct sk_buff *skb,
0275              struct macb_dma_desc *desc)
0276 {
0277     struct skb_shared_hwtstamps *shhwtstamps = skb_hwtstamps(skb);
0278     struct macb_dma_desc_ptp *desc_ptp;
0279     struct timespec64 ts;
0280 
0281     if (GEM_BFEXT(DMA_RXVALID, desc->addr)) {
0282         desc_ptp = macb_ptp_desc(bp, desc);
0283         /* Unlikely but check */
0284         if (!desc_ptp) {
0285             dev_warn_ratelimited(&bp->pdev->dev,
0286                          "Timestamp not supported in BD\n");
0287             return;
0288         }
0289         gem_hw_timestamp(bp, desc_ptp->ts_1, desc_ptp->ts_2, &ts);
0290         memset(shhwtstamps, 0, sizeof(struct skb_shared_hwtstamps));
0291         shhwtstamps->hwtstamp = ktime_set(ts.tv_sec, ts.tv_nsec);
0292     }
0293 }
0294 
0295 static void gem_tstamp_tx(struct macb *bp, struct sk_buff *skb,
0296               struct macb_dma_desc_ptp *desc_ptp)
0297 {
0298     struct skb_shared_hwtstamps shhwtstamps;
0299     struct timespec64 ts;
0300 
0301     gem_hw_timestamp(bp, desc_ptp->ts_1, desc_ptp->ts_2, &ts);
0302     memset(&shhwtstamps, 0, sizeof(shhwtstamps));
0303     shhwtstamps.hwtstamp = ktime_set(ts.tv_sec, ts.tv_nsec);
0304     skb_tstamp_tx(skb, &shhwtstamps);
0305 }
0306 
0307 int gem_ptp_txstamp(struct macb_queue *queue, struct sk_buff *skb,
0308             struct macb_dma_desc *desc)
0309 {
0310     unsigned long tail = READ_ONCE(queue->tx_ts_tail);
0311     unsigned long head = queue->tx_ts_head;
0312     struct macb_dma_desc_ptp *desc_ptp;
0313     struct gem_tx_ts *tx_timestamp;
0314 
0315     if (!GEM_BFEXT(DMA_TXVALID, desc->ctrl))
0316         return -EINVAL;
0317 
0318     if (CIRC_SPACE(head, tail, PTP_TS_BUFFER_SIZE) == 0)
0319         return -ENOMEM;
0320 
0321     desc_ptp = macb_ptp_desc(queue->bp, desc);
0322     /* Unlikely but check */
0323     if (!desc_ptp)
0324         return -EINVAL;
0325     skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
0326     tx_timestamp = &queue->tx_timestamps[head];
0327     tx_timestamp->skb = skb;
0328     /* ensure ts_1/ts_2 is loaded after ctrl (TX_USED check) */
0329     dma_rmb();
0330     tx_timestamp->desc_ptp.ts_1 = desc_ptp->ts_1;
0331     tx_timestamp->desc_ptp.ts_2 = desc_ptp->ts_2;
0332     /* move head */
0333     smp_store_release(&queue->tx_ts_head,
0334               (head + 1) & (PTP_TS_BUFFER_SIZE - 1));
0335 
0336     schedule_work(&queue->tx_ts_task);
0337     return 0;
0338 }
0339 
0340 static void gem_tx_timestamp_flush(struct work_struct *work)
0341 {
0342     struct macb_queue *queue =
0343             container_of(work, struct macb_queue, tx_ts_task);
0344     unsigned long head, tail;
0345     struct gem_tx_ts *tx_ts;
0346 
0347     /* take current head */
0348     head = smp_load_acquire(&queue->tx_ts_head);
0349     tail = queue->tx_ts_tail;
0350 
0351     while (CIRC_CNT(head, tail, PTP_TS_BUFFER_SIZE)) {
0352         tx_ts = &queue->tx_timestamps[tail];
0353         gem_tstamp_tx(queue->bp, tx_ts->skb, &tx_ts->desc_ptp);
0354         /* cleanup */
0355         dev_kfree_skb_any(tx_ts->skb);
0356         /* remove old tail */
0357         smp_store_release(&queue->tx_ts_tail,
0358                   (tail + 1) & (PTP_TS_BUFFER_SIZE - 1));
0359         tail = queue->tx_ts_tail;
0360     }
0361 }
0362 
0363 void gem_ptp_init(struct net_device *dev)
0364 {
0365     struct macb *bp = netdev_priv(dev);
0366     struct macb_queue *queue;
0367     unsigned int q;
0368 
0369     bp->ptp_clock_info = gem_ptp_caps_template;
0370 
0371     /* nominal frequency and maximum adjustment in ppb */
0372     bp->tsu_rate = bp->ptp_info->get_tsu_rate(bp);
0373     bp->ptp_clock_info.max_adj = bp->ptp_info->get_ptp_max_adj();
0374     gem_ptp_init_timer(bp);
0375     bp->ptp_clock = ptp_clock_register(&bp->ptp_clock_info, &dev->dev);
0376     if (IS_ERR(bp->ptp_clock)) {
0377         pr_err("ptp clock register failed: %ld\n",
0378             PTR_ERR(bp->ptp_clock));
0379         bp->ptp_clock = NULL;
0380         return;
0381     } else if (bp->ptp_clock == NULL) {
0382         pr_err("ptp clock register failed\n");
0383         return;
0384     }
0385 
0386     spin_lock_init(&bp->tsu_clk_lock);
0387     for (q = 0, queue = bp->queues; q < bp->num_queues; ++q, ++queue) {
0388         queue->tx_ts_head = 0;
0389         queue->tx_ts_tail = 0;
0390         INIT_WORK(&queue->tx_ts_task, gem_tx_timestamp_flush);
0391     }
0392 
0393     gem_ptp_init_tsu(bp);
0394 
0395     dev_info(&bp->pdev->dev, "%s ptp clock registered.\n",
0396          GEM_PTP_TIMER_NAME);
0397 }
0398 
0399 void gem_ptp_remove(struct net_device *ndev)
0400 {
0401     struct macb *bp = netdev_priv(ndev);
0402 
0403     if (bp->ptp_clock)
0404         ptp_clock_unregister(bp->ptp_clock);
0405 
0406     gem_ptp_clear_timer(bp);
0407 
0408     dev_info(&bp->pdev->dev, "%s ptp clock unregistered.\n",
0409          GEM_PTP_TIMER_NAME);
0410 }
0411 
0412 static int gem_ptp_set_ts_mode(struct macb *bp,
0413                    enum macb_bd_control tx_bd_control,
0414                    enum macb_bd_control rx_bd_control)
0415 {
0416     gem_writel(bp, TXBDCTRL, GEM_BF(TXTSMODE, tx_bd_control));
0417     gem_writel(bp, RXBDCTRL, GEM_BF(RXTSMODE, rx_bd_control));
0418 
0419     return 0;
0420 }
0421 
0422 int gem_get_hwtst(struct net_device *dev, struct ifreq *rq)
0423 {
0424     struct hwtstamp_config *tstamp_config;
0425     struct macb *bp = netdev_priv(dev);
0426 
0427     tstamp_config = &bp->tstamp_config;
0428     if ((bp->hw_dma_cap & HW_DMA_CAP_PTP) == 0)
0429         return -EOPNOTSUPP;
0430 
0431     if (copy_to_user(rq->ifr_data, tstamp_config, sizeof(*tstamp_config)))
0432         return -EFAULT;
0433     else
0434         return 0;
0435 }
0436 
0437 static void gem_ptp_set_one_step_sync(struct macb *bp, u8 enable)
0438 {
0439     u32 reg_val;
0440 
0441     reg_val = macb_readl(bp, NCR);
0442 
0443     if (enable)
0444         macb_writel(bp, NCR, reg_val | MACB_BIT(OSSMODE));
0445     else
0446         macb_writel(bp, NCR, reg_val & ~MACB_BIT(OSSMODE));
0447 }
0448 
0449 int gem_set_hwtst(struct net_device *dev, struct ifreq *ifr, int cmd)
0450 {
0451     enum macb_bd_control tx_bd_control = TSTAMP_DISABLED;
0452     enum macb_bd_control rx_bd_control = TSTAMP_DISABLED;
0453     struct hwtstamp_config *tstamp_config;
0454     struct macb *bp = netdev_priv(dev);
0455     u32 regval;
0456 
0457     tstamp_config = &bp->tstamp_config;
0458     if ((bp->hw_dma_cap & HW_DMA_CAP_PTP) == 0)
0459         return -EOPNOTSUPP;
0460 
0461     if (copy_from_user(tstamp_config, ifr->ifr_data,
0462                sizeof(*tstamp_config)))
0463         return -EFAULT;
0464 
0465     switch (tstamp_config->tx_type) {
0466     case HWTSTAMP_TX_OFF:
0467         break;
0468     case HWTSTAMP_TX_ONESTEP_SYNC:
0469         gem_ptp_set_one_step_sync(bp, 1);
0470         tx_bd_control = TSTAMP_ALL_FRAMES;
0471         break;
0472     case HWTSTAMP_TX_ON:
0473         gem_ptp_set_one_step_sync(bp, 0);
0474         tx_bd_control = TSTAMP_ALL_FRAMES;
0475         break;
0476     default:
0477         return -ERANGE;
0478     }
0479 
0480     switch (tstamp_config->rx_filter) {
0481     case HWTSTAMP_FILTER_NONE:
0482         break;
0483     case HWTSTAMP_FILTER_PTP_V1_L4_SYNC:
0484         break;
0485     case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ:
0486         break;
0487     case HWTSTAMP_FILTER_PTP_V2_EVENT:
0488     case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
0489     case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
0490     case HWTSTAMP_FILTER_PTP_V2_SYNC:
0491     case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
0492     case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
0493     case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
0494     case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
0495     case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
0496         rx_bd_control =  TSTAMP_ALL_PTP_FRAMES;
0497         tstamp_config->rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
0498         regval = macb_readl(bp, NCR);
0499         macb_writel(bp, NCR, (regval | MACB_BIT(SRTSM)));
0500         break;
0501     case HWTSTAMP_FILTER_PTP_V1_L4_EVENT:
0502     case HWTSTAMP_FILTER_ALL:
0503         rx_bd_control = TSTAMP_ALL_FRAMES;
0504         tstamp_config->rx_filter = HWTSTAMP_FILTER_ALL;
0505         break;
0506     default:
0507         tstamp_config->rx_filter = HWTSTAMP_FILTER_NONE;
0508         return -ERANGE;
0509     }
0510 
0511     if (gem_ptp_set_ts_mode(bp, tx_bd_control, rx_bd_control) != 0)
0512         return -ERANGE;
0513 
0514     if (copy_to_user(ifr->ifr_data, tstamp_config, sizeof(*tstamp_config)))
0515         return -EFAULT;
0516     else
0517         return 0;
0518 }
0519
[ 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!