* [PATCH net-next v3 3/3] net: phy: broadcom: Add PTP support for some Broadcom PHYs.
2022-05-04 22:43 [PATCH net-next v3 0/3] Broadcom PTP PHY support Jonathan Lemon
2022-05-04 22:43 ` [PATCH net-next v3 1/3] net: phy: broadcom: Add Broadcom PTP hooks to bcm-phy-lib Jonathan Lemon
2022-05-04 22:43 ` [PATCH net-next v3 2/3] net: phy: broadcom: Hook up the PTP PHY functions Jonathan Lemon
@ 2022-05-04 22:43 ` Jonathan Lemon
2 siblings, 0 replies; 6+ messages in thread
From: Jonathan Lemon @ 2022-05-04 22:43 UTC (permalink / raw)
To: f.fainelli, bcm-kernel-feedback-list, andrew, hkallweit1,
richardcochran, lasse
Cc: netdev, kernel-team
This adds PTP support for BCM54210E Broadcom PHYs, in particular,
the BCM54213PE, as used in the Rasperry PI CM4. It has only been
tested on that hardware.
Signed-off-by: Jonathan Lemon <jonathan.lemon@gmail.com>
---
drivers/net/phy/Kconfig | 10 +
drivers/net/phy/Makefile | 1 +
drivers/net/phy/bcm-phy-ptp.c | 869 ++++++++++++++++++++++++++++++++++
3 files changed, 880 insertions(+)
create mode 100644 drivers/net/phy/bcm-phy-ptp.c
diff --git a/drivers/net/phy/Kconfig b/drivers/net/phy/Kconfig
index bbbf6c07ea53..67c4d4c2410c 100644
--- a/drivers/net/phy/Kconfig
+++ b/drivers/net/phy/Kconfig
@@ -108,6 +108,16 @@ config BROADCOM_PHY
Currently supports the BCM5411, BCM5421, BCM5461, BCM54616S, BCM5464,
BCM5481, BCM54810 and BCM5482 PHYs.
+config BCM_NET_PHYPTP
+ tristate "Broadcom PHY PTP support"
+ depends on NETWORK_PHY_TIMESTAMPING
+ depends on PHYLIB
+ depends on PTP_1588_CLOCK
+ depends on BROADCOM_PHY
+ depends on NET_PTP_CLASSIFY
+ help
+ Supports PTP timestamping for certain Broadcom PHYs.
+
config BCM54140_PHY
tristate "Broadcom BCM54140 PHY"
depends on PHYLIB
diff --git a/drivers/net/phy/Makefile b/drivers/net/phy/Makefile
index b82651b57043..1dc14fbdff12 100644
--- a/drivers/net/phy/Makefile
+++ b/drivers/net/phy/Makefile
@@ -47,6 +47,7 @@ obj-$(CONFIG_BCM84881_PHY) += bcm84881.o
obj-$(CONFIG_BCM87XX_PHY) += bcm87xx.o
obj-$(CONFIG_BCM_CYGNUS_PHY) += bcm-cygnus.o
obj-$(CONFIG_BCM_NET_PHYLIB) += bcm-phy-lib.o
+obj-$(CONFIG_BCM_NET_PHYPTP) += bcm-phy-ptp.o
obj-$(CONFIG_BROADCOM_PHY) += broadcom.o
obj-$(CONFIG_CICADA_PHY) += cicada.o
obj-$(CONFIG_CORTINA_PHY) += cortina.o
diff --git a/drivers/net/phy/bcm-phy-ptp.c b/drivers/net/phy/bcm-phy-ptp.c
new file mode 100644
index 000000000000..bb334991c767
--- /dev/null
+++ b/drivers/net/phy/bcm-phy-ptp.c
@@ -0,0 +1,869 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2022 Meta Platforms Inc.
+ * Copyright (C) 2022 Jonathan Lemon <jonathan.lemon@gmail.com>
+ */
+
+#include <asm/unaligned.h>
+#include <linux/mii.h>
+#include <linux/phy.h>
+#include <linux/ptp_classify.h>
+#include <linux/ptp_clock_kernel.h>
+#include <linux/net_tstamp.h>
+#include <linux/netdevice.h>
+#include <linux/workqueue.h>
+
+#include "bcm-phy-lib.h"
+
+/* IEEE 1588 Expansion registers */
+#define SLICE_CTRL 0x0810
+#define SLICE_TX_EN BIT(0)
+#define SLICE_RX_EN BIT(8)
+#define TX_EVENT_MODE 0x0811
+#define MODE_TX_UPDATE_CF BIT(0)
+#define MODE_TX_REPLACE_TS_CF BIT(1)
+#define MODE_TX_REPLACE_TS GENMASK(1, 0)
+#define RX_EVENT_MODE 0x0819
+#define MODE_RX_UPDATE_CF BIT(0)
+#define MODE_RX_INSERT_TS_48 BIT(1)
+#define MODE_RX_INSERT_TS_64 GENMASK(1, 0)
+
+#define MODE_EVT_SHIFT_SYNC 0
+#define MODE_EVT_SHIFT_DELAY_REQ 2
+#define MODE_EVT_SHIFT_PDELAY_REQ 4
+#define MODE_EVT_SHIFT_PDELAY_RESP 6
+
+#define MODE_SEL_SHIFT_PORT 0
+#define MODE_SEL_SHIFT_CPU 8
+
+#define RX_MODE_SEL(sel, evt, act) \
+ (((MODE_RX_##act) << (MODE_EVT_SHIFT_##evt)) << (MODE_SEL_SHIFT_##sel))
+
+#define TX_MODE_SEL(sel, evt, act) \
+ (((MODE_TX_##act) << (MODE_EVT_SHIFT_##evt)) << (MODE_SEL_SHIFT_##sel))
+
+/* needs global TS capture first */
+#define TX_TS_CAPTURE 0x0821
+#define TX_TS_CAP_EN BIT(0)
+#define RX_TS_CAPTURE 0x0822
+#define RX_TS_CAP_EN BIT(0)
+
+#define TIME_CODE_0 0x0854
+#define TIME_CODE_1 0x0855
+#define TIME_CODE_2 0x0856
+#define TIME_CODE_3 0x0857
+#define TIME_CODE_4 0x0858
+
+#define DPLL_SELECT 0x085b
+#define DPLL_HB_MODE2 BIT(6)
+
+#define SHADOW_CTRL 0x085c
+#define SHADOW_LOAD 0x085d
+#define TIME_CODE_LOAD BIT(10)
+#define SYNC_OUT_LOAD BIT(9)
+#define NCO_TIME_LOAD BIT(7)
+#define FREQ_LOAD BIT(6)
+#define INTR_MASK 0x085e
+#define INTR_STATUS 0x085f
+#define INTC_FSYNC BIT(0)
+#define INTC_SOP BIT(1)
+
+#define NCO_FREQ_LSB 0x0873
+#define NCO_FREQ_MSB 0x0874
+
+#define NCO_TIME_0 0x0875
+#define NCO_TIME_1 0x0876
+#define NCO_TIME_2_CTRL 0x0877
+#define FREQ_MDIO_SEL BIT(14)
+
+#define SYNC_OUT_0 0x0878
+#define SYNC_OUT_1 0x0879
+#define SYNC_OUT_2 0x087a
+
+#define SYNOUT_TS_0 0x087c
+#define SYNOUT_TS_1 0x087d
+#define SYNOUT_TS_2 0x087e
+
+#define NSE_CTRL 0x087f
+#define NSE_GMODE_EN GENMASK(15, 14)
+#define NSE_CAPTURE_EN BIT(13)
+#define NSE_INIT BIT(12)
+#define NSE_CPU_FRAMESYNC BIT(5)
+#define NSE_FRAMESYNC_MASK GENMASK(5, 2)
+#define NSE_PEROUT_EN BIT(1)
+#define NSE_ONESHOT_EN BIT(0)
+#define NSE_SYNC_OUT_MASK GENMASK(1, 0)
+
+#define TS_READ_CTRL 0x0885
+#define TS_READ_START BIT(0)
+#define TS_READ_END BIT(1)
+
+#define HB_REG_0 0x0886
+#define HB_REG_1 0x0887
+#define HB_REG_2 0x0888
+#define HB_REG_3 0x08ec
+#define HB_REG_4 0x08ed
+#define HB_STAT_CTRL 0x088e
+#define HB_READ_START BIT(10)
+#define HB_READ_END BIT(11)
+#define HB_READ_MASK GENMASK(11, 10)
+
+#define TS_REG_0 0x0889
+#define TS_REG_1 0x088a
+#define TS_REG_2 0x088b
+#define TS_REG_3 0x08c4
+
+#define TS_INFO_0 0x088c
+#define TS_INFO_1 0x088d
+
+#define TIMECODE_CTRL 0x08c3
+#define TX_TIMECODE_SEL GENMASK(7, 0)
+#define RX_TIMECODE_SEL GENMASK(15, 8)
+
+#define TIME_SYNC 0x0ff5
+#define TIME_SYNC_EN BIT(0)
+
+struct bcm_ptp_private {
+ struct phy_device *phydev;
+ struct mii_timestamper mii_ts;
+ struct ptp_clock *ptp_clock;
+ struct ptp_clock_info ptp_info;
+ struct mutex mutex;
+ struct sk_buff_head tx_queue;
+ int tx_type;
+ bool hwts_rx;
+ u16 nse_ctrl;
+
+ struct delayed_work out_work;
+ bool fsync_out;
+};
+
+struct bcm_ptp_skb_cb {
+ unsigned long timeout;
+ u16 seq_id;
+ u8 msgtype;
+ bool discard;
+};
+
+struct bcm_ptp_capture {
+ ktime_t hwtstamp;
+ u16 seq_id;
+ u8 msgtype;
+ bool tx_dir;
+};
+
+#define BCM_SKB_CB(skb) ((struct bcm_ptp_skb_cb *)(skb)->cb)
+#define SKB_TS_TIMEOUT 10 /* jiffies */
+
+#define BCM_MAX_PULSE_8NS ((1U << 9) - 1)
+#define BCM_MAX_PERIOD_8NS ((1U << 30) - 1)
+
+#define BRCM_PHY_MODEL(phydev) \
+ ((phydev)->drv->phy_id & (phydev)->drv->phy_id_mask)
+
+static struct bcm_ptp_private *mii2priv(struct mii_timestamper *mii_ts)
+{
+ return container_of(mii_ts, struct bcm_ptp_private, mii_ts);
+}
+
+static struct bcm_ptp_private *ptp2priv(struct ptp_clock_info *info)
+{
+ return container_of(info, struct bcm_ptp_private, ptp_info);
+}
+
+static int bcm_ptp_framesync(struct phy_device *phydev,
+ struct ptp_system_timestamp *sts,
+ u16 ctrl)
+{
+ u16 reg;
+ int i;
+
+ /* prep for framesync */
+ bcm_phy_write_exp(phydev, NSE_CTRL, ctrl);
+
+ ptp_read_system_prets(sts);
+
+ /* trigger framesync */
+ bcm_phy_write_exp(phydev, NSE_CTRL, ctrl | NSE_CPU_FRAMESYNC);
+
+ ptp_read_system_postts(sts);
+
+ if ((ctrl & NSE_CAPTURE_EN) == 0)
+ return 0;
+
+ /* poll for FSYNC interrupt from TS capture */
+ for (i = 0; i < 10; i++) {
+ reg = bcm_phy_read_exp(phydev, INTR_STATUS);
+ if (reg & INTC_FSYNC)
+ break;
+ }
+
+ return reg & INTC_FSYNC ? 0 : -ETIMEDOUT;
+}
+
+static int bcm_ptp_gettime_locked(struct bcm_ptp_private *priv,
+ struct timespec64 *ts,
+ struct ptp_system_timestamp *sts)
+{
+ struct phy_device *phydev = priv->phydev;
+ u16 hb[4];
+ int err;
+
+ err = bcm_ptp_framesync(phydev, sts, priv->nse_ctrl | NSE_CAPTURE_EN);
+ if (err)
+ return err;
+
+ bcm_phy_write_exp(phydev, HB_STAT_CTRL, HB_READ_START);
+
+ hb[0] = bcm_phy_read_exp(phydev, HB_REG_0);
+ hb[1] = bcm_phy_read_exp(phydev, HB_REG_1);
+ hb[2] = bcm_phy_read_exp(phydev, HB_REG_2);
+ hb[3] = bcm_phy_read_exp(phydev, HB_REG_3);
+
+ bcm_phy_write_exp(phydev, HB_STAT_CTRL, HB_READ_END);
+ bcm_phy_write_exp(phydev, HB_STAT_CTRL, 0);
+
+ ts->tv_sec = (hb[3] << 16) | hb[2];
+ ts->tv_nsec = (hb[1] << 16) | hb[0];
+
+ return 0;
+}
+
+static int bcm_ptp_gettimex(struct ptp_clock_info *info,
+ struct timespec64 *ts,
+ struct ptp_system_timestamp *sts)
+{
+ struct bcm_ptp_private *priv = ptp2priv(info);
+ int err;
+
+ mutex_lock(&priv->mutex);
+ err = bcm_ptp_gettime_locked(priv, ts, sts);
+ mutex_unlock(&priv->mutex);
+
+ return err;
+}
+
+static int bcm_ptp_settime_locked(struct bcm_ptp_private *priv,
+ const struct timespec64 *ts)
+{
+ struct phy_device *phydev = priv->phydev;
+ u64 ns;
+
+ /* set up time code */
+ bcm_phy_write_exp(phydev, TIME_CODE_0, ts->tv_nsec);
+ bcm_phy_write_exp(phydev, TIME_CODE_1, ts->tv_nsec >> 16);
+ bcm_phy_write_exp(phydev, TIME_CODE_2, ts->tv_sec);
+ bcm_phy_write_exp(phydev, TIME_CODE_3, ts->tv_sec >> 16);
+ bcm_phy_write_exp(phydev, TIME_CODE_4, ts->tv_sec >> 32);
+
+ /* set NCO counter to match */
+ ns = timespec64_to_ns(ts);
+ bcm_phy_write_exp(phydev, NCO_TIME_0, ns >> 4);
+ bcm_phy_write_exp(phydev, NCO_TIME_1, ns >> 20);
+ bcm_phy_write_exp(phydev, NCO_TIME_2_CTRL, (ns >> 36) & 0xfff);
+
+ /* set up load on next frame sync (auto-clears due to NSE_INIT) */
+ bcm_phy_write_exp(phydev, SHADOW_LOAD, TIME_CODE_LOAD | NCO_TIME_LOAD);
+
+ /* must have NSE_INIT in order to write time code */
+ return bcm_ptp_framesync(phydev, NULL, priv->nse_ctrl | NSE_INIT);
+}
+
+static int bcm_ptp_settime(struct ptp_clock_info *info,
+ const struct timespec64 *ts)
+{
+ struct bcm_ptp_private *priv = ptp2priv(info);
+ int err;
+
+ mutex_lock(&priv->mutex);
+ err = bcm_ptp_settime_locked(priv, ts);
+ mutex_unlock(&priv->mutex);
+
+ return err;
+}
+
+static int bcm_ptp_adjtime_locked(struct bcm_ptp_private *priv,
+ s64 delta_ns)
+{
+ struct timespec64 ts;
+ int err;
+ s64 ns;
+
+ err = bcm_ptp_gettime_locked(priv, &ts, NULL);
+ if (!err) {
+ ns = timespec64_to_ns(&ts) + delta_ns;
+ ts = ns_to_timespec64(ns);
+ err = bcm_ptp_settime_locked(priv, &ts);
+ }
+ return err;
+}
+
+static int bcm_ptp_adjtime(struct ptp_clock_info *info, s64 delta_ns)
+{
+ struct bcm_ptp_private *priv = ptp2priv(info);
+ int err;
+
+ mutex_lock(&priv->mutex);
+ err = bcm_ptp_adjtime_locked(priv, delta_ns);
+ mutex_unlock(&priv->mutex);
+
+ return err;
+}
+
+/* A 125Mhz clock should adjust 8ns per pulse.
+ * The frequency adjustment base is 0x8000 0000, or 8*2^28.
+ *
+ * Frequency adjustment is
+ * adj = scaled_ppm * 8*2^28 / (10^6 * 2^16)
+ * which simplifies to:
+ * adj = scaled_ppm * 2^9 / 5^6
+ */
+static int bcm_ptp_adjfine(struct ptp_clock_info *info, long scaled_ppm)
+{
+ struct bcm_ptp_private *priv = ptp2priv(info);
+ int neg_adj = 0;
+ u32 diff, freq;
+ u64 adj;
+
+ if (scaled_ppm < 0) {
+ neg_adj = 1;
+ scaled_ppm = -scaled_ppm;
+ }
+
+ adj = scaled_ppm << 9;
+ diff = div_u64(adj, 15625);
+ freq = (8 << 28) + (neg_adj ? -diff : diff);
+
+ mutex_lock(&priv->mutex);
+
+ bcm_phy_write_exp(priv->phydev, NCO_FREQ_LSB, freq);
+ bcm_phy_write_exp(priv->phydev, NCO_FREQ_MSB, freq >> 16);
+
+ bcm_phy_write_exp(priv->phydev, NCO_TIME_2_CTRL, FREQ_MDIO_SEL);
+
+ /* load on next framesync */
+ bcm_phy_write_exp(priv->phydev, SHADOW_LOAD, FREQ_LOAD);
+
+ bcm_ptp_framesync(priv->phydev, NULL, priv->nse_ctrl);
+
+ /* clear load */
+ bcm_phy_write_exp(priv->phydev, SHADOW_LOAD, 0);
+
+ mutex_unlock(&priv->mutex);
+
+ return 0;
+}
+
+/* The output pulse and period are stable, but the signal is not
+ * synchronized to anything.
+ */
+static int bcm_ptp_perout_locked(struct bcm_ptp_private *priv,
+ struct ptp_perout_request *req, int on)
+{
+ struct phy_device *phydev = priv->phydev;
+ u64 period, pulse;
+ u16 val;
+
+ if (!on) {
+ priv->nse_ctrl &= ~NSE_SYNC_OUT_MASK;
+ bcm_phy_write_exp(phydev, NSE_CTRL, priv->nse_ctrl);
+ return 0;
+ }
+
+ if (req->flags & PTP_PEROUT_PHASE)
+ return -EOPNOTSUPP;
+
+ period = ktime_to_ns(ktime_set(req->period.sec, req->period.nsec));
+ if (req->flags & PTP_PEROUT_DUTY_CYCLE)
+ pulse = ktime_to_ns(ktime_set(req->on.sec, req->on.nsec));
+ else
+ pulse = min(period / 2, (u64)BCM_MAX_PULSE_8NS << 3);
+
+ /* convert to 8ns units */
+ pulse >>= 3;
+ period >>= 3;
+
+ if (!pulse || !period)
+ return -EINVAL;
+
+ if (pulse > period)
+ return -EINVAL;
+
+ if (pulse > BCM_MAX_PULSE_8NS || period > BCM_MAX_PERIOD_8NS)
+ return -EINVAL;
+
+ bcm_phy_write_exp(phydev, SYNC_OUT_0, period);
+
+ val = ((pulse & 0x3) << 14) | ((period >> 16) & 0x3fff);
+ bcm_phy_write_exp(phydev, SYNC_OUT_1, val);
+
+ val = ((pulse >> 2) & 0x7f) | (pulse << 7);
+ bcm_phy_write_exp(phydev, SYNC_OUT_2, val);
+
+ /* load values on next framesync */
+ bcm_phy_write_exp(phydev, SHADOW_LOAD, SYNC_OUT_LOAD);
+
+ priv->nse_ctrl |= NSE_PEROUT_EN;
+ return bcm_ptp_framesync(phydev, NULL, priv->nse_ctrl | NSE_INIT);
+}
+
+static void bcm_ptp_fsync_work(struct work_struct *out_work)
+{
+ struct bcm_ptp_private *priv =
+ container_of(out_work, struct bcm_ptp_private, out_work.work);
+ struct phy_device *phydev = priv->phydev;
+ struct timespec64 ts;
+ u64 ns, next;
+
+ mutex_lock(&priv->mutex);
+
+ /* no longer running */
+ if (!priv->fsync_out) {
+ mutex_unlock(&priv->mutex);
+ return;
+ }
+
+ bcm_ptp_gettime_locked(priv, &ts, NULL);
+
+ /* this is 1PPS only */
+ next = NSEC_PER_SEC - ts.tv_nsec;
+ ts.tv_sec += next < NSEC_PER_MSEC ? 2 : 1;
+ ts.tv_nsec = 0;
+
+ ns = timespec64_to_ns(&ts);
+
+ bcm_phy_write_exp(phydev, SYNOUT_TS_0, ns & 0xfff0);
+ bcm_phy_write_exp(phydev, SYNOUT_TS_1, ns >> 16);
+ bcm_phy_write_exp(phydev, SYNOUT_TS_2, ns >> 32);
+
+ /* load values on next framesync */
+ bcm_phy_write_exp(phydev, SHADOW_LOAD, SYNC_OUT_LOAD);
+
+ /* disable existing pulse - oneshot needs toggle. */
+ if (priv->nse_ctrl & NSE_SYNC_OUT_MASK) {
+ priv->nse_ctrl &= ~NSE_SYNC_OUT_MASK;
+ bcm_phy_write_exp(phydev, NSE_CTRL, priv->nse_ctrl);
+ }
+
+ priv->nse_ctrl |= NSE_ONESHOT_EN;
+ bcm_ptp_framesync(phydev, NULL, priv->nse_ctrl | NSE_INIT);
+
+ mutex_unlock(&priv->mutex);
+
+ next = next + NSEC_PER_MSEC;
+ schedule_delayed_work(&priv->out_work, nsecs_to_jiffies(next));
+}
+
+static int bcm_ptp_fsync_locked(struct bcm_ptp_private *priv,
+ struct ptp_perout_request *req, int on)
+{
+ struct phy_device *phydev = priv->phydev;
+ u64 period, pulse;
+ u16 val;
+
+ if (!on) {
+ priv->fsync_out = false;
+ priv->nse_ctrl &= ~NSE_SYNC_OUT_MASK;
+ bcm_phy_write_exp(phydev, NSE_CTRL, priv->nse_ctrl);
+ return 0;
+ }
+
+ /* 1PPS for now */
+ if (req->period.sec != 1 || req->period.nsec != 0)
+ return -EINVAL;
+
+ if (req->flags & PTP_PEROUT_PHASE)
+ return -EOPNOTSUPP;
+
+ period = ktime_to_ns(ktime_set(req->period.sec, req->period.nsec));
+ if (req->flags & PTP_PEROUT_DUTY_CYCLE)
+ pulse = ktime_to_ns(ktime_set(req->on.sec, req->on.nsec));
+ else
+ pulse = min(period / 2, (u64)BCM_MAX_PULSE_8NS << 3);
+
+ /* convert to 8ns units */
+ pulse >>= 3;
+ period >>= 3;
+
+ if (!pulse || !period)
+ return -EINVAL;
+
+ if (pulse > period)
+ return -EINVAL;
+
+ if (pulse > BCM_MAX_PULSE_8NS || period > BCM_MAX_PERIOD_8NS)
+ return -EINVAL;
+
+ bcm_phy_write_exp(phydev, SYNC_OUT_0, period);
+
+ val = ((pulse & 0x3) << 14) | ((period >> 16) & 0x3fff);
+ bcm_phy_write_exp(phydev, SYNC_OUT_1, val);
+
+ val = ((pulse >> 2) & 0x7f) | (pulse << 7);
+ bcm_phy_write_exp(phydev, SYNC_OUT_2, val);
+
+ priv->fsync_out = true;
+ schedule_delayed_work(&priv->out_work, 0);
+
+ return 0;
+}
+
+static bool bcm_ptp_rxtstamp(struct mii_timestamper *mii_ts,
+ struct sk_buff *skb, int type)
+{
+ struct bcm_ptp_private *priv = mii2priv(mii_ts);
+ struct skb_shared_hwtstamps *hwts;
+ struct ptp_header *header;
+ u32 sec, nsec;
+ u8 *data;
+ int off;
+
+ if (!priv->hwts_rx)
+ return false;
+
+ header = ptp_parse_header(skb, type);
+ if (!header)
+ return false;
+
+ data = (u8 *)(header + 1);
+ sec = get_unaligned_be32(data);
+ nsec = get_unaligned_be32(data + 4);
+
+ hwts = skb_hwtstamps(skb);
+ hwts->hwtstamp = ktime_set(sec, nsec);
+
+ off = data - skb->data + 8;
+ if (off < skb->len) {
+ memmove(data, data + 8, skb->len - off);
+ __pskb_trim(skb, skb->len - 8);
+ }
+
+ return false;
+}
+
+static bool bcm_ptp_get_tstamp(struct bcm_ptp_private *priv,
+ struct bcm_ptp_capture *capts)
+{
+ struct phy_device *phydev = priv->phydev;
+ u16 ts[4], reg;
+ u32 sec, nsec;
+
+ mutex_lock(&priv->mutex);
+
+ reg = bcm_phy_read_exp(phydev, INTR_STATUS);
+ if ((reg & INTC_SOP) == 0) {
+ mutex_unlock(&priv->mutex);
+ return false;
+ }
+
+ bcm_phy_write_exp(phydev, TS_READ_CTRL, TS_READ_START);
+
+ ts[0] = bcm_phy_read_exp(phydev, TS_REG_0);
+ ts[1] = bcm_phy_read_exp(phydev, TS_REG_1);
+ ts[2] = bcm_phy_read_exp(phydev, TS_REG_2);
+ ts[3] = bcm_phy_read_exp(phydev, TS_REG_3);
+
+ /* not in be32 format for some reason */
+ capts->seq_id = bcm_phy_read_exp(priv->phydev, TS_INFO_0);
+
+ reg = bcm_phy_read_exp(phydev, TS_INFO_1);
+ capts->msgtype = reg >> 12;
+ capts->tx_dir = !!(reg & BIT(11));
+
+ bcm_phy_write_exp(phydev, TS_READ_CTRL, TS_READ_END);
+ bcm_phy_write_exp(phydev, TS_READ_CTRL, 0);
+
+ mutex_unlock(&priv->mutex);
+
+ sec = (ts[3] << 16) | ts[2];
+ nsec = (ts[1] << 16) | ts[0];
+ capts->hwtstamp = ktime_set(sec, nsec);
+
+ return true;
+}
+
+static void bcm_ptp_match_tstamp(struct bcm_ptp_private *priv,
+ struct bcm_ptp_capture *capts)
+{
+ struct skb_shared_hwtstamps hwts;
+ struct sk_buff *skb, *ts_skb;
+ unsigned long flags;
+ bool first = false;
+
+ ts_skb = NULL;
+ spin_lock_irqsave(&priv->tx_queue.lock, flags);
+ skb_queue_walk(&priv->tx_queue, skb) {
+ if (BCM_SKB_CB(skb)->seq_id == capts->seq_id &&
+ BCM_SKB_CB(skb)->msgtype == capts->msgtype) {
+ first = skb_queue_is_first(&priv->tx_queue, skb);
+ __skb_unlink(skb, &priv->tx_queue);
+ ts_skb = skb;
+ break;
+ }
+ }
+ spin_unlock_irqrestore(&priv->tx_queue.lock, flags);
+
+ /* TX captures one-step packets, discard them if needed. */
+ if (ts_skb) {
+ if (BCM_SKB_CB(ts_skb)->discard) {
+ kfree_skb(ts_skb);
+ } else {
+ memset(&hwts, 0, sizeof(hwts));
+ hwts.hwtstamp = capts->hwtstamp;
+ skb_complete_tx_timestamp(ts_skb, &hwts);
+ }
+ }
+
+ /* not first match, try and expire entries */
+ if (!first) {
+ while ((skb = skb_dequeue(&priv->tx_queue))) {
+ if (!time_after(jiffies, BCM_SKB_CB(skb)->timeout)) {
+ skb_queue_head(&priv->tx_queue, skb);
+ break;
+ }
+ kfree_skb(skb);
+ }
+ }
+}
+
+static long bcm_ptp_do_aux_work(struct ptp_clock_info *info)
+{
+ struct bcm_ptp_private *priv = ptp2priv(info);
+ struct bcm_ptp_capture capts;
+ bool reschedule = false;
+
+ while (!skb_queue_empty_lockless(&priv->tx_queue)) {
+ if (!bcm_ptp_get_tstamp(priv, &capts)) {
+ reschedule = true;
+ break;
+ }
+ bcm_ptp_match_tstamp(priv, &capts);
+ }
+
+ return reschedule ? 1 : -1;
+}
+
+static int bcm_ptp_enable(struct ptp_clock_info *info,
+ struct ptp_clock_request *rq, int on)
+{
+ struct bcm_ptp_private *priv = ptp2priv(info);
+ int err = 0;
+
+ switch (rq->type) {
+ case PTP_CLK_REQ_PEROUT:
+ mutex_lock(&priv->mutex);
+ if (rq->perout.index == 0)
+ err = bcm_ptp_perout_locked(priv, &rq->perout, on);
+ else
+ err = bcm_ptp_fsync_locked(priv, &rq->perout, on);
+ mutex_unlock(&priv->mutex);
+ break;
+ default:
+ return -EOPNOTSUPP;
+ }
+ return err;
+}
+
+static const struct ptp_clock_info bcm_ptp_clock_info = {
+ .owner = THIS_MODULE,
+ .name = KBUILD_MODNAME,
+ .max_adj = 100000000,
+ .gettimex64 = bcm_ptp_gettimex,
+ .settime64 = bcm_ptp_settime,
+ .adjtime = bcm_ptp_adjtime,
+ .adjfine = bcm_ptp_adjfine,
+ .enable = bcm_ptp_enable,
+ .do_aux_work = bcm_ptp_do_aux_work,
+ .n_per_out = 2,
+};
+
+static void bcm_ptp_txtstamp(struct mii_timestamper *mii_ts,
+ struct sk_buff *skb, int type)
+{
+ struct bcm_ptp_private *priv = mii2priv(mii_ts);
+ struct ptp_header *hdr;
+ bool discard = false;
+ int msgtype;
+
+ hdr = ptp_parse_header(skb, type);
+ if (!hdr)
+ goto out;
+ msgtype = ptp_get_msgtype(hdr, type);
+
+ switch (priv->tx_type) {
+ case HWTSTAMP_TX_ONESTEP_P2P:
+ if (msgtype == PTP_MSGTYPE_PDELAY_RESP)
+ discard = true;
+ fallthrough;
+ case HWTSTAMP_TX_ONESTEP_SYNC:
+ if (msgtype == PTP_MSGTYPE_SYNC)
+ discard = true;
+ fallthrough;
+ case HWTSTAMP_TX_ON:
+ BCM_SKB_CB(skb)->timeout = jiffies + SKB_TS_TIMEOUT;
+ BCM_SKB_CB(skb)->seq_id = be16_to_cpu(hdr->sequence_id);
+ BCM_SKB_CB(skb)->msgtype = msgtype;
+ BCM_SKB_CB(skb)->discard = discard;
+ skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
+ skb_queue_tail(&priv->tx_queue, skb);
+ ptp_schedule_worker(priv->ptp_clock, 0);
+ return;
+ default:
+ break;
+ }
+
+out:
+ kfree_skb(skb);
+}
+
+static int bcm_ptp_hwtstamp(struct mii_timestamper *mii_ts,
+ struct ifreq *ifr)
+{
+ struct bcm_ptp_private *priv = mii2priv(mii_ts);
+ struct hwtstamp_config cfg;
+ u16 mode, ctrl;
+
+ if (copy_from_user(&cfg, ifr->ifr_data, sizeof(cfg)))
+ return -EFAULT;
+
+ switch (cfg.rx_filter) {
+ case HWTSTAMP_FILTER_NONE:
+ priv->hwts_rx = false;
+ break;
+ case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
+ case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
+ case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
+ case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
+ case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
+ case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
+ case HWTSTAMP_FILTER_PTP_V2_EVENT:
+ case HWTSTAMP_FILTER_PTP_V2_SYNC:
+ case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
+ priv->hwts_rx = true;
+ break;
+ default:
+ return -ERANGE;
+ }
+
+ priv->tx_type = cfg.tx_type;
+
+ ctrl = priv->hwts_rx ? SLICE_RX_EN : 0;
+ ctrl |= priv->tx_type != HWTSTAMP_TX_OFF ? SLICE_TX_EN : 0;
+
+ mode = TX_MODE_SEL(PORT, SYNC, REPLACE_TS) |
+ TX_MODE_SEL(PORT, DELAY_REQ, REPLACE_TS) |
+ TX_MODE_SEL(PORT, PDELAY_REQ, REPLACE_TS) |
+ TX_MODE_SEL(PORT, PDELAY_RESP, REPLACE_TS);
+
+ bcm_phy_write_exp(priv->phydev, TX_EVENT_MODE, mode);
+
+ mode = RX_MODE_SEL(PORT, SYNC, INSERT_TS_64) |
+ RX_MODE_SEL(PORT, DELAY_REQ, INSERT_TS_64) |
+ RX_MODE_SEL(PORT, PDELAY_REQ, INSERT_TS_64) |
+ RX_MODE_SEL(PORT, PDELAY_RESP, INSERT_TS_64);
+
+ bcm_phy_write_exp(priv->phydev, RX_EVENT_MODE, mode);
+
+ bcm_phy_write_exp(priv->phydev, SLICE_CTRL, ctrl);
+
+ if (ctrl & SLICE_TX_EN)
+ bcm_phy_write_exp(priv->phydev, TX_TS_CAPTURE, TX_TS_CAP_EN);
+ else
+ ptp_cancel_worker_sync(priv->ptp_clock);
+
+ /* purge existing data */
+ skb_queue_purge(&priv->tx_queue);
+
+ return copy_to_user(ifr->ifr_data, &cfg, sizeof(cfg)) ? -EFAULT : 0;
+}
+
+static int bcm_ptp_ts_info(struct mii_timestamper *mii_ts,
+ struct ethtool_ts_info *ts_info)
+{
+ struct bcm_ptp_private *priv = mii2priv(mii_ts);
+
+ ts_info->phc_index = ptp_clock_index(priv->ptp_clock);
+ ts_info->so_timestamping =
+ SOF_TIMESTAMPING_TX_HARDWARE |
+ SOF_TIMESTAMPING_RX_HARDWARE |
+ SOF_TIMESTAMPING_RAW_HARDWARE;
+ ts_info->tx_types =
+ BIT(HWTSTAMP_TX_ON) |
+ BIT(HWTSTAMP_TX_OFF) |
+ BIT(HWTSTAMP_TX_ONESTEP_SYNC) |
+ BIT(HWTSTAMP_TX_ONESTEP_P2P);
+ ts_info->rx_filters =
+ BIT(HWTSTAMP_FILTER_NONE) |
+ BIT(HWTSTAMP_FILTER_PTP_V2_L2_EVENT) |
+ BIT(HWTSTAMP_FILTER_PTP_V2_L4_EVENT);
+
+ return 0;
+}
+
+void bcm_ptp_config_init(struct phy_device *phydev)
+{
+ /* init network sync engine */
+ bcm_phy_write_exp(phydev, NSE_CTRL, NSE_GMODE_EN | NSE_INIT);
+
+ /* enable time sync (TX/RX SOP capture) */
+ bcm_phy_write_exp(phydev, TIME_SYNC, TIME_SYNC_EN);
+
+ /* use sec.nsec heartbeat capture */
+ bcm_phy_write_exp(phydev, DPLL_SELECT, DPLL_HB_MODE2);
+
+ /* use 64 bit timecode for TX */
+ bcm_phy_write_exp(phydev, TIMECODE_CTRL, TX_TIMECODE_SEL);
+
+ /* always allow FREQ_LOAD on framesync */
+ bcm_phy_write_exp(phydev, SHADOW_CTRL, FREQ_LOAD);
+}
+EXPORT_SYMBOL_GPL(bcm_ptp_config_init);
+
+static void bcm_ptp_init(struct bcm_ptp_private *priv)
+{
+ priv->nse_ctrl = NSE_GMODE_EN;
+
+ mutex_init(&priv->mutex);
+ skb_queue_head_init(&priv->tx_queue);
+
+ priv->mii_ts.rxtstamp = bcm_ptp_rxtstamp;
+ priv->mii_ts.txtstamp = bcm_ptp_txtstamp;
+ priv->mii_ts.hwtstamp = bcm_ptp_hwtstamp;
+ priv->mii_ts.ts_info = bcm_ptp_ts_info;
+
+ priv->phydev->mii_ts = &priv->mii_ts;
+
+ INIT_DELAYED_WORK(&priv->out_work, bcm_ptp_fsync_work);
+}
+
+struct bcm_ptp_private *bcm_ptp_probe(struct phy_device *phydev)
+{
+ struct bcm_ptp_private *priv;
+ struct ptp_clock *clock;
+
+ switch (BRCM_PHY_MODEL(phydev)) {
+ case PHY_ID_BCM54210E:
+ break;
+ default:
+ return NULL;
+ }
+
+ priv = devm_kzalloc(&phydev->mdio.dev, sizeof(*priv), GFP_KERNEL);
+ if (!priv)
+ return ERR_PTR(-ENOMEM);
+
+ priv->ptp_info = bcm_ptp_clock_info;
+
+ clock = ptp_clock_register(&priv->ptp_info, &phydev->mdio.dev);
+ if (IS_ERR(clock))
+ return ERR_CAST(clock);
+ priv->ptp_clock = clock;
+
+ priv->phydev = phydev;
+ bcm_ptp_init(priv);
+
+ return priv;
+}
+EXPORT_SYMBOL_GPL(bcm_ptp_probe);
+
+MODULE_LICENSE("GPL");
--
2.31.1
^ permalink raw reply related [flat|nested] 6+ messages in thread