net/phy/bcm-phy-ptp.c

0001 // SPDX-License-Identifier: GPL-2.0
0002 /*
0003  * Copyright (C) 2022 Meta Platforms Inc.
0004  * Copyright (C) 2022 Jonathan Lemon <jonathan.lemon@gmail.com>
0005  */
0006
0007 #include <asm/unaligned.h>
0008 #include <linux/mii.h>
0009 #include <linux/phy.h>
0010 #include <linux/ptp_classify.h>
0011 #include <linux/ptp_clock_kernel.h>
0012 #include <linux/net_tstamp.h>
0013 #include <linux/netdevice.h>
0014 #include <linux/workqueue.h>
0015
0016 #include "bcm-phy-lib.h"
0017
0018 /* IEEE 1588 Expansion registers */
0019 #define SLICE_CTRL      0x0810
0020 #define  SLICE_TX_EN            BIT(0)
0021 #define  SLICE_RX_EN            BIT(8)
0022 #define TX_EVENT_MODE       0x0811
0023 #define  MODE_TX_UPDATE_CF      BIT(0)
0024 #define  MODE_TX_REPLACE_TS_CF      BIT(1)
0025 #define  MODE_TX_REPLACE_TS     GENMASK(1, 0)
0026 #define RX_EVENT_MODE       0x0819
0027 #define  MODE_RX_UPDATE_CF      BIT(0)
0028 #define  MODE_RX_INSERT_TS_48       BIT(1)
0029 #define  MODE_RX_INSERT_TS_64       GENMASK(1, 0)
0030
0031 #define MODE_EVT_SHIFT_SYNC     0
0032 #define MODE_EVT_SHIFT_DELAY_REQ    2
0033 #define MODE_EVT_SHIFT_PDELAY_REQ   4
0034 #define MODE_EVT_SHIFT_PDELAY_RESP  6
0035
0036 #define MODE_SEL_SHIFT_PORT     0
0037 #define MODE_SEL_SHIFT_CPU      8
0038
0039 #define RX_MODE_SEL(sel, evt, act) \
0040     (((MODE_RX_##act) << (MODE_EVT_SHIFT_##evt)) << (MODE_SEL_SHIFT_##sel))
0041
0042 #define TX_MODE_SEL(sel, evt, act) \
0043     (((MODE_TX_##act) << (MODE_EVT_SHIFT_##evt)) << (MODE_SEL_SHIFT_##sel))
0044
0045 /* needs global TS capture first */
0046 #define TX_TS_CAPTURE       0x0821
0047 #define  TX_TS_CAP_EN           BIT(0)
0048 #define RX_TS_CAPTURE       0x0822
0049 #define  RX_TS_CAP_EN           BIT(0)
0050
0051 #define TIME_CODE_0     0x0854
0052 #define TIME_CODE_1     0x0855
0053 #define TIME_CODE_2     0x0856
0054 #define TIME_CODE_3     0x0857
0055 #define TIME_CODE_4     0x0858
0056
0057 #define DPLL_SELECT     0x085b
0058 #define  DPLL_HB_MODE2          BIT(6)
0059
0060 #define SHADOW_CTRL     0x085c
0061 #define SHADOW_LOAD     0x085d
0062 #define  TIME_CODE_LOAD         BIT(10)
0063 #define  SYNC_OUT_LOAD          BIT(9)
0064 #define  NCO_TIME_LOAD          BIT(7)
0065 #define  FREQ_LOAD          BIT(6)
0066 #define INTR_MASK       0x085e
0067 #define INTR_STATUS     0x085f
0068 #define  INTC_FSYNC         BIT(0)
0069 #define  INTC_SOP           BIT(1)
0070
0071 #define NCO_FREQ_LSB        0x0873
0072 #define NCO_FREQ_MSB        0x0874
0073
0074 #define NCO_TIME_0      0x0875
0075 #define NCO_TIME_1      0x0876
0076 #define NCO_TIME_2_CTRL     0x0877
0077 #define  FREQ_MDIO_SEL          BIT(14)
0078
0079 #define SYNC_OUT_0      0x0878
0080 #define SYNC_OUT_1      0x0879
0081 #define SYNC_OUT_2      0x087a
0082
0083 #define SYNC_IN_DIVIDER     0x087b
0084
0085 #define SYNOUT_TS_0     0x087c
0086 #define SYNOUT_TS_1     0x087d
0087 #define SYNOUT_TS_2     0x087e
0088
0089 #define NSE_CTRL        0x087f
0090 #define  NSE_GMODE_EN           GENMASK(15, 14)
0091 #define  NSE_CAPTURE_EN         BIT(13)
0092 #define  NSE_INIT           BIT(12)
0093 #define  NSE_CPU_FRAMESYNC      BIT(5)
0094 #define  NSE_SYNC1_FRAMESYNC        BIT(3)
0095 #define  NSE_FRAMESYNC_MASK     GENMASK(5, 2)
0096 #define  NSE_PEROUT_EN          BIT(1)
0097 #define  NSE_ONESHOT_EN         BIT(0)
0098 #define  NSE_SYNC_OUT_MASK      GENMASK(1, 0)
0099
0100 #define TS_READ_CTRL        0x0885
0101 #define  TS_READ_START          BIT(0)
0102 #define  TS_READ_END            BIT(1)
0103
0104 #define HB_REG_0        0x0886
0105 #define HB_REG_1        0x0887
0106 #define HB_REG_2        0x0888
0107 #define HB_REG_3        0x08ec
0108 #define HB_REG_4        0x08ed
0109 #define HB_STAT_CTRL        0x088e
0110 #define  HB_READ_START          BIT(10)
0111 #define  HB_READ_END            BIT(11)
0112 #define  HB_READ_MASK           GENMASK(11, 10)
0113
0114 #define TS_REG_0        0x0889
0115 #define TS_REG_1        0x088a
0116 #define TS_REG_2        0x088b
0117 #define TS_REG_3        0x08c4
0118
0119 #define TS_INFO_0       0x088c
0120 #define TS_INFO_1       0x088d
0121
0122 #define TIMECODE_CTRL       0x08c3
0123 #define  TX_TIMECODE_SEL        GENMASK(7, 0)
0124 #define  RX_TIMECODE_SEL        GENMASK(15, 8)
0125
0126 #define TIME_SYNC       0x0ff5
0127 #define  TIME_SYNC_EN           BIT(0)
0128
0129 struct bcm_ptp_private {
0130     struct phy_device *phydev;
0131     struct mii_timestamper mii_ts;
0132     struct ptp_clock *ptp_clock;
0133     struct ptp_clock_info ptp_info;
0134     struct ptp_pin_desc pin;
0135     struct mutex mutex;
0136     struct sk_buff_head tx_queue;
0137     int tx_type;
0138     bool hwts_rx;
0139     u16 nse_ctrl;
0140     bool pin_active;
0141     struct delayed_work pin_work;
0142 };
0143
0144 struct bcm_ptp_skb_cb {
0145     unsigned long timeout;
0146     u16 seq_id;
0147     u8 msgtype;
0148     bool discard;
0149 };
0150
0151 struct bcm_ptp_capture {
0152     ktime_t hwtstamp;
0153     u16 seq_id;
0154     u8 msgtype;
0155     bool tx_dir;
0156 };
0157
0158 #define BCM_SKB_CB(skb)     ((struct bcm_ptp_skb_cb *)(skb)->cb)
0159 #define SKB_TS_TIMEOUT      10          /* jiffies */
0160
0161 #define BCM_MAX_PULSE_8NS   ((1U << 9) - 1)
0162 #define BCM_MAX_PERIOD_8NS  ((1U << 30) - 1)
0163
0164 #define BRCM_PHY_MODEL(phydev) \
0165     ((phydev)->drv->phy_id & (phydev)->drv->phy_id_mask)
0166
0167 static struct bcm_ptp_private *mii2priv(struct mii_timestamper *mii_ts)
0168 {
0169     return container_of(mii_ts, struct bcm_ptp_private, mii_ts);
0170 }
0171
0172 static struct bcm_ptp_private *ptp2priv(struct ptp_clock_info *info)
0173 {
0174     return container_of(info, struct bcm_ptp_private, ptp_info);
0175 }
0176
0177 static void bcm_ptp_get_framesync_ts(struct phy_device *phydev,
0178                      struct timespec64 *ts)
0179 {
0180     u16 hb[4];
0181
0182     bcm_phy_write_exp(phydev, HB_STAT_CTRL, HB_READ_START);
0183
0184     hb[0] = bcm_phy_read_exp(phydev, HB_REG_0);
0185     hb[1] = bcm_phy_read_exp(phydev, HB_REG_1);
0186     hb[2] = bcm_phy_read_exp(phydev, HB_REG_2);
0187     hb[3] = bcm_phy_read_exp(phydev, HB_REG_3);
0188
0189     bcm_phy_write_exp(phydev, HB_STAT_CTRL, HB_READ_END);
0190     bcm_phy_write_exp(phydev, HB_STAT_CTRL, 0);
0191
0192     ts->tv_sec = (hb[3] << 16) | hb[2];
0193     ts->tv_nsec = (hb[1] << 16) | hb[0];
0194 }
0195
0196 static u16 bcm_ptp_framesync_disable(struct phy_device *phydev, u16 orig_ctrl)
0197 {
0198     u16 ctrl = orig_ctrl & ~(NSE_FRAMESYNC_MASK | NSE_CAPTURE_EN);
0199
0200     bcm_phy_write_exp(phydev, NSE_CTRL, ctrl);
0201
0202     return ctrl;
0203 }
0204
0205 static void bcm_ptp_framesync_restore(struct phy_device *phydev, u16 orig_ctrl)
0206 {
0207     if (orig_ctrl & NSE_FRAMESYNC_MASK)
0208         bcm_phy_write_exp(phydev, NSE_CTRL, orig_ctrl);
0209 }
0210
0211 static void bcm_ptp_framesync(struct phy_device *phydev, u16 ctrl)
0212 {
0213     /* trigger framesync - must have 0->1 transition. */
0214     bcm_phy_write_exp(phydev, NSE_CTRL, ctrl | NSE_CPU_FRAMESYNC);
0215 }
0216
0217 static int bcm_ptp_framesync_ts(struct phy_device *phydev,
0218                 struct ptp_system_timestamp *sts,
0219                 struct timespec64 *ts,
0220                 u16 orig_ctrl)
0221 {
0222     u16 ctrl, reg;
0223     int i;
0224
0225     ctrl = bcm_ptp_framesync_disable(phydev, orig_ctrl);
0226
0227     ptp_read_system_prets(sts);
0228
0229     /* trigger framesync + capture */
0230     bcm_ptp_framesync(phydev, ctrl | NSE_CAPTURE_EN);
0231
0232     ptp_read_system_postts(sts);
0233
0234     /* poll for FSYNC interrupt from TS capture */
0235     for (i = 0; i < 10; i++) {
0236         reg = bcm_phy_read_exp(phydev, INTR_STATUS);
0237         if (reg & INTC_FSYNC) {
0238             bcm_ptp_get_framesync_ts(phydev, ts);
0239             break;
0240         }
0241     }
0242
0243     bcm_ptp_framesync_restore(phydev, orig_ctrl);
0244
0245     return reg & INTC_FSYNC ? 0 : -ETIMEDOUT;
0246 }
0247
0248 static int bcm_ptp_gettimex(struct ptp_clock_info *info,
0249                 struct timespec64 *ts,
0250                 struct ptp_system_timestamp *sts)
0251 {
0252     struct bcm_ptp_private *priv = ptp2priv(info);
0253     int err;
0254
0255     mutex_lock(&priv->mutex);
0256     err = bcm_ptp_framesync_ts(priv->phydev, sts, ts, priv->nse_ctrl);
0257     mutex_unlock(&priv->mutex);
0258
0259     return err;
0260 }
0261
0262 static int bcm_ptp_settime_locked(struct bcm_ptp_private *priv,
0263                   const struct timespec64 *ts)
0264 {
0265     struct phy_device *phydev = priv->phydev;
0266     u16 ctrl;
0267     u64 ns;
0268
0269     ctrl = bcm_ptp_framesync_disable(phydev, priv->nse_ctrl);
0270
0271     /* set up time code */
0272     bcm_phy_write_exp(phydev, TIME_CODE_0, ts->tv_nsec);
0273     bcm_phy_write_exp(phydev, TIME_CODE_1, ts->tv_nsec >> 16);
0274     bcm_phy_write_exp(phydev, TIME_CODE_2, ts->tv_sec);
0275     bcm_phy_write_exp(phydev, TIME_CODE_3, ts->tv_sec >> 16);
0276     bcm_phy_write_exp(phydev, TIME_CODE_4, ts->tv_sec >> 32);
0277
0278     /* set NCO counter to match */
0279     ns = timespec64_to_ns(ts);
0280     bcm_phy_write_exp(phydev, NCO_TIME_0, ns >> 4);
0281     bcm_phy_write_exp(phydev, NCO_TIME_1, ns >> 20);
0282     bcm_phy_write_exp(phydev, NCO_TIME_2_CTRL, (ns >> 36) & 0xfff);
0283
0284     /* set up load on next frame sync (auto-clears due to NSE_INIT) */
0285     bcm_phy_write_exp(phydev, SHADOW_LOAD, TIME_CODE_LOAD | NCO_TIME_LOAD);
0286
0287     /* must have NSE_INIT in order to write time code */
0288     bcm_ptp_framesync(phydev, ctrl | NSE_INIT);
0289
0290     bcm_ptp_framesync_restore(phydev, priv->nse_ctrl);
0291
0292     return 0;
0293 }
0294
0295 static int bcm_ptp_settime(struct ptp_clock_info *info,
0296                const struct timespec64 *ts)
0297 {
0298     struct bcm_ptp_private *priv = ptp2priv(info);
0299     int err;
0300
0301     mutex_lock(&priv->mutex);
0302     err = bcm_ptp_settime_locked(priv, ts);
0303     mutex_unlock(&priv->mutex);
0304
0305     return err;
0306 }
0307
0308 static int bcm_ptp_adjtime_locked(struct bcm_ptp_private *priv,
0309                   s64 delta_ns)
0310 {
0311     struct timespec64 ts;
0312     int err;
0313     s64 ns;
0314
0315     err = bcm_ptp_framesync_ts(priv->phydev, NULL, &ts, priv->nse_ctrl);
0316     if (!err) {
0317         ns = timespec64_to_ns(&ts) + delta_ns;
0318         ts = ns_to_timespec64(ns);
0319         err = bcm_ptp_settime_locked(priv, &ts);
0320     }
0321     return err;
0322 }
0323
0324 static int bcm_ptp_adjtime(struct ptp_clock_info *info, s64 delta_ns)
0325 {
0326     struct bcm_ptp_private *priv = ptp2priv(info);
0327     int err;
0328
0329     mutex_lock(&priv->mutex);
0330     err = bcm_ptp_adjtime_locked(priv, delta_ns);
0331     mutex_unlock(&priv->mutex);
0332
0333     return err;
0334 }
0335
0336 /* A 125Mhz clock should adjust 8ns per pulse.
0337  * The frequency adjustment base is 0x8000 0000, or 8*2^28.
0338  *
0339  * Frequency adjustment is
0340  * adj = scaled_ppm * 8*2^28 / (10^6 * 2^16)
0341  *   which simplifies to:
0342  * adj = scaled_ppm * 2^9 / 5^6
0343  */
0344 static int bcm_ptp_adjfine(struct ptp_clock_info *info, long scaled_ppm)
0345 {
0346     struct bcm_ptp_private *priv = ptp2priv(info);
0347     int neg_adj = 0;
0348     u32 diff, freq;
0349     u16 ctrl;
0350     u64 adj;
0351
0352     if (scaled_ppm < 0) {
0353         neg_adj = 1;
0354         scaled_ppm = -scaled_ppm;
0355     }
0356
0357     adj = scaled_ppm << 9;
0358     diff = div_u64(adj, 15625);
0359     freq = (8 << 28) + (neg_adj ? -diff : diff);
0360
0361     mutex_lock(&priv->mutex);
0362
0363     ctrl = bcm_ptp_framesync_disable(priv->phydev, priv->nse_ctrl);
0364
0365     bcm_phy_write_exp(priv->phydev, NCO_FREQ_LSB, freq);
0366     bcm_phy_write_exp(priv->phydev, NCO_FREQ_MSB, freq >> 16);
0367
0368     bcm_phy_write_exp(priv->phydev, NCO_TIME_2_CTRL, FREQ_MDIO_SEL);
0369
0370     /* load on next framesync */
0371     bcm_phy_write_exp(priv->phydev, SHADOW_LOAD, FREQ_LOAD);
0372
0373     bcm_ptp_framesync(priv->phydev, ctrl);
0374
0375     /* clear load */
0376     bcm_phy_write_exp(priv->phydev, SHADOW_LOAD, 0);
0377
0378     bcm_ptp_framesync_restore(priv->phydev, priv->nse_ctrl);
0379
0380     mutex_unlock(&priv->mutex);
0381
0382     return 0;
0383 }
0384
0385 static bool bcm_ptp_rxtstamp(struct mii_timestamper *mii_ts,
0386                  struct sk_buff *skb, int type)
0387 {
0388     struct bcm_ptp_private *priv = mii2priv(mii_ts);
0389     struct skb_shared_hwtstamps *hwts;
0390     struct ptp_header *header;
0391     u32 sec, nsec;
0392     u8 *data;
0393     int off;
0394
0395     if (!priv->hwts_rx)
0396         return false;
0397
0398     header = ptp_parse_header(skb, type);
0399     if (!header)
0400         return false;
0401
0402     data = (u8 *)(header + 1);
0403     sec = get_unaligned_be32(data);
0404     nsec = get_unaligned_be32(data + 4);
0405
0406     hwts = skb_hwtstamps(skb);
0407     hwts->hwtstamp = ktime_set(sec, nsec);
0408
0409     off = data - skb->data + 8;
0410     if (off < skb->len) {
0411         memmove(data, data + 8, skb->len - off);
0412         __pskb_trim(skb, skb->len - 8);
0413     }
0414
0415     return false;
0416 }
0417
0418 static bool bcm_ptp_get_tstamp(struct bcm_ptp_private *priv,
0419                    struct bcm_ptp_capture *capts)
0420 {
0421     struct phy_device *phydev = priv->phydev;
0422     u16 ts[4], reg;
0423     u32 sec, nsec;
0424
0425     mutex_lock(&priv->mutex);
0426
0427     reg = bcm_phy_read_exp(phydev, INTR_STATUS);
0428     if ((reg & INTC_SOP) == 0) {
0429         mutex_unlock(&priv->mutex);
0430         return false;
0431     }
0432
0433     bcm_phy_write_exp(phydev, TS_READ_CTRL, TS_READ_START);
0434
0435     ts[0] = bcm_phy_read_exp(phydev, TS_REG_0);
0436     ts[1] = bcm_phy_read_exp(phydev, TS_REG_1);
0437     ts[2] = bcm_phy_read_exp(phydev, TS_REG_2);
0438     ts[3] = bcm_phy_read_exp(phydev, TS_REG_3);
0439
0440     /* not in be32 format for some reason */
0441     capts->seq_id = bcm_phy_read_exp(priv->phydev, TS_INFO_0);
0442
0443     reg = bcm_phy_read_exp(phydev, TS_INFO_1);
0444     capts->msgtype = reg >> 12;
0445     capts->tx_dir = !!(reg & BIT(11));
0446
0447     bcm_phy_write_exp(phydev, TS_READ_CTRL, TS_READ_END);
0448     bcm_phy_write_exp(phydev, TS_READ_CTRL, 0);
0449
0450     mutex_unlock(&priv->mutex);
0451
0452     sec = (ts[3] << 16) | ts[2];
0453     nsec = (ts[1] << 16) | ts[0];
0454     capts->hwtstamp = ktime_set(sec, nsec);
0455
0456     return true;
0457 }
0458
0459 static void bcm_ptp_match_tstamp(struct bcm_ptp_private *priv,
0460                  struct bcm_ptp_capture *capts)
0461 {
0462     struct skb_shared_hwtstamps hwts;
0463     struct sk_buff *skb, *ts_skb;
0464     unsigned long flags;
0465     bool first = false;
0466
0467     ts_skb = NULL;
0468     spin_lock_irqsave(&priv->tx_queue.lock, flags);
0469     skb_queue_walk(&priv->tx_queue, skb) {
0470         if (BCM_SKB_CB(skb)->seq_id == capts->seq_id &&
0471             BCM_SKB_CB(skb)->msgtype == capts->msgtype) {
0472             first = skb_queue_is_first(&priv->tx_queue, skb);
0473             __skb_unlink(skb, &priv->tx_queue);
0474             ts_skb = skb;
0475             break;
0476         }
0477     }
0478     spin_unlock_irqrestore(&priv->tx_queue.lock, flags);
0479
0480     /* TX captures one-step packets, discard them if needed. */
0481     if (ts_skb) {
0482         if (BCM_SKB_CB(ts_skb)->discard) {
0483             kfree_skb(ts_skb);
0484         } else {
0485             memset(&hwts, 0, sizeof(hwts));
0486             hwts.hwtstamp = capts->hwtstamp;
0487             skb_complete_tx_timestamp(ts_skb, &hwts);
0488         }
0489     }
0490
0491     /* not first match, try and expire entries */
0492     if (!first) {
0493         while ((skb = skb_dequeue(&priv->tx_queue))) {
0494             if (!time_after(jiffies, BCM_SKB_CB(skb)->timeout)) {
0495                 skb_queue_head(&priv->tx_queue, skb);
0496                 break;
0497             }
0498             kfree_skb(skb);
0499         }
0500     }
0501 }
0502
0503 static long bcm_ptp_do_aux_work(struct ptp_clock_info *info)
0504 {
0505     struct bcm_ptp_private *priv = ptp2priv(info);
0506     struct bcm_ptp_capture capts;
0507     bool reschedule = false;
0508
0509     while (!skb_queue_empty_lockless(&priv->tx_queue)) {
0510         if (!bcm_ptp_get_tstamp(priv, &capts)) {
0511             reschedule = true;
0512             break;
0513         }
0514         bcm_ptp_match_tstamp(priv, &capts);
0515     }
0516
0517     return reschedule ? 1 : -1;
0518 }
0519
0520 static int bcm_ptp_cancel_func(struct bcm_ptp_private *priv)
0521 {
0522     if (!priv->pin_active)
0523         return 0;
0524
0525     priv->pin_active = false;
0526
0527     priv->nse_ctrl &= ~(NSE_SYNC_OUT_MASK | NSE_SYNC1_FRAMESYNC |
0528                 NSE_CAPTURE_EN);
0529     bcm_phy_write_exp(priv->phydev, NSE_CTRL, priv->nse_ctrl);
0530
0531     cancel_delayed_work_sync(&priv->pin_work);
0532
0533     return 0;
0534 }
0535
0536 static void bcm_ptp_perout_work(struct work_struct *pin_work)
0537 {
0538     struct bcm_ptp_private *priv =
0539         container_of(pin_work, struct bcm_ptp_private, pin_work.work);
0540     struct phy_device *phydev = priv->phydev;
0541     struct timespec64 ts;
0542     u64 ns, next;
0543     u16 ctrl;
0544
0545     mutex_lock(&priv->mutex);
0546
0547     /* no longer running */
0548     if (!priv->pin_active) {
0549         mutex_unlock(&priv->mutex);
0550         return;
0551     }
0552
0553     bcm_ptp_framesync_ts(phydev, NULL, &ts, priv->nse_ctrl);
0554
0555     /* this is 1PPS only */
0556     next = NSEC_PER_SEC - ts.tv_nsec;
0557     ts.tv_sec += next < NSEC_PER_MSEC ? 2 : 1;
0558     ts.tv_nsec = 0;
0559
0560     ns = timespec64_to_ns(&ts);
0561
0562     /* force 0->1 transition for ONESHOT */
0563     ctrl = bcm_ptp_framesync_disable(phydev,
0564                      priv->nse_ctrl & ~NSE_ONESHOT_EN);
0565
0566     bcm_phy_write_exp(phydev, SYNOUT_TS_0, ns & 0xfff0);
0567     bcm_phy_write_exp(phydev, SYNOUT_TS_1, ns >> 16);
0568     bcm_phy_write_exp(phydev, SYNOUT_TS_2, ns >> 32);
0569
0570     /* load values on next framesync */
0571     bcm_phy_write_exp(phydev, SHADOW_LOAD, SYNC_OUT_LOAD);
0572
0573     bcm_ptp_framesync(phydev, ctrl | NSE_ONESHOT_EN | NSE_INIT);
0574
0575     priv->nse_ctrl |= NSE_ONESHOT_EN;
0576     bcm_ptp_framesync_restore(phydev, priv->nse_ctrl);
0577
0578     mutex_unlock(&priv->mutex);
0579
0580     next = next + NSEC_PER_MSEC;
0581     schedule_delayed_work(&priv->pin_work, nsecs_to_jiffies(next));
0582 }
0583
0584 static int bcm_ptp_perout_locked(struct bcm_ptp_private *priv,
0585                  struct ptp_perout_request *req, int on)
0586 {
0587     struct phy_device *phydev = priv->phydev;
0588     u64 period, pulse;
0589     u16 val;
0590
0591     if (!on)
0592         return bcm_ptp_cancel_func(priv);
0593
0594     /* 1PPS */
0595     if (req->period.sec != 1 || req->period.nsec != 0)
0596         return -EINVAL;
0597
0598     period = BCM_MAX_PERIOD_8NS;    /* write nonzero value */
0599
0600     if (req->flags & PTP_PEROUT_PHASE)
0601         return -EOPNOTSUPP;
0602
0603     if (req->flags & PTP_PEROUT_DUTY_CYCLE)
0604         pulse = ktime_to_ns(ktime_set(req->on.sec, req->on.nsec));
0605     else
0606         pulse = (u64)BCM_MAX_PULSE_8NS << 3;
0607
0608     /* convert to 8ns units */
0609     pulse >>= 3;
0610
0611     if (!pulse || pulse > period || pulse > BCM_MAX_PULSE_8NS)
0612         return -EINVAL;
0613
0614     bcm_phy_write_exp(phydev, SYNC_OUT_0, period);
0615
0616     val = ((pulse & 0x3) << 14) | ((period >> 16) & 0x3fff);
0617     bcm_phy_write_exp(phydev, SYNC_OUT_1, val);
0618
0619     val = ((pulse >> 2) & 0x7f) | (pulse << 7);
0620     bcm_phy_write_exp(phydev, SYNC_OUT_2, val);
0621
0622     if (priv->pin_active)
0623         cancel_delayed_work_sync(&priv->pin_work);
0624
0625     priv->pin_active = true;
0626     INIT_DELAYED_WORK(&priv->pin_work, bcm_ptp_perout_work);
0627     schedule_delayed_work(&priv->pin_work, 0);
0628
0629     return 0;
0630 }
0631
0632 static void bcm_ptp_extts_work(struct work_struct *pin_work)
0633 {
0634     struct bcm_ptp_private *priv =
0635         container_of(pin_work, struct bcm_ptp_private, pin_work.work);
0636     struct phy_device *phydev = priv->phydev;
0637     struct ptp_clock_event event;
0638     struct timespec64 ts;
0639     u16 reg;
0640
0641     mutex_lock(&priv->mutex);
0642
0643     /* no longer running */
0644     if (!priv->pin_active) {
0645         mutex_unlock(&priv->mutex);
0646         return;
0647     }
0648
0649     reg = bcm_phy_read_exp(phydev, INTR_STATUS);
0650     if ((reg & INTC_FSYNC) == 0)
0651         goto out;
0652
0653     bcm_ptp_get_framesync_ts(phydev, &ts);
0654
0655     event.index = 0;
0656     event.type = PTP_CLOCK_EXTTS;
0657     event.timestamp = timespec64_to_ns(&ts);
0658     ptp_clock_event(priv->ptp_clock, &event);
0659
0660 out:
0661     mutex_unlock(&priv->mutex);
0662     schedule_delayed_work(&priv->pin_work, HZ / 4);
0663 }
0664
0665 static int bcm_ptp_extts_locked(struct bcm_ptp_private *priv, int on)
0666 {
0667     struct phy_device *phydev = priv->phydev;
0668
0669     if (!on)
0670         return bcm_ptp_cancel_func(priv);
0671
0672     if (priv->pin_active)
0673         cancel_delayed_work_sync(&priv->pin_work);
0674
0675     bcm_ptp_framesync_disable(phydev, priv->nse_ctrl);
0676
0677     priv->nse_ctrl |= NSE_SYNC1_FRAMESYNC | NSE_CAPTURE_EN;
0678
0679     bcm_ptp_framesync_restore(phydev, priv->nse_ctrl);
0680
0681     priv->pin_active = true;
0682     INIT_DELAYED_WORK(&priv->pin_work, bcm_ptp_extts_work);
0683     schedule_delayed_work(&priv->pin_work, 0);
0684
0685     return 0;
0686 }
0687
0688 static int bcm_ptp_enable(struct ptp_clock_info *info,
0689               struct ptp_clock_request *rq, int on)
0690 {
0691     struct bcm_ptp_private *priv = ptp2priv(info);
0692     int err = -EBUSY;
0693
0694     mutex_lock(&priv->mutex);
0695
0696     switch (rq->type) {
0697     case PTP_CLK_REQ_PEROUT:
0698         if (priv->pin.func == PTP_PF_PEROUT)
0699             err = bcm_ptp_perout_locked(priv, &rq->perout, on);
0700         break;
0701     case PTP_CLK_REQ_EXTTS:
0702         if (priv->pin.func == PTP_PF_EXTTS)
0703             err = bcm_ptp_extts_locked(priv, on);
0704         break;
0705     default:
0706         err = -EOPNOTSUPP;
0707         break;
0708     }
0709
0710     mutex_unlock(&priv->mutex);
0711
0712     return err;
0713 }
0714
0715 static int bcm_ptp_verify(struct ptp_clock_info *info, unsigned int pin,
0716               enum ptp_pin_function func, unsigned int chan)
0717 {
0718     switch (func) {
0719     case PTP_PF_NONE:
0720     case PTP_PF_EXTTS:
0721     case PTP_PF_PEROUT:
0722         break;
0723     default:
0724         return -EOPNOTSUPP;
0725     }
0726     return 0;
0727 }
0728
0729 static const struct ptp_clock_info bcm_ptp_clock_info = {
0730     .owner      = THIS_MODULE,
0731     .name       = KBUILD_MODNAME,
0732     .max_adj    = 100000000,
0733     .gettimex64 = bcm_ptp_gettimex,
0734     .settime64  = bcm_ptp_settime,
0735     .adjtime    = bcm_ptp_adjtime,
0736     .adjfine    = bcm_ptp_adjfine,
0737     .enable     = bcm_ptp_enable,
0738     .verify     = bcm_ptp_verify,
0739     .do_aux_work    = bcm_ptp_do_aux_work,
0740     .n_pins     = 1,
0741     .n_per_out  = 1,
0742     .n_ext_ts   = 1,
0743 };
0744
0745 static void bcm_ptp_txtstamp(struct mii_timestamper *mii_ts,
0746                  struct sk_buff *skb, int type)
0747 {
0748     struct bcm_ptp_private *priv = mii2priv(mii_ts);
0749     struct ptp_header *hdr;
0750     bool discard = false;
0751     int msgtype;
0752
0753     hdr = ptp_parse_header(skb, type);
0754     if (!hdr)
0755         goto out;
0756     msgtype = ptp_get_msgtype(hdr, type);
0757
0758     switch (priv->tx_type) {
0759     case HWTSTAMP_TX_ONESTEP_P2P:
0760         if (msgtype == PTP_MSGTYPE_PDELAY_RESP)
0761             discard = true;
0762         fallthrough;
0763     case HWTSTAMP_TX_ONESTEP_SYNC:
0764         if (msgtype == PTP_MSGTYPE_SYNC)
0765             discard = true;
0766         fallthrough;
0767     case HWTSTAMP_TX_ON:
0768         BCM_SKB_CB(skb)->timeout = jiffies + SKB_TS_TIMEOUT;
0769         BCM_SKB_CB(skb)->seq_id = be16_to_cpu(hdr->sequence_id);
0770         BCM_SKB_CB(skb)->msgtype = msgtype;
0771         BCM_SKB_CB(skb)->discard = discard;
0772         skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
0773         skb_queue_tail(&priv->tx_queue, skb);
0774         ptp_schedule_worker(priv->ptp_clock, 0);
0775         return;
0776     default:
0777         break;
0778     }
0779
0780 out:
0781     kfree_skb(skb);
0782 }
0783
0784 static int bcm_ptp_hwtstamp(struct mii_timestamper *mii_ts,
0785                 struct ifreq *ifr)
0786 {
0787     struct bcm_ptp_private *priv = mii2priv(mii_ts);
0788     struct hwtstamp_config cfg;
0789     u16 mode, ctrl;
0790
0791     if (copy_from_user(&cfg, ifr->ifr_data, sizeof(cfg)))
0792         return -EFAULT;
0793
0794     switch (cfg.rx_filter) {
0795     case HWTSTAMP_FILTER_NONE:
0796         priv->hwts_rx = false;
0797         break;
0798     case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
0799     case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
0800     case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
0801     case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
0802     case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
0803     case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
0804     case HWTSTAMP_FILTER_PTP_V2_EVENT:
0805     case HWTSTAMP_FILTER_PTP_V2_SYNC:
0806     case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
0807         cfg.rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
0808         priv->hwts_rx = true;
0809         break;
0810     default:
0811         return -ERANGE;
0812     }
0813
0814     priv->tx_type = cfg.tx_type;
0815
0816     ctrl  = priv->hwts_rx ? SLICE_RX_EN : 0;
0817     ctrl |= priv->tx_type != HWTSTAMP_TX_OFF ? SLICE_TX_EN : 0;
0818
0819     mode = TX_MODE_SEL(PORT, SYNC, REPLACE_TS) |
0820            TX_MODE_SEL(PORT, DELAY_REQ, REPLACE_TS) |
0821            TX_MODE_SEL(PORT, PDELAY_REQ, REPLACE_TS) |
0822            TX_MODE_SEL(PORT, PDELAY_RESP, REPLACE_TS);
0823
0824     bcm_phy_write_exp(priv->phydev, TX_EVENT_MODE, mode);
0825
0826     mode = RX_MODE_SEL(PORT, SYNC, INSERT_TS_64) |
0827            RX_MODE_SEL(PORT, DELAY_REQ, INSERT_TS_64) |
0828            RX_MODE_SEL(PORT, PDELAY_REQ, INSERT_TS_64) |
0829            RX_MODE_SEL(PORT, PDELAY_RESP, INSERT_TS_64);
0830
0831     bcm_phy_write_exp(priv->phydev, RX_EVENT_MODE, mode);
0832
0833     bcm_phy_write_exp(priv->phydev, SLICE_CTRL, ctrl);
0834
0835     if (ctrl & SLICE_TX_EN)
0836         bcm_phy_write_exp(priv->phydev, TX_TS_CAPTURE, TX_TS_CAP_EN);
0837     else
0838         ptp_cancel_worker_sync(priv->ptp_clock);
0839
0840     /* purge existing data */
0841     skb_queue_purge(&priv->tx_queue);
0842
0843     return copy_to_user(ifr->ifr_data, &cfg, sizeof(cfg)) ? -EFAULT : 0;
0844 }
0845
0846 static int bcm_ptp_ts_info(struct mii_timestamper *mii_ts,
0847                struct ethtool_ts_info *ts_info)
0848 {
0849     struct bcm_ptp_private *priv = mii2priv(mii_ts);
0850
0851     ts_info->phc_index = ptp_clock_index(priv->ptp_clock);
0852     ts_info->so_timestamping =
0853         SOF_TIMESTAMPING_TX_HARDWARE |
0854         SOF_TIMESTAMPING_RX_HARDWARE |
0855         SOF_TIMESTAMPING_RAW_HARDWARE;
0856     ts_info->tx_types =
0857         BIT(HWTSTAMP_TX_ON) |
0858         BIT(HWTSTAMP_TX_OFF) |
0859         BIT(HWTSTAMP_TX_ONESTEP_SYNC) |
0860         BIT(HWTSTAMP_TX_ONESTEP_P2P);
0861     ts_info->rx_filters =
0862         BIT(HWTSTAMP_FILTER_NONE) |
0863         BIT(HWTSTAMP_FILTER_PTP_V2_EVENT);
0864
0865     return 0;
0866 }
0867
0868 void bcm_ptp_stop(struct bcm_ptp_private *priv)
0869 {
0870     ptp_cancel_worker_sync(priv->ptp_clock);
0871     bcm_ptp_cancel_func(priv);
0872 }
0873 EXPORT_SYMBOL_GPL(bcm_ptp_stop);
0874
0875 void bcm_ptp_config_init(struct phy_device *phydev)
0876 {
0877     /* init network sync engine */
0878     bcm_phy_write_exp(phydev, NSE_CTRL, NSE_GMODE_EN | NSE_INIT);
0879
0880     /* enable time sync (TX/RX SOP capture) */
0881     bcm_phy_write_exp(phydev, TIME_SYNC, TIME_SYNC_EN);
0882
0883     /* use sec.nsec heartbeat capture */
0884     bcm_phy_write_exp(phydev, DPLL_SELECT, DPLL_HB_MODE2);
0885
0886     /* use 64 bit timecode for TX */
0887     bcm_phy_write_exp(phydev, TIMECODE_CTRL, TX_TIMECODE_SEL);
0888
0889     /* always allow FREQ_LOAD on framesync */
0890     bcm_phy_write_exp(phydev, SHADOW_CTRL, FREQ_LOAD);
0891
0892     bcm_phy_write_exp(phydev, SYNC_IN_DIVIDER, 1);
0893 }
0894 EXPORT_SYMBOL_GPL(bcm_ptp_config_init);
0895
0896 static void bcm_ptp_init(struct bcm_ptp_private *priv)
0897 {
0898     priv->nse_ctrl = NSE_GMODE_EN;
0899
0900     mutex_init(&priv->mutex);
0901     skb_queue_head_init(&priv->tx_queue);
0902
0903     priv->mii_ts.rxtstamp = bcm_ptp_rxtstamp;
0904     priv->mii_ts.txtstamp = bcm_ptp_txtstamp;
0905     priv->mii_ts.hwtstamp = bcm_ptp_hwtstamp;
0906     priv->mii_ts.ts_info = bcm_ptp_ts_info;
0907
0908     priv->phydev->mii_ts = &priv->mii_ts;
0909 }
0910
0911 struct bcm_ptp_private *bcm_ptp_probe(struct phy_device *phydev)
0912 {
0913     struct bcm_ptp_private *priv;
0914     struct ptp_clock *clock;
0915
0916     switch (BRCM_PHY_MODEL(phydev)) {
0917     case PHY_ID_BCM54210E:
0918         break;
0919     default:
0920         return NULL;
0921     }
0922
0923     priv = devm_kzalloc(&phydev->mdio.dev, sizeof(*priv), GFP_KERNEL);
0924     if (!priv)
0925         return ERR_PTR(-ENOMEM);
0926
0927     priv->ptp_info = bcm_ptp_clock_info;
0928
0929     snprintf(priv->pin.name, sizeof(priv->pin.name), "SYNC_OUT");
0930     priv->ptp_info.pin_config = &priv->pin;
0931
0932     clock = ptp_clock_register(&priv->ptp_info, &phydev->mdio.dev);
0933     if (IS_ERR(clock))
0934         return ERR_CAST(clock);
0935     priv->ptp_clock = clock;
0936
0937     priv->phydev = phydev;
0938     bcm_ptp_init(priv);
0939
0940     return priv;
0941 }
0942 EXPORT_SYMBOL_GPL(bcm_ptp_probe);
0943
0944 MODULE_LICENSE("GPL");