[PATCH net-next v4 02/14] yt6801: Implement mdio register

[Frank Sae <Frank.Sae@xxxxxxxxxxxxxx>]

Implement the mdio bus read, write and register function.

Signed-off-by: Frank Sae <Frank.Sae@xxxxxxxxxxxxxx>
---
 .../ethernet/motorcomm/yt6801/yt6801_main.c   | 86 +++++++++++++++++++
 .../ethernet/motorcomm/yt6801/yt6801_type.h   |  8 ++
 2 files changed, 94 insertions(+)

diff --git a/drivers/net/ethernet/motorcomm/yt6801/yt6801_main.c b/drivers/net/ethernet/motorcomm/yt6801/yt6801_main.c
index 10d63a8ed..39f03b4a4 100644
--- a/drivers/net/ethernet/motorcomm/yt6801/yt6801_main.c
+++ b/drivers/net/ethernet/motorcomm/yt6801/yt6801_main.c
@@ -20,6 +20,92 @@
 #include <linux/module.h>
 #include "yt6801_type.h"
 
+#define PHY_WR_CONFIG(reg_offset)	(0x8000205 + ((reg_offset) * 0x10000))
+static int fxgmac_phy_write_reg(struct fxgmac_pdata *priv, u32 reg_id, u32 data)
+{
+	u32 val;
+	int ret;
+
+	fxgmac_io_wr(priv, MAC_MDIO_DATA, data);
+	fxgmac_io_wr(priv, MAC_MDIO_ADDR, PHY_WR_CONFIG(reg_id));
+	ret = read_poll_timeout_atomic(fxgmac_io_rd, val,
+				       !FIELD_GET(MAC_MDIO_ADDR_BUSY, val),
+				       10, 250, false, priv, MAC_MDIO_ADDR);
+	if (ret == -ETIMEDOUT)
+		dev_err(priv->dev, "%s, id:%x ctrl:0x%08x, data:0x%08x\n",
+			__func__, reg_id, PHY_WR_CONFIG(reg_id), data);
+
+	return ret;
+}
+
+#define PHY_RD_CONFIG(reg_offset)	(0x800020d + ((reg_offset) * 0x10000))
+static int fxgmac_phy_read_reg(struct fxgmac_pdata *priv, u32 reg_id)
+{
+	u32 val;
+	int ret;
+
+	fxgmac_io_wr(priv, MAC_MDIO_ADDR, PHY_RD_CONFIG(reg_id));
+	ret = read_poll_timeout_atomic(fxgmac_io_rd, val,
+				       !FIELD_GET(MAC_MDIO_ADDR_BUSY, val),
+				       10, 250, false, priv, MAC_MDIO_ADDR);
+	if (ret == -ETIMEDOUT) {
+		dev_err(priv->dev, "%s, id:%x, ctrl:0x%08x, val:0x%08x.\n",
+			__func__, reg_id, PHY_RD_CONFIG(reg_id), val);
+		return ret;
+	}
+
+	return fxgmac_io_rd(priv, MAC_MDIO_DATA); /* Read data */
+}
+
+static int fxgmac_mdio_write_reg(struct mii_bus *mii_bus, int phyaddr,
+				 int phyreg, u16 val)
+{
+	if (phyaddr > 0)
+		return -ENODEV;
+
+	return fxgmac_phy_write_reg(mii_bus->priv, phyreg, val);
+}
+
+static int fxgmac_mdio_read_reg(struct mii_bus *mii_bus, int phyaddr,
+				int phyreg)
+{
+	if (phyaddr > 0)
+		return -ENODEV;
+
+	return fxgmac_phy_read_reg(mii_bus->priv, phyreg);
+}
+
+static int fxgmac_mdio_register(struct fxgmac_pdata *priv)
+{
+	struct pci_dev *pdev = to_pci_dev(priv->dev);
+	struct phy_device *phydev;
+	struct mii_bus *new_bus;
+	int ret;
+
+	new_bus = devm_mdiobus_alloc(&pdev->dev);
+	if (!new_bus)
+		return -ENOMEM;
+
+	new_bus->name = "yt6801";
+	new_bus->priv = priv;
+	new_bus->parent = &pdev->dev;
+	new_bus->read = fxgmac_mdio_read_reg;
+	new_bus->write = fxgmac_mdio_write_reg;
+	snprintf(new_bus->id, MII_BUS_ID_SIZE, "yt6801-%x-%x",
+		 pci_domain_nr(pdev->bus), pci_dev_id(pdev));
+
+	ret = devm_mdiobus_register(&pdev->dev, new_bus);
+	if (ret < 0)
+		return ret;
+
+	phydev = mdiobus_get_phy(new_bus, 0);
+	if (!phydev)
+		return -ENODEV;
+
+	priv->phydev = phydev;
+	return 0;
+}
+
 static void fxgmac_phy_release(struct fxgmac_pdata *priv)
 {
 	fxgmac_io_wr_bits(priv, EPHY_CTRL, EPHY_CTRL_RESET, 1);
diff --git a/drivers/net/ethernet/motorcomm/yt6801/yt6801_type.h b/drivers/net/ethernet/motorcomm/yt6801/yt6801_type.h
index bb6c2640a..b43952981 100644
--- a/drivers/net/ethernet/motorcomm/yt6801/yt6801_type.h
+++ b/drivers/net/ethernet/motorcomm/yt6801/yt6801_type.h
@@ -34,6 +34,14 @@
 #define EPHY_CTRL_STA_DUPLEX			BIT(2)
 #define EPHY_CTRL_STA_SPEED			GENMASK(4, 3)
 
+#define MAC_MDIO_ADDR			0x2200
+#define MAC_MDIO_ADDR_BUSY		BIT(0)
+#define MAC_MDIO_ADDR_GOC		GENMASK(3, 2)
+
+#define MAC_MDIO_DATA			0x2204
+#define MAC_MDIO_DATA_GD		GENMASK(15, 0)
+#define MAC_MDIO_DATA_RA		GENMASK(31, 16)
+
 struct fxgmac_resources {
 	void __iomem *addr;
 	int irq;
-- 
2.34.1