[PATCH] net: xgmac: Augment mdio read/write with cl-45 format support

[Nikunj Kela]

Currently, clause-22 format is supported. This change adds
support for clause-45 format.

Signed-off-by: Nikunj Kela <nikunj.k...@sima.ai>
---
 drivers/net/dwc_eth_xgmac.c | 67 +++++++++++++++++++++++++++----------
 1 file changed, 49 insertions(+), 18 deletions(-)

diff --git a/drivers/net/dwc_eth_xgmac.c b/drivers/net/dwc_eth_xgmac.c
index 03959ea95a5..68c69b7b3df 100644
--- a/drivers/net/dwc_eth_xgmac.c
+++ b/drivers/net/dwc_eth_xgmac.c
@@ -147,6 +147,7 @@ static int xgmac_mdio_read(struct mii_dev *bus, int 
mdio_addr, int mdio_devad,
        u32 val;
        u32 hw_addr;
        int ret;
+       u32 c45 = 1;
 
        debug("%s(dev=%p, addr=0x%x, reg=%d):\n", __func__, xgmac->dev, 
mdio_addr,
              mdio_reg);
@@ -159,19 +160,34 @@ static int xgmac_mdio_read(struct mii_dev *bus, int 
mdio_addr, int mdio_devad,
                return ret;
        }
 
-       /* Set clause 22 format */
-       val = BIT(mdio_addr);
-       writel(val, &xgmac->mac_regs->mdio_clause_22_port);
+       if (mdio_devad == MDIO_DEVAD_NONE) {
+               /* Set clause 22 format */
+               c45 = 0;
+               val = BIT(mdio_addr);
+               writel(val, &xgmac->mac_regs->mdio_clause_22_port);
+       } else {
+               val = readl(&xgmac->mac_regs->mdio_clause_22_port);
+               val &= ~BIT(mdio_addr);
+               writel(val, &xgmac->mac_regs->mdio_clause_22_port);
+       }
 
-       hw_addr = (mdio_addr << XGMAC_MAC_MDIO_ADDRESS_PA_SHIFT) |
-                  (mdio_reg & XGMAC_MAC_MDIO_REG_ADDR_C22P_MASK);
+       if (c45) {
+               hw_addr = (mdio_addr << XGMAC_MAC_MDIO_ADDRESS_PA_SHIFT) |
+                          (mdio_reg & 0xffff);
+               hw_addr |= mdio_devad << XGMAC_MAC_MDIO_ADDRESS_DA_SHIFT;
+       } else {
+               hw_addr = (mdio_addr << XGMAC_MAC_MDIO_ADDRESS_PA_SHIFT) |
+                          (mdio_reg & XGMAC_MAC_MDIO_REG_ADDR_C22P_MASK);
+       }
 
        val = xgmac->config->config_mac_mdio <<
              XGMAC_MAC_MDIO_ADDRESS_CR_SHIFT;
 
-       val |= XGMAC_MAC_MDIO_ADDRESS_SADDR |
-              XGMAC_MDIO_SINGLE_CMD_ADDR_CMD_READ |
-              XGMAC_MAC_MDIO_ADDRESS_SBUSY;
+       if (!c45)
+               val |= XGMAC_MAC_MDIO_ADDRESS_SADDR;
+
+       val |= XGMAC_MDIO_SINGLE_CMD_ADDR_CMD_READ |
+               XGMAC_MAC_MDIO_ADDRESS_SBUSY;
 
        ret = xgmac_mdio_wait_idle(xgmac);
        if (ret) {
@@ -207,6 +223,7 @@ static int xgmac_mdio_write(struct mii_dev *bus, int 
mdio_addr, int mdio_devad,
        u32 val;
        u32 hw_addr;
        int ret;
+       u32 c45 = 1;
 
        debug("%s(dev=%p, addr=0x%x, reg=%d, val=0x%x):\n", __func__, 
xgmac->dev,
              mdio_addr, mdio_reg, mdio_val);
@@ -219,21 +236,35 @@ static int xgmac_mdio_write(struct mii_dev *bus, int 
mdio_addr, int mdio_devad,
                return ret;
        }
 
-       /* Set clause 22 format */
-       val = BIT(mdio_addr);
-       writel(val, &xgmac->mac_regs->mdio_clause_22_port);
-
-       hw_addr = (mdio_addr << XGMAC_MAC_MDIO_ADDRESS_PA_SHIFT) |
-                  (mdio_reg & XGMAC_MAC_MDIO_REG_ADDR_C22P_MASK);
+       if (mdio_devad == MDIO_DEVAD_NONE) {
+               c45 = 0;
+               /* Set clause 22 format */
+               val = BIT(mdio_addr);
+               writel(val, &xgmac->mac_regs->mdio_clause_22_port);
+       } else {
+               val = readl(&xgmac->mac_regs->mdio_clause_22_port);
+               val &= ~BIT(mdio_addr);
+               writel(val, &xgmac->mac_regs->mdio_clause_22_port);
+       }
 
-       hw_addr |= (mdio_reg >> XGMAC_MAC_MDIO_ADDRESS_PA_SHIFT) <<
-                   XGMAC_MAC_MDIO_ADDRESS_DA_SHIFT;
+       if (c45) {
+               hw_addr = (mdio_addr << XGMAC_MAC_MDIO_ADDRESS_PA_SHIFT) |
+                          (mdio_reg & 0xffff);
+               hw_addr |= mdio_devad << XGMAC_MAC_MDIO_ADDRESS_DA_SHIFT;
+       } else {
+               hw_addr = (mdio_addr << XGMAC_MAC_MDIO_ADDRESS_PA_SHIFT) |
+                          (mdio_reg & XGMAC_MAC_MDIO_REG_ADDR_C22P_MASK);
+               hw_addr |= (mdio_reg >> XGMAC_MAC_MDIO_ADDRESS_PA_SHIFT) <<
+                           XGMAC_MAC_MDIO_ADDRESS_DA_SHIFT;
+       }
 
        val = (xgmac->config->config_mac_mdio <<
               XGMAC_MAC_MDIO_ADDRESS_CR_SHIFT);
 
-       val |= XGMAC_MAC_MDIO_ADDRESS_SADDR |
-               mdio_val | XGMAC_MDIO_SINGLE_CMD_ADDR_CMD_WRITE |
+       if (!c45)
+               val |= XGMAC_MAC_MDIO_ADDRESS_SADDR;
+
+       val |=  mdio_val | XGMAC_MDIO_SINGLE_CMD_ADDR_CMD_WRITE |
                XGMAC_MAC_MDIO_ADDRESS_SBUSY;
 
        ret = xgmac_mdio_wait_idle(xgmac);
-- 
2.34.1