[PATCH net-next v1 0/4] Broadcom PTP PHY support

[PATCH net-next v1 0/4] Broadcom PTP PHY support

[Jonathan Lemon]

This adds PTP support for the Broadcom PHY BCM54210E (and the
specific variant BCM54213PE that the rpi-5.15 branch uses).

This has only been tested on the RPI CM4, which has one port.

There are other Broadcom chips which may benefit from using the 
same framework here, although with different register sets.

Jonathan Lemon (4):
  net: phy: broadcom: Add PTP support for some Broadcom PHYs.
  net: phy: broadcom: Add Broadcom PTP hooks to bcm-phy-lib
  net: phy: broadcom: Hook up the PTP PHY functions
  net: phy: Kconfig: Add broadcom PTP PHY library

 drivers/net/phy/Kconfig       |  10 +
 drivers/net/phy/Makefile      |   1 +
 drivers/net/phy/bcm-phy-lib.c |  13 +
 drivers/net/phy/bcm-phy-lib.h |   3 +
 drivers/net/phy/bcm-phy-ptp.c | 736 ++++++++++++++++++++++++++++++++++
 drivers/net/phy/broadcom.c    |  23 +-
 6 files changed, 782 insertions(+), 4 deletions(-)
 create mode 100644 drivers/net/phy/bcm-phy-ptp.c

-- 
2.31.1

[PATCH net-next v1 1/4] net: phy: broadcom: Add PTP support for some Broadcom PHYs.

[Jonathan Lemon]

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/bcm-phy-ptp.c | 736 ++++++++++++++++++++++++++++++++++
 1 file changed, 736 insertions(+)
 create mode 100644 drivers/net/phy/bcm-phy-ptp.c

diff --git a/drivers/net/phy/bcm-phy-ptp.c b/drivers/net/phy/bcm-phy-ptp.c
new file mode 100644
index 000000000000..64c2c96dcad4
--- /dev/null
+++ b/drivers/net/phy/bcm-phy-ptp.c
@@ -0,0 +1,736 @@
+// 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 "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, evt, act) \
+	(((MODE_RX_##act) << (MODE_EVT_SHIFT_##evt)) << (MODE_SEL_SHIFT_##sel))
+
+#define tx_mode(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 FREQ_REG_LSB		0x0873
+#define FREQ_REG_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 TS_READ_CTRL		0x0885
+#define  TS_READ_START			BIT(0)
+#define  TS_READ_END			BIT(1)
+
+#define TIMECODE_CTRL		0x08c3
+#define  TX_TIMECODE_SEL		GENMASK(7, 0)
+#define  RX_TIMECODE_SEL		GENMASK(15, 8)
+
+#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 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 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_SYNC_OUT_MASK		GENMASK(1, 0)
+
+#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 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], ctrl;
+	int err;
+
+	ctrl = priv->nse_ctrl;
+	err = bcm_ptp_framesync(phydev, sts, 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;
+	u16 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);
+
+	/* zero out NCO counter */
+	bcm_phy_write_exp(phydev, NCO_TIME_0, 0);
+	bcm_phy_write_exp(phydev, NCO_TIME_1, 0);
+	bcm_phy_write_exp(phydev, NCO_TIME_2_CTRL, 0);
+
+	/* set up load on next frame sync */
+	bcm_phy_write_exp(phydev, SHADOW_LOAD, TIME_CODE_LOAD | NCO_TIME_LOAD);
+
+	ctrl = priv->nse_ctrl;
+	return bcm_ptp_framesync(phydev, NULL, 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;
+
+	err = bcm_ptp_gettime_locked(priv, &ts, NULL);
+	if (!err) {
+		timespec64_add_ns(&ts, delta_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, FREQ_REG_LSB, freq);
+	bcm_phy_write_exp(priv->phydev, FREQ_REG_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);
+
+	mutex_unlock(&priv->mutex);
+
+	return 0;
+}
+
+static int bcm_ptp_perout_locked(struct bcm_ptp_private *priv,
+				 struct ptp_perout_request *req, int on)
+{
+	u64 period, pulse;
+	u16 val;
+
+	if (!on) {
+		priv->nse_ctrl &= ~NSE_SYNC_OUT_MASK;
+		bcm_phy_write_exp(priv->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(priv->phydev, SYNC_OUT_0, period);
+
+	val = ((pulse & 0x3) << 14) | ((period >> 16) & 0x3fff);
+	bcm_phy_write_exp(priv->phydev, SYNC_OUT_1, val);
+
+	val = (pulse >> 2) & 0x7f;
+	bcm_phy_write_exp(priv->phydev, SYNC_OUT_2, val);
+
+	/* load values on next framesync */
+	bcm_phy_write_exp(priv->phydev, SHADOW_LOAD, SYNC_OUT_LOAD);
+
+	priv->nse_ctrl |= NSE_PEROUT_EN;
+	return bcm_ptp_framesync(priv->phydev, NULL, priv->nse_ctrl);
+}
+
+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);
+		err = bcm_ptp_perout_locked(priv, &rq->perout, on);
+		mutex_unlock(&priv->mutex);
+		break;
+	default:
+		return -EOPNOTSUPP;
+	}
+	return err;
+}
+
+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;
+
+	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);
+
+	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 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	= 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:
+		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(PORT, SYNC, REPLACE_TS) |
+	       tx_mode(PORT, DELAY_REQ, REPLACE_TS) |
+	       tx_mode(PORT, PDELAY_REQ, REPLACE_TS) |
+	       tx_mode(PORT, PDELAY_RESP, REPLACE_TS);
+
+	bcm_phy_write_exp(priv->phydev, TX_EVENT_MODE, mode);
+
+	mode = rx_mode(PORT, SYNC, INSERT_TS_64) |
+	       rx_mode(PORT, DELAY_REQ, INSERT_TS_64) |
+	       rx_mode(PORT, PDELAY_REQ, INSERT_TS_64) |
+	       rx_mode(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 in TX */
+	bcm_phy_write_exp(phydev, TIMECODE_CTRL, TX_TIMECODE_SEL);
+}
+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:
+#ifdef PHY_ID_BCM54213PE
+	case PHY_ID_BCM54213PE:
+#endif
+		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 (void *)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

[PATCH net-next v1 2/4] net: phy: broadcom: Add Broadcom PTP hooks to bcm-phy-lib

[Jonathan Lemon]

Add the public bcm_ptp_probe() and bcm_ptp_config_init() functions
to the bcm-phy library.  The PTP functions are contained in a separate
file for clarity, and also to simplify the PTP clock dependencies.

Signed-off-by: Jonathan Lemon <jonathan.lemon@gmail.com>
---
 drivers/net/phy/bcm-phy-lib.c | 13 +++++++++++++
 drivers/net/phy/bcm-phy-lib.h |  3 +++
 2 files changed, 16 insertions(+)

diff --git a/drivers/net/phy/bcm-phy-lib.c b/drivers/net/phy/bcm-phy-lib.c
index 287cccf8f7f4..b9d2d1d48402 100644
--- a/drivers/net/phy/bcm-phy-lib.c
+++ b/drivers/net/phy/bcm-phy-lib.c
@@ -816,6 +816,19 @@ int bcm_phy_cable_test_get_status_rdb(struct phy_device *phydev,
 }
 EXPORT_SYMBOL_GPL(bcm_phy_cable_test_get_status_rdb);
 
+#if !IS_ENABLED(CONFIG_BCM_NET_PHYPTP)
+struct bcm_ptp_private *bcm_ptp_probe(struct phy_device *phydev)
+{
+	return NULL;
+}
+EXPORT_SYMBOL_GPL(bcm_ptp_probe);
+
+void bcm_ptp_config_init(struct phy_device *phydev)
+{
+}
+EXPORT_SYMBOL_GPL(bcm_ptp_config_init);
+#endif
+
 MODULE_DESCRIPTION("Broadcom PHY Library");
 MODULE_LICENSE("GPL v2");
 MODULE_AUTHOR("Broadcom Corporation");
diff --git a/drivers/net/phy/bcm-phy-lib.h b/drivers/net/phy/bcm-phy-lib.h
index c3842f87c33b..66fa731554a3 100644
--- a/drivers/net/phy/bcm-phy-lib.h
+++ b/drivers/net/phy/bcm-phy-lib.h
@@ -87,4 +87,7 @@ int bcm_phy_cable_test_start_rdb(struct phy_device *phydev);
 int bcm_phy_cable_test_start(struct phy_device *phydev);
 int bcm_phy_cable_test_get_status(struct phy_device *phydev, bool *finished);
 
+struct bcm_ptp_private *bcm_ptp_probe(struct phy_device *phydev);
+void bcm_ptp_config_init(struct phy_device *phydev);
+
 #endif /* _LINUX_BCM_PHY_LIB_H */
-- 
2.31.1

[PATCH net-next v1 3/4] net: phy: broadcom: Hook up the PTP PHY functions

[Jonathan Lemon]

Add 'struct bcm_ptp_private' to bcm54xx_phy_priv which points to
an optional PTP structure attached to the PHY.  This is allocated
on probe, if PHY PTP support is configured, and if the PHY has a
PTP supported by the driver.

Signed-off-by: Jonathan Lemon <jonathan.lemon@gmail.com>
---
 drivers/net/phy/broadcom.c | 23 +++++++++++++++++++----
 1 file changed, 19 insertions(+), 4 deletions(-)

diff --git a/drivers/net/phy/broadcom.c b/drivers/net/phy/broadcom.c
index e36809aa6d30..a7722599b5f9 100644
--- a/drivers/net/phy/broadcom.c
+++ b/drivers/net/phy/broadcom.c
@@ -27,6 +27,11 @@ MODULE_DESCRIPTION("Broadcom PHY driver");
 MODULE_AUTHOR("Maciej W. Rozycki");
 MODULE_LICENSE("GPL");
 
+struct bcm54xx_phy_priv {
+	u64	*stats;
+	struct bcm_ptp_private *ptp;
+};
+
 static int bcm54xx_config_clock_delay(struct phy_device *phydev)
 {
 	int rc, val;
@@ -313,6 +318,14 @@ static void bcm54xx_adjust_rxrefclk(struct phy_device *phydev)
 		bcm_phy_write_shadow(phydev, BCM54XX_SHD_APD, val);
 }
 
+static void bcm54xx_ptp_config_init(struct phy_device *phydev)
+{
+	struct bcm54xx_phy_priv *priv = phydev->priv;
+
+	if (priv->ptp)
+		bcm_ptp_config_init(phydev);
+}
+
 static int bcm54xx_config_init(struct phy_device *phydev)
 {
 	int reg, err, val;
@@ -390,6 +403,8 @@ static int bcm54xx_config_init(struct phy_device *phydev)
 		bcm_phy_write_exp(phydev, BCM_EXP_MULTICOLOR, val);
 	}
 
+	bcm54xx_ptp_config_init(phydev);
+
 	return 0;
 }
 
@@ -741,10 +756,6 @@ static irqreturn_t brcm_fet_handle_interrupt(struct phy_device *phydev)
 	return IRQ_HANDLED;
 }
 
-struct bcm54xx_phy_priv {
-	u64	*stats;
-};
-
 static int bcm54xx_phy_probe(struct phy_device *phydev)
 {
 	struct bcm54xx_phy_priv *priv;
@@ -761,6 +772,10 @@ static int bcm54xx_phy_probe(struct phy_device *phydev)
 	if (!priv->stats)
 		return -ENOMEM;
 
+	priv->ptp = bcm_ptp_probe(phydev);
+	if (IS_ERR(priv->ptp))
+		return PTR_ERR(priv->ptp);
+
 	return 0;
 }
 
-- 
2.31.1

[PATCH net-next v1 4/4] net: phy: Kconfig: Add broadcom PTP PHY library

[Jonathan Lemon]

Add a Broadcom PTP PHY library, initially supporting the BCM54213PE
found in the RPi CM4.

Signed-off-by: Jonathan Lemon <jonathan.lemon@gmail.com>
---
 drivers/net/phy/Kconfig  | 10 ++++++++++
 drivers/net/phy/Makefile |  1 +
 2 files changed, 11 insertions(+)

diff --git a/drivers/net/phy/Kconfig b/drivers/net/phy/Kconfig
index ea7571a2b39b..66f9c9cc51cf 100644
--- a/drivers/net/phy/Kconfig
+++ b/drivers/net/phy/Kconfig
@@ -101,6 +101,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 b2728d00fc9a..bb21d4240a40 100644
--- a/drivers/net/phy/Makefile
+++ b/drivers/net/phy/Makefile
@@ -46,6 +46,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
-- 
2.31.1

Re: [PATCH net-next v1 1/4] net: phy: broadcom: Add PTP support for some Broadcom PHYs.

[Florian Fainelli]

On 4/23/2022 7:23 PM, Jonathan Lemon wrote:
> 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>
> ---

[snip]

Mostly checking register names/offsets/usage because I am not familiar 
enough with how PTP is supposed to work.

> +#define MODE_SEL_SHIFT_PORT		0
> +#define MODE_SEL_SHIFT_CPU		8
> +
> +#define rx_mode(sel, evt, act) \
> +	(((MODE_RX_##act) << (MODE_EVT_SHIFT_##evt)) << (MODE_SEL_SHIFT_##sel))
> +
> +#define tx_mode(sel, evt, act) \
> +	(((MODE_TX_##act) << (MODE_EVT_SHIFT_##evt)) << (MODE_SEL_SHIFT_##sel))

I would capitalize these two macros to make it clear that they are 
exactly that, macros.

> +
> +/* 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

Maybe add a macro here as well:

#define TIME_CODE(x)		(TIME_CODE0 + (x))

> +#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)

Seems like you have better documentation than I do :)

> +#define SHADOW_CTRL		0x085c

Unused

> +#define SHADOW_LOAD		0x085d
> +#define  TIME_CODE_LOAD			BIT(10)
> +#define  SYNC_OUT_LOAD			BIT(9)
> +#define  NCO_TIME_LOAD			BIT(7)

NCO Divider load is bit 8 and Local time Load bit is 7, can you check 
which one you need?

> +#define  FREQ_LOAD			BIT(6)
> +#define INTR_MASK		0x085e
> +#define INTR_STATUS		0x085f
> +#define  INTC_FSYNC			BIT(0)
> +#define  INTC_SOP			BIT(1)
> +
> +#define FREQ_REG_LSB		0x0873
> +#define FREQ_REG_MSB		0x0874

Those two hold the NCO frequency, can you rename accordingly?

> +
> +#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

Likewise a macro could be useful here.

> +
> +#define TS_READ_CTRL		0x0885
> +#define  TS_READ_START			BIT(0)
> +#define  TS_READ_END			BIT(1)
> +
> +#define TIMECODE_CTRL		0x08c3
> +#define  TX_TIMECODE_SEL		GENMASK(7, 0)
> +#define  RX_TIMECODE_SEL		GENMASK(15, 8)

This one looks out of order compared to the other registers

> +
> +#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 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 NSE_CTRL		0x087f

This one is also out of order.

[snip]
> +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:
> +#ifdef PHY_ID_BCM54213PE
> +	case PHY_ID_BCM54213PE:
> +#endif

This does not exist upstream.
-- 
Florian

Re: [PATCH net-next v1 3/4] net: phy: broadcom: Hook up the PTP PHY functions

[Florian Fainelli]

On 4/23/2022 7:23 PM, Jonathan Lemon wrote:
> Add 'struct bcm_ptp_private' to bcm54xx_phy_priv which points to
> an optional PTP structure attached to the PHY.  This is allocated
> on probe, if PHY PTP support is configured, and if the PHY has a
> PTP supported by the driver.
> 
> Signed-off-by: Jonathan Lemon <jonathan.lemon@gmail.com>

Reviewed-by: Florian Fainelli <f.fainelli@gmail.com>
-- 
Florian

Re: [PATCH net-next v1 4/4] net: phy: Kconfig: Add broadcom PTP PHY library

[Florian Fainelli]

On 4/23/2022 7:23 PM, Jonathan Lemon wrote:
> Add a Broadcom PTP PHY library, initially supporting the BCM54213PE
> found in the RPi CM4.
> 
> Signed-off-by: Jonathan Lemon <jonathan.lemon@gmail.com>

This should be squashed into the first patch.
-- 
Florian

Re: [PATCH net-next v1 2/4] net: phy: broadcom: Add Broadcom PTP hooks to bcm-phy-lib

[Florian Fainelli]

On 4/23/2022 7:23 PM, Jonathan Lemon wrote:
> Add the public bcm_ptp_probe() and bcm_ptp_config_init() functions
> to the bcm-phy library.  The PTP functions are contained in a separate
> file for clarity, and also to simplify the PTP clock dependencies.
> 
> Signed-off-by: Jonathan Lemon <jonathan.lemon@gmail.com>
> ---
>   drivers/net/phy/bcm-phy-lib.c | 13 +++++++++++++
>   drivers/net/phy/bcm-phy-lib.h |  3 +++
>   2 files changed, 16 insertions(+)
> 
> diff --git a/drivers/net/phy/bcm-phy-lib.c b/drivers/net/phy/bcm-phy-lib.c
> index 287cccf8f7f4..b9d2d1d48402 100644
> --- a/drivers/net/phy/bcm-phy-lib.c
> +++ b/drivers/net/phy/bcm-phy-lib.c
> @@ -816,6 +816,19 @@ int bcm_phy_cable_test_get_status_rdb(struct phy_device *phydev,
>   }
>   EXPORT_SYMBOL_GPL(bcm_phy_cable_test_get_status_rdb);
>   
> +#if !IS_ENABLED(CONFIG_BCM_NET_PHYPTP)
> +struct bcm_ptp_private *bcm_ptp_probe(struct phy_device *phydev)
> +{
> +	return NULL;
> +}
> +EXPORT_SYMBOL_GPL(bcm_ptp_probe);
> +
> +void bcm_ptp_config_init(struct phy_device *phydev)
> +{
> +}
> +EXPORT_SYMBOL_GPL(bcm_ptp_config_init);
> +#endif

I would place those in bcm-phy-lib.h instead of in bcm-phy-lib.c and 
have them be static inline stubs.
-- 
Florian

Re: [PATCH net-next v1 0/4] Broadcom PTP PHY support

[Richard Cochran]

On Sat, Apr 23, 2022 at 07:23:52PM -0700, Jonathan Lemon wrote:

> There are other Broadcom chips which may benefit from using the 
> same framework here, although with different register sets.

Based on two examples, the present 542xx and the 541xx (which I am
familiar with), it appears that the registers sets are the same in
gen1, just the base offset is different.

So it would be great to store the base in bcm_ptp_private, adding the
base into each read/write operation.

Thanks,
Richard

Re: [PATCH net-next v1 1/4] net: phy: broadcom: Add PTP support for some Broadcom PHYs.

[Richard Cochran]

On Sat, Apr 23, 2022 at 07:23:53PM -0700, Jonathan Lemon wrote:

> +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;
> +
> +	if (!priv->hwts_rx)
> +		return false;
> +
> +	header = ptp_parse_header(skb, type);
> +	if (!header)
> +		return false;
> +
> +	data = (u8 *)(header + 1);

No need to pointer math, as ptp_header already has reserved1 and reserved2.

> +	sec = get_unaligned_be32(data);

Something is missing here.  The seconds field is only four bits, so
the code needs to read the 80 bit counter once in a while and augment
the time stamp with the upper bits.

> +	nsec = get_unaligned_be32(data + 4);
> +
> +	hwts = skb_hwtstamps(skb);
> +	hwts->hwtstamp = ktime_set(sec, nsec);
> +
> +	return false;
> +}

Thanks,
Richard

Re: [PATCH net-next v1 1/4] net: phy: broadcom: Add PTP support for some Broadcom PHYs.

[Richard Cochran]

On Sat, Apr 23, 2022 at 07:23:53PM -0700, Jonathan Lemon wrote:

> +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));

Okay, so now I am sad.  The 541xx has:

  TIMESTAMP_INFO_1 0xA8C  bit 0 DIR, bits 1-2 msg_type, etc
  TIMESTAMP_INFO_2 0xA8D  sequence ID

It is the same info, but randomly shuffled among the two registers in
a different way.

So much for supporting multiple devices with a common code base.  :(

> +	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;
> +}

Thanks,
Richard

Re: [PATCH net-next v1 1/4] net: phy: broadcom: Add PTP support for some Broadcom PHYs.

[Richard Cochran]

On Sat, Apr 23, 2022 at 07:23:53PM -0700, Jonathan Lemon wrote:

> +static int bcm_ptp_settime_locked(struct bcm_ptp_private *priv,
> +				  const struct timespec64 *ts)
> +{
> +	struct phy_device *phydev = priv->phydev;
> +	u16 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);
> +
> +	/* zero out NCO counter */
> +	bcm_phy_write_exp(phydev, NCO_TIME_0, 0);
> +	bcm_phy_write_exp(phydev, NCO_TIME_1, 0);
> +	bcm_phy_write_exp(phydev, NCO_TIME_2_CTRL, 0);

You are setting the 48 bit counter to zero.

But Lasse's version does this:

	// Assign original time codes (48 bit)
	local_time_codes[2] = 0x4000;
	local_time_codes[1] = (u16)(ts->tv_nsec >> 20);
	local_time_codes[0] = (u16)(ts->tv_nsec >> 4);

	...

	// Write Local Time Code Register
	bcm_phy_write_exp(phydev, NSE_DPPL_NCO_2_0_REG, local_time_codes[0]);
	bcm_phy_write_exp(phydev, NSE_DPPL_NCO_2_1_REG, local_time_codes[1]);
	bcm_phy_write_exp(phydev, NSE_DPPL_NCO_2_2_REG, local_time_codes[2]);

My understanding is that the PPS output function uses the 48 bit
counter, and so it ought to be set to a non-zero value.

In any case, it would be nice to have the 80/48 bit register usage
clearly explained.

> +	/* set up load on next frame sync */
> +	bcm_phy_write_exp(phydev, SHADOW_LOAD, TIME_CODE_LOAD | NCO_TIME_LOAD);
> +
> +	ctrl = priv->nse_ctrl;
> +	return bcm_ptp_framesync(phydev, NULL, ctrl | NSE_INIT);
> +}

Thanks,
Richard

Re: [PATCH net-next v1 1/4] net: phy: broadcom: Add PTP support for some Broadcom PHYs.

[Jonathan Lemon]

On Sun, Apr 24, 2022 at 06:19:31PM -0700, Richard Cochran wrote:
> On Sat, Apr 23, 2022 at 07:23:53PM -0700, Jonathan Lemon wrote:
> 
> > +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;
> > +
> > +	if (!priv->hwts_rx)
> > +		return false;
> > +
> > +	header = ptp_parse_header(skb, type);
> > +	if (!header)
> > +		return false;
> > +
> > +	data = (u8 *)(header + 1);
> 
> No need to pointer math, as ptp_header already has reserved1 and reserved2.
> 
> > +	sec = get_unaligned_be32(data);
> 
> Something is missing here.  The seconds field is only four bits, so
> the code needs to read the 80 bit counter once in a while and augment
> the time stamp with the upper bits.

The BCM chip inserts a 64-bit sec.nsec RX timestamp immediately after
the PTP header.  So I'm recovering it here.  I'll also update the patch
to memmove() the tail of the skb up in order to remove it, just in case
it makes a difference.
-- 
Jonathan

Re: [PATCH net-next v1 1/4] net: phy: broadcom: Add PTP support for some Broadcom PHYs.

[Jonathan Lemon]

On Sun, Apr 24, 2022 at 08:12:39PM -0700, Richard Cochran wrote:
> On Sat, Apr 23, 2022 at 07:23:53PM -0700, Jonathan Lemon wrote:
> 
> > +static int bcm_ptp_settime_locked(struct bcm_ptp_private *priv,
> > +				  const struct timespec64 *ts)
> > +{
> > +	struct phy_device *phydev = priv->phydev;
> > +	u16 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);
> > +
> > +	/* zero out NCO counter */
> > +	bcm_phy_write_exp(phydev, NCO_TIME_0, 0);
> > +	bcm_phy_write_exp(phydev, NCO_TIME_1, 0);
> > +	bcm_phy_write_exp(phydev, NCO_TIME_2_CTRL, 0);
> 
> You are setting the 48 bit counter to zero.
> 
> But Lasse's version does this:
> 
> 	// Assign original time codes (48 bit)
> 	local_time_codes[2] = 0x4000;
> 	local_time_codes[1] = (u16)(ts->tv_nsec >> 20);
> 	local_time_codes[0] = (u16)(ts->tv_nsec >> 4);
> 
> 	...
> 
> 	// Write Local Time Code Register
> 	bcm_phy_write_exp(phydev, NSE_DPPL_NCO_2_0_REG, local_time_codes[0]);
> 	bcm_phy_write_exp(phydev, NSE_DPPL_NCO_2_1_REG, local_time_codes[1]);
> 	bcm_phy_write_exp(phydev, NSE_DPPL_NCO_2_2_REG, local_time_codes[2]);
> 
> My understanding is that the PPS output function uses the 48 bit
> counter, and so it ought to be set to a non-zero value.

I'm not sure what this is doing.  Setting BIT(14) says this is a
frequency control adjustment.  From my understanding, the local timer is
used for generating a oneshot output pulse, which the driver currently
doesn't do.


> In any case, it would be nice to have the 80/48 bit register usage
> clearly explained.

You and me both.
-- 
Jonathan

Re: [PATCH net-next v1 1/4] net: phy: broadcom: Add PTP support for some Broadcom PHYs.

[Jonathan Lemon]

On Sun, Apr 24, 2022 at 04:27:06PM -0700, Florian Fainelli wrote:
> 
> 
> On 4/23/2022 7:23 PM, Jonathan Lemon wrote:
> > 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>
> > ---
> 
> [snip]
> 
> Mostly checking register names/offsets/usage because I am not familiar
> enough with how PTP is supposed to work.
> 
> > +#define MODE_SEL_SHIFT_PORT		0
> > +#define MODE_SEL_SHIFT_CPU		8
> > +
> > +#define rx_mode(sel, evt, act) \
> > +	(((MODE_RX_##act) << (MODE_EVT_SHIFT_##evt)) << (MODE_SEL_SHIFT_##sel))
> > +
> > +#define tx_mode(sel, evt, act) \
> > +	(((MODE_TX_##act) << (MODE_EVT_SHIFT_##evt)) << (MODE_SEL_SHIFT_##sel))
> 
> I would capitalize these two macros to make it clear that they are exactly
> that, macros.

Ack.


> > +/* 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
> 
> Maybe add a macro here as well:
> 
> #define TIME_CODE(x)		(TIME_CODE0 + (x))

I'd prrefer keep them separate, as not all of the registers are
sequential - the heartbeat registers for example.


> > +#define SHADOW_LOAD		0x085d
> > +#define  TIME_CODE_LOAD			BIT(10)
> > +#define  SYNC_OUT_LOAD			BIT(9)
> > +#define  NCO_TIME_LOAD			BIT(7)
> 
> NCO Divider load is bit 8 and Local time Load bit is 7, can you check which
> one you need?

Local time load.  NCO divider counts the SYNC_IN pulses and generates a
timestamp after <n> events (as I understand things).

 
> > +#define  FREQ_LOAD			BIT(6)
> > +#define INTR_MASK		0x085e
> > +#define INTR_STATUS		0x085f
> > +#define  INTC_FSYNC			BIT(0)
> > +#define  INTC_SOP			BIT(1)
> > +
> > +#define FREQ_REG_LSB		0x0873
> > +#define FREQ_REG_MSB		0x0874
> 
> Those two hold the NCO frequency, can you rename accordingly?


> > +#define TS_READ_CTRL		0x0885
> > +#define  TS_READ_START			BIT(0)
> > +#define  TS_READ_END			BIT(1)
> > +
> > +#define TIMECODE_CTRL		0x08c3
> > +#define  TX_TIMECODE_SEL		GENMASK(7, 0)
> > +#define  RX_TIMECODE_SEL		GENMASK(15, 8)
> 
> This one looks out of order compared to the other registers

Will rearrange the groups a bit.


> > +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:
> > +#ifdef PHY_ID_BCM54213PE
> > +	case PHY_ID_BCM54213PE:
> > +#endif
> 
> This does not exist upstream.

Yes, hence the #ifdef.  Will remove this for the next patch.  It's here
since I can just copy it over to the rpi-5.15 tree and have it still
work.
-- 
Jonathan

Re: [PATCH net-next v1 1/4] net: phy: broadcom: Add PTP support for some Broadcom PHYs.

[Jonathan Lemon]

On Sun, Apr 24, 2022 at 06:38:00PM -0700, Richard Cochran wrote:
> On Sat, Apr 23, 2022 at 07:23:53PM -0700, Jonathan Lemon wrote:
> 
> > +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));
> 
> Okay, so now I am sad.  The 541xx has:
> 
>   TIMESTAMP_INFO_1 0xA8C  bit 0 DIR, bits 1-2 msg_type, etc
>   TIMESTAMP_INFO_2 0xA8D  sequence ID
> 
> It is the same info, but randomly shuffled among the two registers in
> a different way.
> 
> So much for supporting multiple devices with a common code base.  :(

We could just have a chip-specific version of this function.  The
recovered timestamp is passed back in a structure, so the rest of the
code would be unchanged.
-- 
Jonathan    (no, not volunteering to do this...)

Re: [PATCH net-next v1 1/4] net: phy: broadcom: Add PTP support for some Broadcom PHYs.

[Richard Cochran]

On Mon, Apr 25, 2022 at 04:30:43PM -0700, Jonathan Lemon wrote:
> On Sun, Apr 24, 2022 at 06:19:31PM -0700, Richard Cochran wrote:
> > On Sat, Apr 23, 2022 at 07:23:53PM -0700, Jonathan Lemon wrote:
> > 
> > > +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;
> > > +
> > > +	if (!priv->hwts_rx)
> > > +		return false;
> > > +
> > > +	header = ptp_parse_header(skb, type);
> > > +	if (!header)
> > > +		return false;
> > > +
> > > +	data = (u8 *)(header + 1);
> > 
> > No need to pointer math, as ptp_header already has reserved1 and reserved2.
> > 
> > > +	sec = get_unaligned_be32(data);
> > 
> > Something is missing here.  The seconds field is only four bits, so
> > the code needs to read the 80 bit counter once in a while and augment
> > the time stamp with the upper bits.
> 
> The BCM chip inserts a 64-bit sec.nsec RX timestamp immediately after
> the PTP header.  So I'm recovering it here.  I'll also update the patch
> to memmove() the tail of the skb up in order to remove it, just in case
> it makes a difference.

Okay, this is something different.  This won't work because that
corrupts the PTP message format.

I'm talking about a different mode where the PHY places the time stamp
into the reserved1/2 fields.

Thanks,
Richard

Re: [PATCH net-next v1 1/4] net: phy: broadcom: Add PTP support for some Broadcom PHYs.

[Richard Cochran]

On Mon, Apr 25, 2022 at 04:55:40PM -0700, Jonathan Lemon wrote:

> We could just have a chip-specific version of this function.  The
> recovered timestamp is passed back in a structure, so the rest of the
> code would be unchanged.

Yeah, but it means that I'll have to check each and every bit of every
register to see what other random changes are there...

> Jonathan    (no, not volunteering to do this...)

For now, just get your chip merged, and then the next chip's driver
will refactor/reuse as needed.

Thanks,
Richard

Re: [PATCH net-next v1 1/4] net: phy: broadcom: Add PTP support for some Broadcom PHYs.

[Richard Cochran]

On Mon, Apr 25, 2022 at 07:49:15PM -0700, Richard Cochran wrote:
> On Mon, Apr 25, 2022 at 04:30:43PM -0700, Jonathan Lemon wrote:

> > The BCM chip inserts a 64-bit sec.nsec RX timestamp immediately after
> > the PTP header.  So I'm recovering it here.  I'll also update the patch
> > to memmove() the tail of the skb up in order to remove it, just in case
> > it makes a difference.
> 
> Okay, this is something different.  This won't work because that
> corrupts the PTP message format.

Wait, I see, you want to copy the back of the frame data over the time
stamp.  That makes sense, and it avoids clobbering the reserved1 field
(which aquired a meaning in 1588 v2.1)

Thanks,
Richard

Re: [PATCH net-next v1 1/4] net: phy: broadcom: Add PTP support for some Broadcom PHYs.

[Florian Fainelli]

On 4/25/22 19:53, Richard Cochran wrote:
> On Mon, Apr 25, 2022 at 04:55:40PM -0700, Jonathan Lemon wrote:
> 
>> We could just have a chip-specific version of this function.  The
>> recovered timestamp is passed back in a structure, so the rest of the
>> code would be unchanged.
> 
> Yeah, but it means that I'll have to check each and every bit of every
> register to see what other random changes are there...
> 
>> Jonathan    (no, not volunteering to do this...)
> 
> For now, just get your chip merged, and then the next chip's driver
> will refactor/reuse as needed.

Agreed, if we want to support Gen1 PHYs we can easily do that with 
adding a table of register offsets per family.
-- 
Florian

Re: [PATCH net-next v1 1/4] net: phy: broadcom: Add PTP support for some Broadcom PHYs.

[Richard Cochran]

On Sat, Apr 23, 2022 at 07:23:53PM -0700, Jonathan Lemon wrote:

> +static int bcm_ptp_adjtime_locked(struct bcm_ptp_private *priv,
> +				  s64 delta_ns)
> +{
> +	struct timespec64 ts;
> +	int err;
> +
> +	err = bcm_ptp_gettime_locked(priv, &ts, NULL);
> +	if (!err) {
> +		timespec64_add_ns(&ts, delta_ns);

When delta_ns is negative, this takes a long time to complete.

> +		err = bcm_ptp_settime_locked(priv, &ts);
> +	}
> +	return err;
> +}

Thanks,
Richard

Re: [PATCH net-next v1 1/4] net: phy: broadcom: Add PTP support for some Broadcom PHYs.

[Jonathan Lemon]

On Wed, Apr 27, 2022 at 07:40:25AM -0700, Richard Cochran wrote:
> On Sat, Apr 23, 2022 at 07:23:53PM -0700, Jonathan Lemon wrote:
> 
> > +static int bcm_ptp_adjtime_locked(struct bcm_ptp_private *priv,
> > +				  s64 delta_ns)
> > +{
> > +	struct timespec64 ts;
> > +	int err;
> > +
> > +	err = bcm_ptp_gettime_locked(priv, &ts, NULL);
> > +	if (!err) {
> > +		timespec64_add_ns(&ts, delta_ns);
> 
> When delta_ns is negative, this takes a long time to complete.

Hmm, just looked up the function signature - addend is u64, sigh.  Will
fix in next patch.

Thanks for pointing that out!
--
Jonathan