diff options
| -rw-r--r-- | drivers/net/ethernet/stmicro/stmmac/stmmac_mdio.c | 55 |
1 files changed, 28 insertions, 27 deletions
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_mdio.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_mdio.c index 0a302b711bc2..c305c202055a 100644 --- a/drivers/net/ethernet/stmicro/stmmac/stmmac_mdio.c +++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_mdio.c @@ -229,6 +229,26 @@ static int stmmac_xgmac2_mdio_write_c45(struct mii_bus *bus, int phyaddr, phydata); } +/** + * stmmac_mdio_format_addr() - format the address register + * @priv: struct stmmac_priv pointer + * @pa: 5-bit MDIO package address + * @gr: 5-bit MDIO register address (C22) or MDIO device address (C45) + * + * Return: formatted address register + */ +static u32 stmmac_mdio_format_addr(struct stmmac_priv *priv, + unsigned int pa, unsigned int gr) +{ + const struct mii_regs *mii_regs = &priv->hw->mii; + + return ((pa << mii_regs->addr_shift) & mii_regs->addr_mask) | + ((gr << mii_regs->reg_shift) & mii_regs->reg_mask) | + ((priv->clk_csr << mii_regs->clk_csr_shift) & + mii_regs->clk_csr_mask) | + MII_BUSY; +} + static int stmmac_mdio_read(struct stmmac_priv *priv, int data, u32 value) { unsigned int mii_address = priv->hw->mii.addr; @@ -263,18 +283,14 @@ static int stmmac_mdio_read(struct stmmac_priv *priv, int data, u32 value) static int stmmac_mdio_read_c22(struct mii_bus *bus, int phyaddr, int phyreg) { struct stmmac_priv *priv = netdev_priv(bus->priv); - u32 value = MII_BUSY; int data = 0; + u32 value; data = pm_runtime_resume_and_get(priv->device); if (data < 0) return data; - 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; + value = stmmac_mdio_format_addr(priv, phyaddr, phyreg); if (priv->plat->has_gmac4) value |= MII_GMAC4_READ; @@ -300,20 +316,16 @@ static int stmmac_mdio_read_c45(struct mii_bus *bus, int phyaddr, int devad, int phyreg) { struct stmmac_priv *priv = netdev_priv(bus->priv); - u32 value = MII_BUSY; int data = 0; + u32 value; data = pm_runtime_resume_and_get(priv->device); if (data < 0) return data; - value |= (phyaddr << priv->hw->mii.addr_shift) - & priv->hw->mii.addr_mask; - value |= (priv->clk_csr << priv->hw->mii.clk_csr_shift) - & priv->hw->mii.clk_csr_mask; + value = stmmac_mdio_format_addr(priv, phyaddr, devad); value |= MII_GMAC4_READ; value |= MII_GMAC4_C45E; - value |= (devad << priv->hw->mii.reg_shift) & priv->hw->mii.reg_mask; data |= phyreg << MII_GMAC4_REG_ADDR_SHIFT; @@ -357,18 +369,13 @@ static int stmmac_mdio_write_c22(struct mii_bus *bus, int phyaddr, int phyreg, { struct stmmac_priv *priv = netdev_priv(bus->priv); int ret, data = phydata; - u32 value = MII_BUSY; + u32 value; ret = pm_runtime_resume_and_get(priv->device); if (ret < 0) return ret; - 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; + value = stmmac_mdio_format_addr(priv, phyaddr, phyreg); if (priv->plat->has_gmac4) value |= MII_GMAC4_WRITE; else @@ -395,21 +402,15 @@ static int stmmac_mdio_write_c45(struct mii_bus *bus, int phyaddr, { struct stmmac_priv *priv = netdev_priv(bus->priv); int ret, data = phydata; - u32 value = MII_BUSY; + u32 value; ret = pm_runtime_resume_and_get(priv->device); if (ret < 0) return ret; - value |= (phyaddr << priv->hw->mii.addr_shift) - & priv->hw->mii.addr_mask; - - value |= (priv->clk_csr << priv->hw->mii.clk_csr_shift) - & priv->hw->mii.clk_csr_mask; - + value = stmmac_mdio_format_addr(priv, phyaddr, devad); value |= MII_GMAC4_WRITE; value |= MII_GMAC4_C45E; - value |= (devad << priv->hw->mii.reg_shift) & priv->hw->mii.reg_mask; data |= phyreg << MII_GMAC4_REG_ADDR_SHIFT; |