Improve synchronization accuracy between the BCM54210PE PHC and system clock

drivers/net/ethernet/cadence/macb.h

@@ -1358,6 +1358,8 @@ struct macb {
 	struct macb_or_gem_ops	macbgem_ops;
 
 	struct mii_bus		*mii_bus;
+	u64			mdio_write_pre_ns;
+	u64			mdio_write_post_ns;
 	struct phylink		*phylink;
 	struct phylink_config	phylink_config;
 	struct phylink_pcs	phylink_usx_pcs;

drivers/net/ethernet/cadence/macb_main.c

@@ -416,11 +416,13 @@ static int macb_mdio_read_c45(struct mii_bus *bus, int mii_id, int devad,
 	return status;
 }
 
-static int macb_mdio_write_c22(struct mii_bus *bus, int mii_id, int regnum,
-			       u16 value)
+static int macb_mdio_write_c22_sts(struct mii_bus *bus, int mii_id, int regnum,
+				   u16 value, struct ptp_system_timestamp *sts)
 {
 	struct macb *bp = bus->priv;
+	unsigned long flags;
 	int status;
+	u32 cmd;
 
 	status = pm_runtime_resume_and_get(&bp->pdev->dev);
 	if (status < 0)
@@ -430,12 +432,31 @@ static int macb_mdio_write_c22(struct mii_bus *bus, int mii_id, int regnum,
 	if (status < 0)
 		goto mdio_write_exit;
 
-	macb_writel(bp, MAN, (MACB_BF(SOF, MACB_MAN_C22_SOF)
-			      | MACB_BF(RW, MACB_MAN_C22_WRITE)
-			      | MACB_BF(PHYA, mii_id)
-			      | MACB_BF(REGA, regnum)
-			      | MACB_BF(CODE, MACB_MAN_C22_CODE)
-			      | MACB_BF(DATA, value)));
+	cmd = MACB_BF(SOF, MACB_MAN_C22_SOF)
+	      | MACB_BF(RW, MACB_MAN_C22_WRITE)
+	      | MACB_BF(PHYA, mii_id)
+	      | MACB_BF(REGA, regnum)
+	      | MACB_BF(CODE, MACB_MAN_C22_CODE)
+	      | MACB_BF(DATA, value);
+
+	if (sts) {
+		local_irq_save(flags);
+		ptp_read_system_prets(sts);
+		/* macb_writel() is relaxed; order it after the timestamp. */
+		mb();
+	}
+	macb_writel(bp, MAN, cmd);
+	if (sts) {
+		/* Flush the posted write before taking the upper bound. */
+		macb_readl(bp, NSR);
+		/* Order the read-back before the system timestamp. */
+		rmb();
+		ptp_read_system_postts(sts);
+		local_irq_restore(flags);
+
+		timespec64_add_ns(&sts->pre_ts, bp->mdio_write_pre_ns);
+		timespec64_add_ns(&sts->post_ts, bp->mdio_write_post_ns);
+	}
 
 	status = macb_mdio_wait_for_idle(bp);
 	if (status < 0)
@@ -448,6 +469,12 @@ static int macb_mdio_write_c22(struct mii_bus *bus, int mii_id, int regnum,
 	return status;
 }
 
+static int macb_mdio_write_c22(struct mii_bus *bus, int mii_id, int regnum,
+			       u16 value)
+{
+	return macb_mdio_write_c22_sts(bus, mii_id, regnum, value, NULL);
+}
+
 static int macb_mdio_write_c45(struct mii_bus *bus, int mii_id,
 			       int devad, int regnum,
 			       u16 value)
@@ -1150,6 +1177,45 @@ static int macb_mdiobus_register(struct macb *bp, struct device_node *mdio_np)
 	return mdiobus_register(bp->mii_bus);
 }
 
+static bool macb_mdio_init_sts(struct macb *bp)
+{
+	static const u16 gem_divisors[] = {
+		[GEM_CLK_DIV8] = 8,
+		[GEM_CLK_DIV16] = 16,
+		[GEM_CLK_DIV32] = 32,
+		[GEM_CLK_DIV48] = 48,
+		[GEM_CLK_DIV64] = 64,
+		[GEM_CLK_DIV96] = 96,
+		[GEM_CLK_DIV128] = 128,
+		[GEM_CLK_DIV224] = 224,
+	};
+	static const u16 macb_divisors[] = {
+		[MACB_CLK_DIV8] = 8,
+		[MACB_CLK_DIV16] = 16,
+		[MACB_CLK_DIV32] = 32,
+		[MACB_CLK_DIV64] = 64,
+	};
+	unsigned long rate = clk_get_rate(bp->pclk);
+	u32 config = macb_readl(bp, NCFGR);
+	u32 divisor;
+
+	if (!rate)
+		return false;
+
+	if (macb_is_gem(bp))
+		divisor = gem_divisors[GEM_BFEXT(CLK, config)];
+	else
+		divisor = macb_divisors[MACB_BFEXT(CLK, config)];
+
+	/* A full-preamble write completes 63..65 MDC periods after the
+	 * command write, including engine start phase.
+	 */
+	mdiobus_sts_bounds(rate, divisor, 63, 65,
+			   &bp->mdio_write_pre_ns, &bp->mdio_write_post_ns);
+
+	return true;
+}
+
 static int macb_mii_init(struct macb *bp)
 {
 	struct device_node *mdio_np, *np = bp->pdev->dev.of_node;
@@ -1175,6 +1241,8 @@ static int macb_mii_init(struct macb *bp)
 	bp->mii_bus->name = "MACB_mii_bus";
 	bp->mii_bus->read = &macb_mdio_read_c22;
 	bp->mii_bus->write = &macb_mdio_write_c22;
+	if (macb_mdio_init_sts(bp))
+		bp->mii_bus->write_sts = &macb_mdio_write_c22_sts;
 	bp->mii_bus->read_c45 = &macb_mdio_read_c45;
 	bp->mii_bus->write_c45 = &macb_mdio_write_c45;
 	bp->mii_bus->reset = &macb_mdio_reset;

drivers/net/mdio/mdio-bcm-unimac.c

@@ -16,6 +16,7 @@
 #include <linux/phy.h>
 #include <linux/platform_data/mdio-bcm-unimac.h>
 #include <linux/platform_device.h>
+#include <linux/ptp_clock_kernel.h>
 #include <linux/sched.h>
 
 #define MDIO_CMD		0x00
@@ -42,6 +43,7 @@ struct unimac_mdio_priv {
 	void			*wait_func_data;
 	struct clk		*clk;
 	u32			clk_freq;
+	unsigned long		mdio_ref_rate;
 };
 
 static inline u32 unimac_mdio_readl(struct unimac_mdio_priv *priv, u32 offset)
@@ -73,6 +75,27 @@ static inline void unimac_mdio_start(struct unimac_mdio_priv *priv)
 	unimac_mdio_writel(priv, reg, MDIO_CMD);
 }
 
+static void unimac_mdio_start_sts(struct unimac_mdio_priv *priv,
+				  struct ptp_system_timestamp *sts)
+{
+	unsigned long flags;
+	u32 reg;
+
+	reg = unimac_mdio_readl(priv, MDIO_CMD);
+	reg |= MDIO_START_BUSY;
+	local_irq_save(flags);
+	ptp_read_system_prets(sts);
+	/* Order the timestamp before the relaxed command write. */
+	mb();
+	unimac_mdio_writel(priv, reg, MDIO_CMD);
+	/* Flush the posted write before taking the upper bound. */
+	unimac_mdio_readl(priv, MDIO_CMD);
+	/* Order the read-back before the system timestamp. */
+	rmb();
+	ptp_read_system_postts(sts);
+	local_irq_restore(flags);
+}
+
 static int unimac_mdio_poll(void *wait_func_data)
 {
 	struct unimac_mdio_priv *priv = wait_func_data;
@@ -127,30 +150,69 @@ static int unimac_mdio_read(struct mii_bus *bus, int phy_id, int reg)
 	return ret;
 }
 
-static int unimac_mdio_write(struct mii_bus *bus, int phy_id,
-			     int reg, u16 val)
+static void unimac_mdio_sts_bounds(struct unimac_mdio_priv *priv, u32 config,
+				   u64 *pre_ns, u64 *post_ns)
+{
+	u32 divisor;
+
+	divisor = 2 * (((config >> MDIO_CLK_DIV_SHIFT) & MDIO_CLK_DIV_MASK) + 1);
+	/* A full-preamble write completes 63..65 MDC periods after the
+	 * command write, including engine start phase.
+	 */
+	mdiobus_sts_bounds(priv->mdio_ref_rate, divisor, 63, 65, pre_ns, post_ns);
+}
+
+static int unimac_mdio_write_sts(struct mii_bus *bus, int phy_id, int reg,
+				 u16 val, struct ptp_system_timestamp *sts)
 {
 	struct unimac_mdio_priv *priv = bus->priv;
-	u32 cmd;
+	u64 pre_ns, post_ns;
+	u32 cmd, config;
 	int ret;
 
 	ret = clk_prepare_enable(priv->clk);
 	if (ret)
 		return ret;
 
+	if (sts) {
+		config = unimac_mdio_readl(priv, MDIO_CFG);
+		if (config & MDIO_SUPP_PREAMBLE) {
+			ret = -EOPNOTSUPP;
+			goto out;
+		}
+		unimac_mdio_sts_bounds(priv, config, &pre_ns, &post_ns);
+	}
+
 	/* Prepare the write operation */
 	cmd = MDIO_WR | (phy_id << MDIO_PMD_SHIFT) |
 		(reg << MDIO_REG_SHIFT) | (0xffff & val);
 	unimac_mdio_writel(priv, cmd, MDIO_CMD);
 
-	unimac_mdio_start(priv);
+	if (sts) {
+		unimac_mdio_start_sts(priv, sts);
+		timespec64_add_ns(&sts->pre_ts, pre_ns);
+		timespec64_add_ns(&sts->post_ts, post_ns);
+	} else {
+		unimac_mdio_start(priv);
+	}
 
 	ret = priv->wait_func(priv->wait_func_data);
+	/* Some wait callbacks do not propagate a completion timeout. */
+	if (!ret && sts &&
+	    (unimac_mdio_readl(priv, MDIO_CMD) & MDIO_START_BUSY))
+		ret = -ETIMEDOUT;
+out:
 	clk_disable_unprepare(priv->clk);
 
 	return ret;
 }
 
+static int unimac_mdio_write(struct mii_bus *bus, int phy_id,
+			     int reg, u16 val)
+{
+	return unimac_mdio_write_sts(bus, phy_id, reg, val, NULL);
+}
+
 /* Workaround for integrated BCM7xxx Gigabit PHYs which have a problem with
  * their internal MDIO management controller making them fail to successfully
  * be read from or written to for the first transaction.  We insert a dummy
@@ -234,6 +296,19 @@ static int unimac_mdio_clk_set(struct unimac_mdio_priv *priv)
 	return ret;
 }
 
+static bool unimac_mdio_init_sts(struct unimac_mdio_priv *priv,
+				 struct device *dev)
+{
+	priv->mdio_ref_rate = clk_get_rate(priv->clk);
+	/* BCM2711's 200 MHz GENET reference clock is not described in DT. */
+	if (!priv->mdio_ref_rate && dev->parent &&
+	    of_device_is_compatible(dev->parent->of_node,
+				    "brcm,bcm2711-genet-v5"))
+		priv->mdio_ref_rate = 200000000;
+
+	return priv->mdio_ref_rate != 0;
+}
+
 static int unimac_mdio_probe(struct platform_device *pdev)
 {
 	struct unimac_mdio_pdata *pdata = pdev->dev.platform_data;
@@ -292,6 +367,8 @@ static int unimac_mdio_probe(struct platform_device *pdev)
 	bus->parent = &pdev->dev;
 	bus->read = unimac_mdio_read;
 	bus->write = unimac_mdio_write;
+	if (unimac_mdio_init_sts(priv, &pdev->dev))
+		bus->write_sts = unimac_mdio_write_sts;
 	bus->reset = unimac_mdio_reset;
 	snprintf(bus->id, MII_BUS_ID_SIZE, "%s-%d", pdev->name, pdev->id);
 

drivers/net/phy/bcm-phy-lib.c

@@ -42,6 +42,32 @@ int bcm_phy_write_exp(struct phy_device *phydev, u16 reg, u16 val)
 }
 EXPORT_SYMBOL_GPL(bcm_phy_write_exp);
 
+int __bcm_phy_write_exp_sts(struct phy_device *phydev, u16 reg, u16 val,
+			    struct ptp_system_timestamp *sts)
+{
+	int rc;
+
+	rc = __phy_write(phydev, MII_BCM54XX_EXP_SEL, reg);
+	if (rc < 0)
+		return rc;
+
+	return __phy_write_sts(phydev, MII_BCM54XX_EXP_DATA, val, sts);
+}
+EXPORT_SYMBOL_GPL(__bcm_phy_write_exp_sts);
+
+int bcm_phy_write_exp_sts(struct phy_device *phydev, u16 reg, u16 val,
+			  struct ptp_system_timestamp *sts)
+{
+	int rc;
+
+	phy_lock_mdio_bus(phydev);
+	rc = __bcm_phy_write_exp_sts(phydev, reg, val, sts);
+	phy_unlock_mdio_bus(phydev);
+
+	return rc;
+}
+EXPORT_SYMBOL_GPL(bcm_phy_write_exp_sts);
+
 int __bcm_phy_read_exp(struct phy_device *phydev, u16 reg)
 {
 	int val;

drivers/net/phy/bcm-phy-lib.h

@@ -31,9 +31,13 @@ struct ethtool_wolinfo;
 
 
 int __bcm_phy_write_exp(struct phy_device *phydev, u16 reg, u16 val);
+int __bcm_phy_write_exp_sts(struct phy_device *phydev, u16 reg, u16 val,
+			    struct ptp_system_timestamp *sts);
 int __bcm_phy_read_exp(struct phy_device *phydev, u16 reg);
 int __bcm_phy_modify_exp(struct phy_device *phydev, u16 reg, u16 mask, u16 set);
 int bcm_phy_write_exp(struct phy_device *phydev, u16 reg, u16 val);
+int bcm_phy_write_exp_sts(struct phy_device *phydev, u16 reg, u16 val,
+			  struct ptp_system_timestamp *sts);
 int bcm_phy_read_exp(struct phy_device *phydev, u16 reg);
 int bcm_phy_modify_exp(struct phy_device *phydev, u16 reg, u16 mask, u16 set);
 

drivers/net/phy/bcm-phy-ptp.c

@@ -214,22 +214,42 @@ static void bcm_ptp_framesync(struct phy_device *phydev, u16 ctrl)
 	bcm_phy_write_exp(phydev, NSE_CTRL, ctrl | NSE_CPU_FRAMESYNC);
 }
 
+static int bcm_ptp_framesync_sts(struct phy_device *phydev, u16 ctrl,
+				 struct ptp_system_timestamp *sts)
+{
+	return bcm_phy_write_exp_sts(phydev, NSE_CTRL,
+				     ctrl | NSE_CPU_FRAMESYNC, sts);
+}
+
 static int bcm_ptp_framesync_ts(struct phy_device *phydev,
 				struct ptp_system_timestamp *sts,
 				struct timespec64 *ts,
 				u16 orig_ctrl)
 {
+	bool mdio_sts = false;
 	u16 ctrl, reg;
-	int i;
+	int i, err;
 
 	ctrl = bcm_ptp_framesync_disable(phydev, orig_ctrl);
 
-	ptp_read_system_prets(sts);
-
 	/* trigger framesync + capture */
-	bcm_ptp_framesync(phydev, ctrl | NSE_CAPTURE_EN);
+	if (sts && phydev->mdio.bus->write_sts) {
+		mdio_sts = true;
+		err = bcm_ptp_framesync_sts(phydev, ctrl | NSE_CAPTURE_EN, sts);
+		if (err == -EOPNOTSUPP) {
+			mdio_sts = false;
+		} else if (err) {
+			/* sts may be uninitialized on error */
+			bcm_ptp_framesync_restore(phydev, orig_ctrl);
+			return err;
+		}
+	}
 
-	ptp_read_system_postts(sts);
+	if (!mdio_sts) {
+		ptp_read_system_prets(sts);
+		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++) {