From d585c55b9f70cf9e8c66820d7efe7130c683f19e Mon Sep 17 00:00:00 2001
From: Antoine Tenart <antoine.tenart@bootlin.com>
Date: Fri, 21 Feb 2020 11:51:27 +0100
Subject: [PATCH 2/3] net: phy: add an MDIO SMBus library

Signed-off-by: Antoine Tenart <antoine.tenart@bootlin.com>
---
 drivers/net/mdio/Kconfig      | 11 +++++++
 drivers/net/mdio/Makefile     |  1 +
 drivers/net/mdio/mdio-smbus.c | 62 +++++++++++++++++++++++++++++++++++
 drivers/net/phy/Kconfig       |  1 +
 include/linux/mdio/mdio-i2c.h | 16 +++++++++
 5 files changed, 91 insertions(+)
 create mode 100644 drivers/net/mdio/mdio-smbus.c

--- a/drivers/net/mdio/Kconfig
+++ b/drivers/net/mdio/Kconfig
@@ -54,6 +54,17 @@ config MDIO_SUN4I
 	  interface units of the Allwinner SoC that have an EMAC (A10,
 	  A12, A10s, etc.)
 
+config MDIO_SMBUS
+	tristate
+	depends on I2C_SMBUS
+	help
+	  Support SMBus based PHYs. This provides a MDIO bus bridged
+	  to SMBus to allow PHYs connected in SMBus mode to be accessed
+	  using the existing infrastructure.
+
+	  This is library mode.
+
+
 config MDIO_XGENE
 	tristate "APM X-Gene SoC MDIO bus controller"
 	depends on ARCH_XGENE || COMPILE_TEST
--- a/drivers/net/mdio/Makefile
+++ b/drivers/net/mdio/Makefile
@@ -20,6 +20,7 @@ obj-$(CONFIG_MDIO_MSCC_MIIM)		+= mdio-ms
 obj-$(CONFIG_MDIO_MVUSB)		+= mdio-mvusb.o
 obj-$(CONFIG_MDIO_OCTEON)		+= mdio-octeon.o
 obj-$(CONFIG_MDIO_REGMAP)		+= mdio-regmap.o
+obj-$(CONFIG_MDIO_SMBUS)		+= mdio-smbus.o
 obj-$(CONFIG_MDIO_SUN4I)		+= mdio-sun4i.o
 obj-$(CONFIG_MDIO_THUNDER)		+= mdio-thunder.o
 obj-$(CONFIG_MDIO_XGENE)		+= mdio-xgene.o
--- /dev/null
+++ b/drivers/net/mdio/mdio-smbus.c
@@ -0,0 +1,341 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * MDIO SMBus bridge
+ *
+ * Copyright (C) 2020 Antoine Tenart
+ * Copyright (C) 2025 Bjørn Mork <bjorn@mork.no>
+ *
+ * Network PHYs can appear on SMBus when they are part of SFP modules.
+ */
+#include <linux/i2c.h>
+#include <linux/phy.h>
+#include <linux/mdio/mdio-i2c.h>
+#include <linux/sfp.h>
+
+static int smbus_mii_read_c45(struct mii_bus *mii, int phy_id, int devad, int reg)
+{
+	u16 bus_addr = i2c_mii_phy_addr(phy_id);
+	struct i2c_adapter *i2c = mii->priv;
+	union i2c_smbus_data data;
+	size_t addrlen;
+	u8 buf[5], *p;
+	int i, ret;
+
+	if (!i2c_mii_valid_phy_id(phy_id))
+		return 0xffff;
+
+	p = buf;
+	if (devad >= 0) {
+		*p++ = 0x20 | devad;
+		*p++ = reg >> 8;
+	}
+	*p++ = reg;
+	addrlen = p - buf;
+
+	i2c_lock_bus(i2c, I2C_LOCK_SEGMENT);
+	if (addrlen > 1) {
+		for (i = 1; i < addrlen; i++) {
+			data.byte = buf[i];
+			ret = __i2c_smbus_xfer(i2c, bus_addr, 0, I2C_SMBUS_WRITE, buf[0], I2C_SMBUS_BYTE_DATA, &data);
+			if (ret < 0)
+				goto unlock;
+		}
+	}
+
+	for (i = addrlen; i < addrlen + 2; i++) {
+		ret = __i2c_smbus_xfer(i2c, bus_addr, 0, I2C_SMBUS_READ, buf[0], I2C_SMBUS_BYTE_DATA, &data);
+		if (ret < 0)
+			goto unlock;
+		buf[i] = data.byte;
+	}
+
+unlock:
+	i2c_unlock_bus(i2c, I2C_LOCK_SEGMENT);
+	if (ret < 0)
+		return 0xffff;
+	return buf[addrlen] << 8 | buf[addrlen + 1];
+}
+
+static int smbus_mii_write_c45(struct mii_bus *mii, int phy_id, int devad, int reg, u16 val)
+{
+	u16 bus_addr = i2c_mii_phy_addr(phy_id);
+	struct i2c_adapter *i2c = mii->priv;
+	union i2c_smbus_data data;
+	size_t buflen;
+	u8 buf[5], *p;
+	int i, ret;
+
+	if (!i2c_mii_valid_phy_id(phy_id))
+		return 0;
+
+	p = buf;
+	if (devad >= 0) {
+		*p++ = devad;
+		*p++ = reg >> 8;
+	}
+	*p++ = reg;
+	*p++ = val >> 8;
+	*p++ = val;
+	buflen = p - buf;
+
+	i2c_lock_bus(i2c, I2C_LOCK_SEGMENT);
+	for (i = 1; i < buflen; i++) {
+		data.byte = buf[i];
+		ret = __i2c_smbus_xfer(i2c, bus_addr, 0, I2C_SMBUS_WRITE, buf[0], I2C_SMBUS_BYTE_DATA, &data);
+		if (ret < 0)
+			goto unlock;
+	}
+unlock:
+	i2c_unlock_bus(i2c, I2C_LOCK_SEGMENT);
+	return ret < 0 ? ret : 0;
+}
+
+static int smbus_mii_read_c22(struct mii_bus *bus, int phy_id, int reg)
+{
+	return smbus_mii_read_c45(bus, phy_id, -1, reg);
+}
+
+static int smbus_mii_write_c22(struct mii_bus *bus, int phy_id, int reg, u16 val)
+{
+	return smbus_mii_write_c45(bus, phy_id, -1, reg, val);
+}
+
+/* From mdio-i2c.c:
+ *
+ * RollBall SFPs do not access internal PHY via I2C address 0x56, but
+ * instead via address 0x51, when SFP page is set to 0x03 and password to
+ * 0xffffffff.
+ *
+ * address  size  contents  description
+ * -------  ----  --------  -----------
+ * 0x80     1     CMD       0x01/0x02/0x04 for write/read/done
+ * 0x81     1     DEV       Clause 45 device
+ * 0x82     2     REG       Clause 45 register
+ * 0x84     2     VAL       Register value
+ */
+#define ROLLBALL_PHY_I2C_ADDR		0x51
+
+#define ROLLBALL_PASSWORD		(SFP_VSL + 3)
+
+#define ROLLBALL_CMD_ADDR		0x80
+#define ROLLBALL_DATA_ADDR		0x81
+
+#define ROLLBALL_CMD_WRITE		0x01
+#define ROLLBALL_CMD_READ		0x02
+#define ROLLBALL_CMD_DONE		0x04
+
+#define SFP_PAGE_ROLLBALL_MDIO		3
+
+static int smbus_set_sfp_page_lock(struct i2c_adapter *i2c, int bus_addr, u8 page)
+{
+	union i2c_smbus_data data;
+	u8 oldpage;
+	int ret;
+
+	i2c_lock_bus(i2c, I2C_LOCK_SEGMENT);
+
+	/* read current page */
+	ret = __i2c_smbus_xfer(i2c, bus_addr, 0, I2C_SMBUS_READ, SFP_PAGE, I2C_SMBUS_BYTE_DATA, &data);
+	if (ret < 0)
+		goto unlock;
+
+	oldpage = data.byte;
+	data.byte = page;
+	ret = __i2c_smbus_xfer(i2c, bus_addr, 0, I2C_SMBUS_WRITE, SFP_PAGE, I2C_SMBUS_BYTE_DATA, &data);
+	if (ret == 0)
+		return oldpage;
+
+unlock:
+	i2c_unlock_bus(i2c, I2C_LOCK_SEGMENT);
+
+	return ret;
+}
+
+static int __smbus_set_sfp_page_unlock(struct i2c_adapter *i2c, int bus_addr, u8 page)
+{
+	union i2c_smbus_data data;
+	int ret;
+
+	data.byte = page;
+	ret = __i2c_smbus_xfer(i2c, bus_addr, 0, I2C_SMBUS_WRITE, SFP_PAGE, I2C_SMBUS_BYTE_DATA, &data);
+	i2c_unlock_bus(i2c, I2C_LOCK_SEGMENT);
+
+	return ret;
+}
+
+/* Wait for the ROLLBALL_CMD_ADDR register to read ROLLBALL_CMD_DONE,
+ * indicating that the previous command has completed.
+ *
+ * Quoting from the mdio-i2c.c implementation:
+ *
+ * 	By experiment it takes up to 70 ms to access a register for these
+ * 	SFPs. Sleep 20ms between iterations and try 10 times.
+ */
+static int __smbus_rollball_mii_poll(struct i2c_adapter *i2c , int bus_addr)
+{
+	union i2c_smbus_data data;
+	int i, ret;
+
+	i = 10;
+	do {
+		msleep(20);
+
+		ret = __i2c_smbus_xfer(i2c, bus_addr, 0, I2C_SMBUS_READ, ROLLBALL_CMD_ADDR, I2C_SMBUS_BYTE_DATA, &data);
+		if (ret < 0)
+			return ret;
+
+		if (data.byte == ROLLBALL_CMD_DONE)
+			return 0;
+	} while (i-- > 0);
+	dev_dbg(&i2c->dev, "poll timed out\n");
+	return -ETIMEDOUT;
+}
+
+static int smbus_mii_read_rollball(struct mii_bus *bus, int phy_id, int devad, int reg)
+{
+	struct i2c_adapter *i2c = bus->priv;
+	union i2c_smbus_data data;
+	int i, bus_addr, old, ret;
+	u8 buf[6];
+
+	bus_addr = i2c_mii_phy_addr(phy_id);
+	if (bus_addr != ROLLBALL_PHY_I2C_ADDR)
+ 		return 0xffff;
+
+	old = smbus_set_sfp_page_lock(i2c, bus_addr, SFP_PAGE_ROLLBALL_MDIO);
+	if (old < 0)
+		return 0xffff;
+
+	/* set address */
+	buf[0] = ROLLBALL_CMD_READ;
+	buf[1] = devad;
+	buf[2] = reg >> 8;
+	buf[3] = reg & 0xff;
+
+	/* send address */
+	for (i = 0; i < 4; i++) {
+		data.byte = buf[i];
+		ret = __i2c_smbus_xfer(i2c, bus_addr, 0, I2C_SMBUS_WRITE, ROLLBALL_CMD_ADDR + i, I2C_SMBUS_BYTE_DATA, &data);
+		if (ret < 0)
+			goto unlock;
+	}
+
+	/* wait for command to complete */
+	ret = __smbus_rollball_mii_poll(i2c, bus_addr);
+	if (ret)
+		goto unlock;
+
+	/* read result */
+	for (i = 4; i < 6; i++) {
+		ret = __i2c_smbus_xfer(i2c, bus_addr, 0, I2C_SMBUS_READ, ROLLBALL_CMD_ADDR + i, I2C_SMBUS_BYTE_DATA, &data);
+		if (ret < 0)
+			goto unlock;
+		buf[i] = data.byte;
+	}
+
+unlock:
+	__smbus_set_sfp_page_unlock(i2c, bus_addr, old);
+	if (ret < 0)
+		return 0xffff;
+	return buf[4] << 8 | buf[5];
+}
+
+static int smbus_mii_write_rollball(struct mii_bus *bus, int phy_id, int devad, int reg, u16 val)
+{
+	struct i2c_adapter *i2c = bus->priv;
+	union i2c_smbus_data data;
+	int i, bus_addr, old, ret;
+	u8 buf[6];
+
+	bus_addr = i2c_mii_phy_addr(phy_id);
+	if (bus_addr != ROLLBALL_PHY_I2C_ADDR)
+		return 0;
+
+	old = smbus_set_sfp_page_lock(i2c, bus_addr, SFP_PAGE_ROLLBALL_MDIO);
+	if (old < 0)
+		return old;
+
+	/* set address */
+	buf[0] = ROLLBALL_CMD_WRITE;
+	buf[1] = devad;
+	buf[2] = reg >> 8;
+	buf[3] = reg & 0xff;
+	buf[4] = val >> 8;
+	buf[5] = val & 0xff;
+
+	/* send address and value */
+	for (i = 0; i < 6; i++) {
+		data.byte = buf[i];
+		ret = __i2c_smbus_xfer(i2c, bus_addr, 0, I2C_SMBUS_WRITE, ROLLBALL_CMD_ADDR + i, I2C_SMBUS_BYTE_DATA, &data);
+		if (ret < 0)
+			goto unlock;
+	}
+
+	/* wait for command to complete */
+	ret = __smbus_rollball_mii_poll(i2c, bus_addr);
+
+unlock:
+	__smbus_set_sfp_page_unlock(i2c, bus_addr, old);
+	return ret;
+}
+
+/* write "password" - four 0xff bytes - to the ROLLBALL_PASSWORD register */
+static int smbus_mii_init_rollball(struct i2c_adapter *i2c)
+{
+	union i2c_smbus_data data;
+	int i, ret;
+
+	data.byte = 0xff;
+	for (i = 0; i < 4; i++) {
+		ret = i2c_smbus_xfer(i2c, ROLLBALL_PHY_I2C_ADDR, 0, I2C_SMBUS_WRITE, ROLLBALL_PASSWORD + i, I2C_SMBUS_BYTE_DATA, &data);
+		if (ret < 0)
+			return ret;
+	}
+	return 0;
+}
+
+struct mii_bus *mdio_smbus_alloc(struct device *parent, struct i2c_adapter *i2c,
+				 enum mdio_i2c_proto protocol)
+{
+	struct mii_bus *mii;
+	int ret;
+
+	if (!i2c_check_functionality(i2c, I2C_FUNC_SMBUS_BYTE_DATA))
+		return ERR_PTR(-EINVAL);
+
+	mii = mdiobus_alloc();
+	if (!mii)
+		return ERR_PTR(-ENOMEM);
+
+	snprintf(mii->id, MII_BUS_ID_SIZE, "smbus:%s", dev_name(parent));
+	mii->parent = parent;
+	mii->priv = i2c;
+
+	switch (protocol) {
+	case MDIO_I2C_ROLLBALL:
+		ret = smbus_mii_init_rollball(i2c);
+		if (ret < 0) {
+			dev_err(parent,
+				"Cannot initialize RollBall MDIO protocol on SMBus: %d\n",
+				ret);
+			mdiobus_free(mii);
+			return ERR_PTR(ret);
+		}
+
+		mii->read_c45 = smbus_mii_read_rollball;
+		mii->write_c45 = smbus_mii_write_rollball;
+		break;
+	default:
+		mii->read = smbus_mii_read_c22;
+		mii->write = smbus_mii_write_c22;
+		mii->read_c45 = smbus_mii_read_c45;
+		mii->write_c45 = smbus_mii_write_c45;
+		break;
+	}
+
+	return mii;
+}
+
+MODULE_AUTHOR("Antoine Tenart");
+MODULE_DESCRIPTION("MDIO SMBus bridge library");
+MODULE_LICENSE("GPL");
--- a/drivers/net/phy/Kconfig
+++ b/drivers/net/phy/Kconfig
@@ -65,6 +65,7 @@ config SFP
 	depends on I2C && PHYLINK
 	depends on HWMON || HWMON=n
 	select MDIO_I2C
+	select MDIO_SMBUS
 
 comment "Switch configuration API + drivers"
 
--- a/include/linux/mdio/mdio-i2c.h
+++ b/include/linux/mdio/mdio-i2c.h
@@ -20,5 +20,9 @@ enum mdio_i2c_proto {
 
 struct mii_bus *mdio_i2c_alloc(struct device *parent, struct i2c_adapter *i2c,
 			       enum mdio_i2c_proto protocol);
+struct mii_bus *mdio_smbus_alloc(struct device *parent, struct i2c_adapter *i2c,
+				 enum mdio_i2c_proto protocol);
+bool i2c_mii_valid_phy_id(int phy_id);
+unsigned int i2c_mii_phy_addr(int phy_id);
 
 #endif
--- a/drivers/net/mdio/mdio-i2c.c
+++ b/drivers/net/mdio/mdio-i2c.c
@@ -20,12 +20,12 @@
  * specified to be present in SFP modules.  These correspond with PHY
  * addresses 16 and 17.  Disallow access to these "phy" addresses.
  */
-static bool i2c_mii_valid_phy_id(int phy_id)
+bool i2c_mii_valid_phy_id(int phy_id)
 {
 	return phy_id != 0x10 && phy_id != 0x11;
 }
 
-static unsigned int i2c_mii_phy_addr(int phy_id)
+unsigned int i2c_mii_phy_addr(int phy_id)
 {
 	return phy_id + 0x40;
 }
