Adding upstream version 6.6.15.upstream/6.6.15

Signed-off-by: Daniel Baumann <daniel.baumann@progress-linux.org>

1 files changed, 944 insertions, 0 deletions

diff --git a/drivers/net/phy/bcm-phy-ptp.c b/drivers/net/phy/bcm-phy-ptp.c
new file mode 100644
index 0000000000..ef00d61630
--- /dev/null
+++ b/drivers/net/phy/bcm-phy-ptp.c
@@ -0,0 +1,944 @@
+// 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 SYNC_IN_DIVIDER		0x087b
+
+#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_SYNC1_FRAMESYNC		BIT(3)
+#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 ptp_pin_desc pin;
+	struct mutex mutex;
+	struct sk_buff_head tx_queue;
+	int tx_type;
+	bool hwts_rx;
+	u16 nse_ctrl;
+	bool pin_active;
+	struct delayed_work pin_work;
+};
+
+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 void bcm_ptp_get_framesync_ts(struct phy_device *phydev,
+				     struct timespec64 *ts)
+{
+	u16 hb[4];
+
+	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];
+}
+
+static u16 bcm_ptp_framesync_disable(struct phy_device *phydev, u16 orig_ctrl)
+{
+	u16 ctrl = orig_ctrl & ~(NSE_FRAMESYNC_MASK | NSE_CAPTURE_EN);
+
+	bcm_phy_write_exp(phydev, NSE_CTRL, ctrl);
+
+	return ctrl;
+}
+
+static void bcm_ptp_framesync_restore(struct phy_device *phydev, u16 orig_ctrl)
+{
+	if (orig_ctrl & NSE_FRAMESYNC_MASK)
+		bcm_phy_write_exp(phydev, NSE_CTRL, orig_ctrl);
+}
+
+static void bcm_ptp_framesync(struct phy_device *phydev, u16 ctrl)
+{
+	/* trigger framesync - must have 0->1 transition. */
+	bcm_phy_write_exp(phydev, NSE_CTRL, ctrl | NSE_CPU_FRAMESYNC);
+}
+
+static int bcm_ptp_framesync_ts(struct phy_device *phydev,
+				struct ptp_system_timestamp *sts,
+				struct timespec64 *ts,
+				u16 orig_ctrl)
+{
+	u16 ctrl, reg;
+	int i;
+
+	ctrl = bcm_ptp_framesync_disable(phydev, orig_ctrl);
+
+	ptp_read_system_prets(sts);
+
+	/* trigger framesync + capture */
+	bcm_ptp_framesync(phydev, ctrl | NSE_CAPTURE_EN);
+
+	ptp_read_system_postts(sts);
+
+	/* 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) {
+			bcm_ptp_get_framesync_ts(phydev, ts);
+			break;
+		}
+	}
+
+	bcm_ptp_framesync_restore(phydev, orig_ctrl);
+
+	return reg & INTC_FSYNC ? 0 : -ETIMEDOUT;
+}
+
+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_framesync_ts(priv->phydev, sts, ts, priv->nse_ctrl);
+	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;
+	u16 ctrl;
+	u64 ns;
+
+	ctrl = bcm_ptp_framesync_disable(phydev, priv->nse_ctrl);
+
+	/* 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 */
+	bcm_ptp_framesync(phydev, ctrl | NSE_INIT);
+
+	bcm_ptp_framesync_restore(phydev, priv->nse_ctrl);
+
+	return 0;
+}
+
+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_framesync_ts(priv->phydev, NULL, &ts, priv->nse_ctrl);
+	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;
+	u16 ctrl;
+	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);
+
+	ctrl = bcm_ptp_framesync_disable(priv->phydev, priv->nse_ctrl);
+
+	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, ctrl);
+
+	/* clear load */
+	bcm_phy_write_exp(priv->phydev, SHADOW_LOAD, 0);
+
+	bcm_ptp_framesync_restore(priv->phydev, priv->nse_ctrl);
+
+	mutex_unlock(&priv->mutex);
+
+	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_cancel_func(struct bcm_ptp_private *priv)
+{
+	if (!priv->pin_active)
+		return 0;
+
+	priv->pin_active = false;
+
+	priv->nse_ctrl &= ~(NSE_SYNC_OUT_MASK | NSE_SYNC1_FRAMESYNC |
+			    NSE_CAPTURE_EN);
+	bcm_phy_write_exp(priv->phydev, NSE_CTRL, priv->nse_ctrl);
+
+	cancel_delayed_work_sync(&priv->pin_work);
+
+	return 0;
+}
+
+static void bcm_ptp_perout_work(struct work_struct *pin_work)
+{
+	struct bcm_ptp_private *priv =
+		container_of(pin_work, struct bcm_ptp_private, pin_work.work);
+	struct phy_device *phydev = priv->phydev;
+	struct timespec64 ts;
+	u64 ns, next;
+	u16 ctrl;
+
+	mutex_lock(&priv->mutex);
+
+	/* no longer running */
+	if (!priv->pin_active) {
+		mutex_unlock(&priv->mutex);
+		return;
+	}
+
+	bcm_ptp_framesync_ts(phydev, NULL, &ts, priv->nse_ctrl);
+
+	/* 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);
+
+	/* force 0->1 transition for ONESHOT */
+	ctrl = bcm_ptp_framesync_disable(phydev,
+					 priv->nse_ctrl & ~NSE_ONESHOT_EN);
+
+	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);
+
+	bcm_ptp_framesync(phydev, ctrl | NSE_ONESHOT_EN | NSE_INIT);
+
+	priv->nse_ctrl |= NSE_ONESHOT_EN;
+	bcm_ptp_framesync_restore(phydev, priv->nse_ctrl);
+
+	mutex_unlock(&priv->mutex);
+
+	next = next + NSEC_PER_MSEC;
+	schedule_delayed_work(&priv->pin_work, nsecs_to_jiffies(next));
+}
+
+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)
+		return bcm_ptp_cancel_func(priv);
+
+	/* 1PPS */
+	if (req->period.sec != 1 || req->period.nsec != 0)
+		return -EINVAL;
+
+	period = BCM_MAX_PERIOD_8NS;	/* write nonzero value */
+
+	if (req->flags & PTP_PEROUT_PHASE)
+		return -EOPNOTSUPP;
+
+	if (req->flags & PTP_PEROUT_DUTY_CYCLE)
+		pulse = ktime_to_ns(ktime_set(req->on.sec, req->on.nsec));
+	else
+		pulse = (u64)BCM_MAX_PULSE_8NS << 3;
+
+	/* convert to 8ns units */
+	pulse >>= 3;
+
+	if (!pulse || pulse > period || pulse > BCM_MAX_PULSE_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);
+
+	if (priv->pin_active)
+		cancel_delayed_work_sync(&priv->pin_work);
+
+	priv->pin_active = true;
+	INIT_DELAYED_WORK(&priv->pin_work, bcm_ptp_perout_work);
+	schedule_delayed_work(&priv->pin_work, 0);
+
+	return 0;
+}
+
+static void bcm_ptp_extts_work(struct work_struct *pin_work)
+{
+	struct bcm_ptp_private *priv =
+		container_of(pin_work, struct bcm_ptp_private, pin_work.work);
+	struct phy_device *phydev = priv->phydev;
+	struct ptp_clock_event event;
+	struct timespec64 ts;
+	u16 reg;
+
+	mutex_lock(&priv->mutex);
+
+	/* no longer running */
+	if (!priv->pin_active) {
+		mutex_unlock(&priv->mutex);
+		return;
+	}
+
+	reg = bcm_phy_read_exp(phydev, INTR_STATUS);
+	if ((reg & INTC_FSYNC) == 0)
+		goto out;
+
+	bcm_ptp_get_framesync_ts(phydev, &ts);
+
+	event.index = 0;
+	event.type = PTP_CLOCK_EXTTS;
+	event.timestamp = timespec64_to_ns(&ts);
+	ptp_clock_event(priv->ptp_clock, &event);
+
+out:
+	mutex_unlock(&priv->mutex);
+	schedule_delayed_work(&priv->pin_work, HZ / 4);
+}
+
+static int bcm_ptp_extts_locked(struct bcm_ptp_private *priv, int on)
+{
+	struct phy_device *phydev = priv->phydev;
+
+	if (!on)
+		return bcm_ptp_cancel_func(priv);
+
+	if (priv->pin_active)
+		cancel_delayed_work_sync(&priv->pin_work);
+
+	bcm_ptp_framesync_disable(phydev, priv->nse_ctrl);
+
+	priv->nse_ctrl |= NSE_SYNC1_FRAMESYNC | NSE_CAPTURE_EN;
+
+	bcm_ptp_framesync_restore(phydev, priv->nse_ctrl);
+
+	priv->pin_active = true;
+	INIT_DELAYED_WORK(&priv->pin_work, bcm_ptp_extts_work);
+	schedule_delayed_work(&priv->pin_work, 0);
+
+	return 0;
+}
+
+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 = -EBUSY;
+
+	mutex_lock(&priv->mutex);
+
+	switch (rq->type) {
+	case PTP_CLK_REQ_PEROUT:
+		if (priv->pin.func == PTP_PF_PEROUT)
+			err = bcm_ptp_perout_locked(priv, &rq->perout, on);
+		break;
+	case PTP_CLK_REQ_EXTTS:
+		if (priv->pin.func == PTP_PF_EXTTS)
+			err = bcm_ptp_extts_locked(priv, on);
+		break;
+	default:
+		err = -EOPNOTSUPP;
+		break;
+	}
+
+	mutex_unlock(&priv->mutex);
+
+	return err;
+}
+
+static int bcm_ptp_verify(struct ptp_clock_info *info, unsigned int pin,
+			  enum ptp_pin_function func, unsigned int chan)
+{
+	switch (func) {
+	case PTP_PF_NONE:
+	case PTP_PF_EXTTS:
+	case PTP_PF_PEROUT:
+		break;
+	default:
+		return -EOPNOTSUPP;
+	}
+	return 0;
+}
+
+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,
+	.verify		= bcm_ptp_verify,
+	.do_aux_work	= bcm_ptp_do_aux_work,
+	.n_pins		= 1,
+	.n_per_out	= 1,
+	.n_ext_ts	= 1,
+};
+
+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:
+		cfg.rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
+		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_EVENT);
+
+	return 0;
+}
+
+void bcm_ptp_stop(struct bcm_ptp_private *priv)
+{
+	ptp_cancel_worker_sync(priv->ptp_clock);
+	bcm_ptp_cancel_func(priv);
+}
+EXPORT_SYMBOL_GPL(bcm_ptp_stop);
+
+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);
+
+	bcm_phy_write_exp(phydev, SYNC_IN_DIVIDER, 1);
+}
+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;
+}
+
+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;
+
+	snprintf(priv->pin.name, sizeof(priv->pin.name), "SYNC_OUT");
+	priv->ptp_info.pin_config = &priv->pin;
+
+	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");