[PATCH 3/3] net: phy: Use device tree properties to initialize any PHYs

[Michael Walle <michael@walle.cc>]

Some PHYs drivers, like the marvell and the broadcom one, are able to
initialize PHY registers via device tree properties. This patch adds a
more generic property which applies to any PHY. It supports clause-22,
clause-45 and paged PHY writes.

Hopefully, some board maintainers will pick this up and switch to this
instead of adding more phy_fixups in architecture specific code,
although it is board specific. For example have a look at
arch/arm/mach-imx/ for phy_register_fixup.

Signed-off-by: Michael Walle <michael@walle.cc>
---
 drivers/net/phy/phy_device.c | 97 +++++++++++++++++++++++++++++++++++-
 1 file changed, 96 insertions(+), 1 deletion(-)

diff --git a/drivers/net/phy/phy_device.c b/drivers/net/phy/phy_device.c
index 9d2bbb13293e..3c4cbaf72c27 100644
--- a/drivers/net/phy/phy_device.c
+++ b/drivers/net/phy/phy_device.c
@@ -30,6 +30,9 @@
 #include <linux/mdio.h>
 #include <linux/io.h>
 #include <linux/uaccess.h>
+#include <linux/of.h>
+
+#include <dt-bindings/net/phy.h>
 
 MODULE_DESCRIPTION("PHY library");
 MODULE_AUTHOR("Andy Fleming");
@@ -1064,6 +1067,95 @@ static int phy_poll_reset(struct phy_device *phydev)
 	return 0;
 }
 
+/**
+ * phy_of_reg_init - Set and/or override configuration registers.
+ * @phydev: target phy_device struct
+ *
+ * Description: Set and/or override some configuration registers based
+ *   on the reg-init property stored in the of_node for the phydev.
+ *
+ *   reg-init = <dev reg mask value>,...;
+ *   There may be one or more sets of <dev reg mask value>:
+ */
+static int phy_of_reg_init(struct phy_device *phydev)
+{
+	struct device_node *node = phydev->mdio.dev.of_node;
+	int oldpage = -1, savedpage = -1;
+	const __be32 *paddr;
+	int len, i;
+	int ret;
+
+	if (!IS_ENABLED(CONFIG_OF_MDIO))
+		return 0;
+
+	if (!node)
+		return 0;
+
+	paddr = of_get_property(node, "reg-init", &len);
+	if (!paddr || len < (4 * sizeof(*paddr)))
+		return 0;
+
+	mutex_lock(&phydev->mdio.bus->mdio_lock);
+
+	savedpage = -1;
+	len /= sizeof(*paddr);
+	for (i = 0; i < len - 3; i += 4) {
+		u32 dev = be32_to_cpup(paddr + i);
+		u16 reg = be32_to_cpup(paddr + i + 1);
+		u16 mask = be32_to_cpup(paddr + i + 2);
+		u16 val_bits = be32_to_cpup(paddr + i + 3);
+		int page = dev & 0xffff;
+		int devad = dev & 0x1f;
+		int val;
+
+		if (dev & PHY_REG_PAGE) {
+			if (savedpage < 0) {
+				savedpage = __phy_read_page(phydev);
+				if (savedpage < 0) {
+					ret = savedpage;
+					goto err;
+				}
+				oldpage = savedpage;
+			}
+			if (oldpage != page) {
+				ret = __phy_write_page(phydev, page);
+				if (ret < 0)
+					goto err;
+				oldpage = page;
+			}
+		}
+
+		val = 0;
+		if (mask) {
+			if (dev & PHY_REG_C45)
+				val = __phy_read_mmd(phydev, devad, reg);
+			else
+				val = __phy_read(phydev, reg);
+			if (val < 0) {
+				ret = val;
+				goto err;
+			}
+			val &= mask;
+		}
+		val |= val_bits;
+
+		if (dev & PHY_REG_C45)
+			ret = __phy_write_mmd(phydev, devad, reg, val);
+		else
+			ret = __phy_write(phydev, reg, val);
+		if (ret < 0)
+			goto err;
+	}
+
+err:
+	if (savedpage >= 0)
+		__phy_write_page(phydev, savedpage);
+
+	mutex_unlock(&phydev->mdio.bus->mdio_lock);
+
+	return ret;
+}
+
 int phy_init_hw(struct phy_device *phydev)
 {
 	int ret = 0;
@@ -1087,7 +1179,10 @@ int phy_init_hw(struct phy_device *phydev)
 	if (phydev->drv->config_init)
 		ret = phydev->drv->config_init(phydev);
 
-	return ret;
+	if (ret < 0)
+		return ret;
+
+	return phy_of_reg_init(phydev);
 }
 EXPORT_SYMBOL(phy_init_hw);
 
-- 
2.20.1