net: stmmac: enable clause 45 mdio support

DWMAC4 is capable to support clause 45 mdio communication.
This patch enable the feature on stmmac_mdio_write() and
stmmac_mdio_read() by following phy_write_mmd() and
phy_read_mmd() mdiobus read write implementation format.

Reviewed-by: Li, Yifan <yifan2.li@intel.com>
Signed-off-by: Kweh Hock Leong <hock.leong.kweh@intel.com>
Signed-off-by: Ong Boon Leong <boon.leong.ong@intel.com>
Signed-off-by: Voon Weifeng <weifeng.voon@intel.com>
Reviewed-by: Andrew Lunn <andrew@lunn.ch>
Signed-off-by: David S. Miller <davem@davemloft.net>

diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_mdio.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_mdio.c
index 18cadf0b0d6684bf6ef20d0c0069b5d526089711..4304c1abc5d1da1d55f4006f603006355e9b3172 100644 (file)
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_mdio.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_mdio.c
@@ -24,11 +24,14 @@
 
 #define MII_BUSY 0x00000001
 #define MII_WRITE 0x00000002
+#define MII_DATA_MASK GENMASK(15, 0)
 
 /* GMAC4 defines */
 #define MII_GMAC4_GOC_SHIFT            2
+#define MII_GMAC4_REG_ADDR_SHIFT       16
 #define MII_GMAC4_WRITE                        (1 << MII_GMAC4_GOC_SHIFT)
 #define MII_GMAC4_READ                 (3 << MII_GMAC4_GOC_SHIFT)
+#define MII_GMAC4_C45E                 BIT(1)
 
 /* XGMAC defines */
 #define MII_XGMAC_SADDR                        BIT(18)
@@ -155,22 +158,34 @@ static int stmmac_mdio_read(struct mii_bus *bus, int phyaddr, int phyreg)
        struct stmmac_priv *priv = netdev_priv(ndev);
        unsigned int mii_address = priv->hw->mii.addr;
        unsigned int mii_data = priv->hw->mii.data;
-       u32 v;
-       int data;
        u32 value = MII_BUSY;
+       int data = 0;
+       u32 v;
 
        value |= (phyaddr << priv->hw->mii.addr_shift)
                & priv->hw->mii.addr_mask;
        value |= (phyreg << priv->hw->mii.reg_shift) & priv->hw->mii.reg_mask;
        value |= (priv->clk_csr << priv->hw->mii.clk_csr_shift)
                & priv->hw->mii.clk_csr_mask;
-       if (priv->plat->has_gmac4)
+       if (priv->plat->has_gmac4) {
                value |= MII_GMAC4_READ;
+               if (phyreg & MII_ADDR_C45) {
+                       value |= MII_GMAC4_C45E;
+                       value &= ~priv->hw->mii.reg_mask;
+                       value |= ((phyreg >> MII_DEVADDR_C45_SHIFT) <<
+                              priv->hw->mii.reg_shift) &
+                              priv->hw->mii.reg_mask;
+
+                       data |= (phyreg & MII_REGADDR_C45_MASK) <<
+                               MII_GMAC4_REG_ADDR_SHIFT;
+               }
+       }
 
        if (readl_poll_timeout(priv->ioaddr + mii_address, v, !(v & MII_BUSY),
                               100, 10000))
                return -EBUSY;
 
+       writel(data, priv->ioaddr + mii_data);
        writel(value, priv->ioaddr + mii_address);
 
        if (readl_poll_timeout(priv->ioaddr + mii_address, v, !(v & MII_BUSY),
@@ -178,7 +193,7 @@ static int stmmac_mdio_read(struct mii_bus *bus, int phyaddr, int phyreg)
                return -EBUSY;
 
        /* Read the data from the MII data register */
-       data = (int)readl(priv->ioaddr + mii_data);
+       data = (int)readl(priv->ioaddr + mii_data) & MII_DATA_MASK;
 
        return data;
 }
@@ -198,8 +213,9 @@ static int stmmac_mdio_write(struct mii_bus *bus, int phyaddr, int phyreg,
        struct stmmac_priv *priv = netdev_priv(ndev);
        unsigned int mii_address = priv->hw->mii.addr;
        unsigned int mii_data = priv->hw->mii.data;
-       u32 v;
        u32 value = MII_BUSY;
+       int data = phydata;
+       u32 v;
 
        value |= (phyaddr << priv->hw->mii.addr_shift)
                & priv->hw->mii.addr_mask;
@@ -207,10 +223,21 @@ static int stmmac_mdio_write(struct mii_bus *bus, int phyaddr, int phyreg,
 
        value |= (priv->clk_csr << priv->hw->mii.clk_csr_shift)
                & priv->hw->mii.clk_csr_mask;
-       if (priv->plat->has_gmac4)
+       if (priv->plat->has_gmac4) {
                value |= MII_GMAC4_WRITE;
-       else
+               if (phyreg & MII_ADDR_C45) {
+                       value |= MII_GMAC4_C45E;
+                       value &= ~priv->hw->mii.reg_mask;
+                       value |= ((phyreg >> MII_DEVADDR_C45_SHIFT) <<
+                              priv->hw->mii.reg_shift) &
+                              priv->hw->mii.reg_mask;
+
+                       data |= (phyreg & MII_REGADDR_C45_MASK) <<
+                               MII_GMAC4_REG_ADDR_SHIFT;
+               }
+       } else {
                value |= MII_WRITE;
+       }
 
        /* Wait until any existing MII operation is complete */
        if (readl_poll_timeout(priv->ioaddr + mii_address, v, !(v & MII_BUSY),
@@ -218,7 +245,7 @@ static int stmmac_mdio_write(struct mii_bus *bus, int phyaddr, int phyreg,
                return -EBUSY;
 
        /* Set the MII address register to write */
-       writel(phydata, priv->ioaddr + mii_data);
+       writel(data, priv->ioaddr + mii_data);
        writel(value, priv->ioaddr + mii_address);
 
        /* Wait until any existing MII operation is complete */

diff --git a/include/linux/phy.h b/include/linux/phy.h
index d0af7d37fdf9a8c170b4543a2166c1b82fa8a471..1739c6dc470e311c10c37123eee42f7b58dd9a20 100644 (file)
--- a/include/linux/phy.h
+++ b/include/linux/phy.h
@@ -195,6 +195,8 @@ static inline const char *phy_modes(phy_interface_t interface)
 /* Or MII_ADDR_C45 into regnum for read/write on mii_bus to enable the 21 bit
    IEEE 802.3ae clause 45 addressing mode used by 10GIGE phy chips. */
 #define MII_ADDR_C45 (1<<30)
+#define MII_DEVADDR_C45_SHIFT  16
+#define MII_REGADDR_C45_MASK   GENMASK(15, 0)
 
 struct device;
 struct phylink;