diff mbox series

[net-next,07/10] net: ethernet: freescale: xgmac: Separate C22 and C45 transactions for xgmac

Message ID 20220508153049.427227-8-andrew@lunn.ch (mailing list archive)
State Changes Requested
Delegated to: Netdev Maintainers
Headers show
Series net: mdio: Start separating C22 and C45 | expand

Checks

Context Check Description
netdev/tree_selection success Clearly marked for net-next
netdev/fixes_present success Fixes tag not required for -next series
netdev/subject_prefix success Link
netdev/cover_letter success Series has a cover letter
netdev/patch_count success Link
netdev/header_inline success No static functions without inline keyword in header files
netdev/build_32bit success Errors and warnings before: 0 this patch: 0
netdev/cc_maintainers warning 6 maintainers not CCed: linux-arm-kernel@lists.infradead.org linux-mediatek@lists.infradead.org edumazet@google.com pabeni@redhat.com kuba@kernel.org davem@davemloft.net
netdev/build_clang success Errors and warnings before: 0 this patch: 0
netdev/module_param success Was 0 now: 0
netdev/verify_signedoff success Signed-off-by tag matches author and committer
netdev/verify_fixes success No Fixes tag
netdev/build_allmodconfig_warn success Errors and warnings before: 0 this patch: 0
netdev/checkpatch success total: 0 errors, 0 warnings, 0 checks, 212 lines checked
netdev/kdoc success Errors and warnings before: 0 this patch: 0
netdev/source_inline success Was 0 now: 0

Commit Message

Andrew Lunn May 8, 2022, 3:30 p.m. UTC
The xgmac MDIO bus driver can perform both C22 and C45 transfers.
Create separate functions for each and register the C45 versions using
the new API calls where appropriate.

Signed-off-by: Andrew Lunn <andrew@lunn.ch>
---
 drivers/net/ethernet/freescale/xgmac_mdio.c | 154 +++++++++++++++-----
 1 file changed, 117 insertions(+), 37 deletions(-)

Comments

Vladimir Oltean May 10, 2022, 6:28 p.m. UTC | #1
On Sun, May 08, 2022 at 05:30:46PM +0200, Andrew Lunn wrote:
> The xgmac MDIO bus driver can perform both C22 and C45 transfers.
> Create separate functions for each and register the C45 versions using
> the new API calls where appropriate.
> 
> Signed-off-by: Andrew Lunn <andrew@lunn.ch>
> ---
>  drivers/net/ethernet/freescale/xgmac_mdio.c | 154 +++++++++++++++-----
>  1 file changed, 117 insertions(+), 37 deletions(-)
> 
> diff --git a/drivers/net/ethernet/freescale/xgmac_mdio.c b/drivers/net/ethernet/freescale/xgmac_mdio.c
> index ec90da1de030..ddfe6bf1f231 100644
> --- a/drivers/net/ethernet/freescale/xgmac_mdio.c
> +++ b/drivers/net/ethernet/freescale/xgmac_mdio.c
> @@ -128,30 +128,59 @@ static int xgmac_wait_until_done(struct device *dev,
>  	return 0;
>  }
>  
> -/*
> - * Write value to the PHY for this device to the register at regnum,waiting
> +/* Write value to the PHY for this device to the register at regnum,waiting
>   * until the write is done before it returns.  All PHY configuration has to be
>   * done through the TSEC1 MIIM regs.
>   */
> -static int xgmac_mdio_write(struct mii_bus *bus, int phy_id, int regnum, u16 value)
> +static int xgmac_mdio_write_c22(struct mii_bus *bus, int phy_id, int regnum,
> +				u16 value)
>  {
>  	struct mdio_fsl_priv *priv = (struct mdio_fsl_priv *)bus->priv;
>  	struct tgec_mdio_controller __iomem *regs = priv->mdio_base;
> -	uint16_t dev_addr;
> +	bool endian = priv->is_little_endian;
>  	u32 mdio_ctl, mdio_stat;
> +	u16 dev_addr;
>  	int ret;
> +
> +	mdio_stat = xgmac_read32(&regs->mdio_stat, endian);
> +	dev_addr = regnum & 0x1f;

Please move this either to the variable declaration, or near the mdio_ctl write,
or just integrate it into the macro argument.

> +	mdio_stat &= ~MDIO_STAT_ENC;
> +

You can remove this empty line during read-modify-write patterns.

> +	xgmac_write32(mdio_stat, &regs->mdio_stat, endian);
> +
> +	ret = xgmac_wait_until_free(&bus->dev, regs, endian);
> +	if (ret)
> +		return ret;
> +
> +	/* Set the port and dev addr */
> +	mdio_ctl = MDIO_CTL_PORT_ADDR(phy_id) | MDIO_CTL_DEV_ADDR(dev_addr);
> +	xgmac_write32(mdio_ctl, &regs->mdio_ctl, endian);
> +
> +	/* Write the value to the register */
> +	xgmac_write32(MDIO_DATA(value), &regs->mdio_data, endian);
> +
> +	ret = xgmac_wait_until_done(&bus->dev, regs, endian);
> +	if (ret)
> +		return ret;
> +
> +	return 0;
> +}
> +
> +/* Write value to the PHY for this device to the register at regnum,waiting
> + * until the write is done before it returns.  All PHY configuration has to be
> + * done through the TSEC1 MIIM regs.
> + */
> +static int xgmac_mdio_write_c45(struct mii_bus *bus, int phy_id, int dev_addr,
> +				int regnum, u16 value)
> +{
> +	struct mdio_fsl_priv *priv = (struct mdio_fsl_priv *)bus->priv;
> +	struct tgec_mdio_controller __iomem *regs = priv->mdio_base;
>  	bool endian = priv->is_little_endian;
> +	u32 mdio_ctl, mdio_stat;
> +	int ret;
>  
>  	mdio_stat = xgmac_read32(&regs->mdio_stat, endian);
> -	if (regnum & MII_ADDR_C45) {
> -		/* Clause 45 (ie 10G) */
> -		dev_addr = (regnum >> 16) & 0x1f;
> -		mdio_stat |= MDIO_STAT_ENC;
> -	} else {
> -		/* Clause 22 (ie 1G) */
> -		dev_addr = regnum & 0x1f;
> -		mdio_stat &= ~MDIO_STAT_ENC;
> -	}
> +	mdio_stat |= MDIO_STAT_ENC;
>  
>  	xgmac_write32(mdio_stat, &regs->mdio_stat, endian);
>  
> @@ -164,13 +193,11 @@ static int xgmac_mdio_write(struct mii_bus *bus, int phy_id, int regnum, u16 val
>  	xgmac_write32(mdio_ctl, &regs->mdio_ctl, endian);
>  
>  	/* Set the register address */
> -	if (regnum & MII_ADDR_C45) {
> -		xgmac_write32(regnum & 0xffff, &regs->mdio_addr, endian);
> +	xgmac_write32(regnum & 0xffff, &regs->mdio_addr, endian);

Does regnum ever exceed 0xffff now that the MMD is no longer encoded into it?

>  
> -		ret = xgmac_wait_until_free(&bus->dev, regs, endian);
> -		if (ret)
> -			return ret;
> -	}
> +	ret = xgmac_wait_until_free(&bus->dev, regs, endian);
> +	if (ret)
> +		return ret;
>  
>  	/* Write the value to the register */
>  	xgmac_write32(MDIO_DATA(value), &regs->mdio_data, endian);
> @@ -182,31 +209,84 @@ static int xgmac_mdio_write(struct mii_bus *bus, int phy_id, int regnum, u16 val
>  	return 0;
>  }
>  
> -/*
> - * Reads from register regnum in the PHY for device dev, returning the value.
> +/* Reads from register regnum in the PHY for device dev, returning the value.
>   * Clears miimcom first.  All PHY configuration has to be done through the
>   * TSEC1 MIIM regs.
>   */
> -static int xgmac_mdio_read(struct mii_bus *bus, int phy_id, int regnum)
> +static int xgmac_mdio_read_c22(struct mii_bus *bus, int phy_id, int regnum)
>  {
>  	struct mdio_fsl_priv *priv = (struct mdio_fsl_priv *)bus->priv;
>  	struct tgec_mdio_controller __iomem *regs = priv->mdio_base;
> +	bool endian = priv->is_little_endian;
>  	unsigned long flags;
> -	uint16_t dev_addr;
>  	uint32_t mdio_stat;
>  	uint32_t mdio_ctl;
> +	u16 dev_addr;
>  	int ret;
> -	bool endian = priv->is_little_endian;
>  
>  	mdio_stat = xgmac_read32(&regs->mdio_stat, endian);
> -	if (regnum & MII_ADDR_C45) {
> -		dev_addr = (regnum >> 16) & 0x1f;
> -		mdio_stat |= MDIO_STAT_ENC;
> +	dev_addr = regnum & 0x1f;

I'm thinking we could just pass "regnum & 0x1f" (or just regnum) to
MDIO_CTL_DEV_ADDR() for the c22 functions.

> +	mdio_stat &= ~MDIO_STAT_ENC;
> +
> +	xgmac_write32(mdio_stat, &regs->mdio_stat, endian);
> +
> +	ret = xgmac_wait_until_free(&bus->dev, regs, endian);
> +	if (ret)
> +		return ret;
> +
> +	/* Set the Port and Device Addrs */
> +	mdio_ctl = MDIO_CTL_PORT_ADDR(phy_id) | MDIO_CTL_DEV_ADDR(dev_addr);
> +	xgmac_write32(mdio_ctl, &regs->mdio_ctl, endian);
> +
> +	if (priv->has_a009885)
> +		/* Once the operation completes, i.e. MDIO_STAT_BSY clears, we
> +		 * must read back the data register within 16 MDC cycles.
> +		 */
> +		local_irq_save(flags);
> +
> +	/* Initiate the read */
> +	xgmac_write32(mdio_ctl | MDIO_CTL_READ, &regs->mdio_ctl, endian);
> +
> +	ret = xgmac_wait_until_done(&bus->dev, regs, endian);
> +	if (ret)
> +		goto irq_restore;
> +
> +	/* Return all Fs if nothing was there */
> +	if ((xgmac_read32(&regs->mdio_stat, endian) & MDIO_STAT_RD_ER) &&
> +	    !priv->has_a011043) {
> +		dev_dbg(&bus->dev,
> +			"Error while reading PHY%d reg at %d.%d\n",
> +			phy_id, dev_addr, regnum);
> +		ret = 0xffff;
>  	} else {
> -		dev_addr = regnum & 0x1f;
> -		mdio_stat &= ~MDIO_STAT_ENC;
> +		ret = xgmac_read32(&regs->mdio_data, endian) & 0xffff;
> +		dev_dbg(&bus->dev, "read %04x\n", ret);
>  	}
>  
> +irq_restore:
> +	if (priv->has_a009885)
> +		local_irq_restore(flags);
> +
> +	return ret;
> +}
> +
> +/* Reads from register regnum in the PHY for device dev, returning the value.
> + * Clears miimcom first.  All PHY configuration has to be done through the
> + * TSEC1 MIIM regs.
> + */
> +static int xgmac_mdio_read_c45(struct mii_bus *bus, int phy_id, int dev_addr,
> +			       int regnum)
> +{
> +	struct mdio_fsl_priv *priv = (struct mdio_fsl_priv *)bus->priv;
> +	struct tgec_mdio_controller __iomem *regs = priv->mdio_base;
> +	bool endian = priv->is_little_endian;
> +	u32 mdio_stat, mdio_ctl;
> +	unsigned long flags;
> +	int ret;
> +
> +	mdio_stat = xgmac_read32(&regs->mdio_stat, endian);
> +	mdio_stat |= MDIO_STAT_ENC;
> +
>  	xgmac_write32(mdio_stat, &regs->mdio_stat, endian);
>  
>  	ret = xgmac_wait_until_free(&bus->dev, regs, endian);
> @@ -218,13 +298,11 @@ static int xgmac_mdio_read(struct mii_bus *bus, int phy_id, int regnum)
>  	xgmac_write32(mdio_ctl, &regs->mdio_ctl, endian);
>  
>  	/* Set the register address */
> -	if (regnum & MII_ADDR_C45) {
> -		xgmac_write32(regnum & 0xffff, &regs->mdio_addr, endian);
> +	xgmac_write32(regnum & 0xffff, &regs->mdio_addr, endian);
>  
> -		ret = xgmac_wait_until_free(&bus->dev, regs, endian);
> -		if (ret)
> -			return ret;
> -	}
> +	ret = xgmac_wait_until_free(&bus->dev, regs, endian);
> +	if (ret)
> +		return ret;
>  
>  	if (priv->has_a009885)
>  		/* Once the operation completes, i.e. MDIO_STAT_BSY clears, we
> @@ -326,8 +404,10 @@ static int xgmac_mdio_probe(struct platform_device *pdev)
>  		return -ENOMEM;
>  
>  	bus->name = "Freescale XGMAC MDIO Bus";
> -	bus->read = xgmac_mdio_read;
> -	bus->write = xgmac_mdio_write;
> +	bus->read = xgmac_mdio_read_c22;
> +	bus->write = xgmac_mdio_write_c22;
> +	bus->read_c45 = xgmac_mdio_read_c45;
> +	bus->write_c45 = xgmac_mdio_write_c45;
>  	bus->parent = &pdev->dev;
>  	bus->probe_capabilities = MDIOBUS_C22_C45;
>  	snprintf(bus->id, MII_BUS_ID_SIZE, "%pa", &res->start);
> -- 
> 2.36.0
>
Andrew Lunn May 10, 2022, 7:01 p.m. UTC | #2
On Tue, May 10, 2022 at 09:28:18PM +0300, Vladimir Oltean wrote:
> On Sun, May 08, 2022 at 05:30:46PM +0200, Andrew Lunn wrote:
> > The xgmac MDIO bus driver can perform both C22 and C45 transfers.
> > Create separate functions for each and register the C45 versions using
> > the new API calls where appropriate.
> > 
> > Signed-off-by: Andrew Lunn <andrew@lunn.ch>
> > ---
> >  drivers/net/ethernet/freescale/xgmac_mdio.c | 154 +++++++++++++++-----
> >  1 file changed, 117 insertions(+), 37 deletions(-)
> > 
> > diff --git a/drivers/net/ethernet/freescale/xgmac_mdio.c b/drivers/net/ethernet/freescale/xgmac_mdio.c
> > index ec90da1de030..ddfe6bf1f231 100644
> > --- a/drivers/net/ethernet/freescale/xgmac_mdio.c
> > +++ b/drivers/net/ethernet/freescale/xgmac_mdio.c
> > @@ -128,30 +128,59 @@ static int xgmac_wait_until_done(struct device *dev,
> >  	return 0;
> >  }
> >  
> > -/*
> > - * Write value to the PHY for this device to the register at regnum,waiting
> > +/* Write value to the PHY for this device to the register at regnum,waiting
> >   * until the write is done before it returns.  All PHY configuration has to be
> >   * done through the TSEC1 MIIM regs.
> >   */
> > -static int xgmac_mdio_write(struct mii_bus *bus, int phy_id, int regnum, u16 value)
> > +static int xgmac_mdio_write_c22(struct mii_bus *bus, int phy_id, int regnum,
> > +				u16 value)
> >  {
> >  	struct mdio_fsl_priv *priv = (struct mdio_fsl_priv *)bus->priv;
> >  	struct tgec_mdio_controller __iomem *regs = priv->mdio_base;
> > -	uint16_t dev_addr;
> > +	bool endian = priv->is_little_endian;
> >  	u32 mdio_ctl, mdio_stat;
> > +	u16 dev_addr;
> >  	int ret;
> > +
> > +	mdio_stat = xgmac_read32(&regs->mdio_stat, endian);
> > +	dev_addr = regnum & 0x1f;
> 
> Please move this either to the variable declaration, or near the mdio_ctl write,
> or just integrate it into the macro argument.

There are masks like this in some drivers, others don't. Since for the
majority of the MDIO bus drivers i don't have the hardware i was
trying to keep my changes to a minimum, so i'm less likely to break
it.

Once we have all the bus drivers converted, we can validate all the
requests in the core to guarantee no users are passing invalid values
to the drivers. And then all these masks can be removed.

> 
> > +	mdio_stat &= ~MDIO_STAT_ENC;
> > +
> 
> You can remove this empty line during read-modify-write patterns.

Sure, but just an FYI: the old code probably did it that way. My aim
is to split C22 from C45, not re-write/clean up every driver. I have
over 40 patches in total, without doing cleanups.

   Andrew
Vladimir Oltean May 10, 2022, 7:09 p.m. UTC | #3
On Tue, May 10, 2022 at 09:01:58PM +0200, Andrew Lunn wrote:
> On Tue, May 10, 2022 at 09:28:18PM +0300, Vladimir Oltean wrote:
> > On Sun, May 08, 2022 at 05:30:46PM +0200, Andrew Lunn wrote:
> > > The xgmac MDIO bus driver can perform both C22 and C45 transfers.
> > > Create separate functions for each and register the C45 versions using
> > > the new API calls where appropriate.
> > > 
> > > Signed-off-by: Andrew Lunn <andrew@lunn.ch>
> > > ---
> > >  drivers/net/ethernet/freescale/xgmac_mdio.c | 154 +++++++++++++++-----
> > >  1 file changed, 117 insertions(+), 37 deletions(-)
> > > 
> > > diff --git a/drivers/net/ethernet/freescale/xgmac_mdio.c b/drivers/net/ethernet/freescale/xgmac_mdio.c
> > > index ec90da1de030..ddfe6bf1f231 100644
> > > --- a/drivers/net/ethernet/freescale/xgmac_mdio.c
> > > +++ b/drivers/net/ethernet/freescale/xgmac_mdio.c
> > > @@ -128,30 +128,59 @@ static int xgmac_wait_until_done(struct device *dev,
> > >  	return 0;
> > >  }
> > >  
> > > -/*
> > > - * Write value to the PHY for this device to the register at regnum,waiting
> > > +/* Write value to the PHY for this device to the register at regnum,waiting
> > >   * until the write is done before it returns.  All PHY configuration has to be
> > >   * done through the TSEC1 MIIM regs.
> > >   */
> > > -static int xgmac_mdio_write(struct mii_bus *bus, int phy_id, int regnum, u16 value)
> > > +static int xgmac_mdio_write_c22(struct mii_bus *bus, int phy_id, int regnum,
> > > +				u16 value)
> > >  {
> > >  	struct mdio_fsl_priv *priv = (struct mdio_fsl_priv *)bus->priv;
> > >  	struct tgec_mdio_controller __iomem *regs = priv->mdio_base;
> > > -	uint16_t dev_addr;
> > > +	bool endian = priv->is_little_endian;
> > >  	u32 mdio_ctl, mdio_stat;
> > > +	u16 dev_addr;
> > >  	int ret;
> > > +
> > > +	mdio_stat = xgmac_read32(&regs->mdio_stat, endian);
> > > +	dev_addr = regnum & 0x1f;
> > 
> > Please move this either to the variable declaration, or near the mdio_ctl write,
> > or just integrate it into the macro argument.
> 
> There are masks like this in some drivers, others don't. Since for the
> majority of the MDIO bus drivers i don't have the hardware i was
> trying to keep my changes to a minimum, so i'm less likely to break
> it.
> 
> Once we have all the bus drivers converted, we can validate all the
> requests in the core to guarantee no users are passing invalid values
> to the drivers. And then all these masks can be removed.

Sure, I was going to revisit this comment, keep the masking with 0x1f,
I remembered in the meanwhile that it's supposed to represent
MII_MMD_CTRL_DEVAD_MASK, and that there is other stuff potentially
encoded in the devad, like post-increment stuff.

> > 
> > > +	mdio_stat &= ~MDIO_STAT_ENC;
> > > +
> > 
> > You can remove this empty line during read-modify-write patterns.
> 
> Sure, but just an FYI: the old code probably did it that way. My aim
> is to split C22 from C45, not re-write/clean up every driver. I have
> over 40 patches in total, without doing cleanups.

You are modifying this part of the code anyway.
diff mbox series

Patch

diff --git a/drivers/net/ethernet/freescale/xgmac_mdio.c b/drivers/net/ethernet/freescale/xgmac_mdio.c
index ec90da1de030..ddfe6bf1f231 100644
--- a/drivers/net/ethernet/freescale/xgmac_mdio.c
+++ b/drivers/net/ethernet/freescale/xgmac_mdio.c
@@ -128,30 +128,59 @@  static int xgmac_wait_until_done(struct device *dev,
 	return 0;
 }
 
-/*
- * Write value to the PHY for this device to the register at regnum,waiting
+/* Write value to the PHY for this device to the register at regnum,waiting
  * until the write is done before it returns.  All PHY configuration has to be
  * done through the TSEC1 MIIM regs.
  */
-static int xgmac_mdio_write(struct mii_bus *bus, int phy_id, int regnum, u16 value)
+static int xgmac_mdio_write_c22(struct mii_bus *bus, int phy_id, int regnum,
+				u16 value)
 {
 	struct mdio_fsl_priv *priv = (struct mdio_fsl_priv *)bus->priv;
 	struct tgec_mdio_controller __iomem *regs = priv->mdio_base;
-	uint16_t dev_addr;
+	bool endian = priv->is_little_endian;
 	u32 mdio_ctl, mdio_stat;
+	u16 dev_addr;
 	int ret;
+
+	mdio_stat = xgmac_read32(&regs->mdio_stat, endian);
+	dev_addr = regnum & 0x1f;
+	mdio_stat &= ~MDIO_STAT_ENC;
+
+	xgmac_write32(mdio_stat, &regs->mdio_stat, endian);
+
+	ret = xgmac_wait_until_free(&bus->dev, regs, endian);
+	if (ret)
+		return ret;
+
+	/* Set the port and dev addr */
+	mdio_ctl = MDIO_CTL_PORT_ADDR(phy_id) | MDIO_CTL_DEV_ADDR(dev_addr);
+	xgmac_write32(mdio_ctl, &regs->mdio_ctl, endian);
+
+	/* Write the value to the register */
+	xgmac_write32(MDIO_DATA(value), &regs->mdio_data, endian);
+
+	ret = xgmac_wait_until_done(&bus->dev, regs, endian);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+/* Write value to the PHY for this device to the register at regnum,waiting
+ * until the write is done before it returns.  All PHY configuration has to be
+ * done through the TSEC1 MIIM regs.
+ */
+static int xgmac_mdio_write_c45(struct mii_bus *bus, int phy_id, int dev_addr,
+				int regnum, u16 value)
+{
+	struct mdio_fsl_priv *priv = (struct mdio_fsl_priv *)bus->priv;
+	struct tgec_mdio_controller __iomem *regs = priv->mdio_base;
 	bool endian = priv->is_little_endian;
+	u32 mdio_ctl, mdio_stat;
+	int ret;
 
 	mdio_stat = xgmac_read32(&regs->mdio_stat, endian);
-	if (regnum & MII_ADDR_C45) {
-		/* Clause 45 (ie 10G) */
-		dev_addr = (regnum >> 16) & 0x1f;
-		mdio_stat |= MDIO_STAT_ENC;
-	} else {
-		/* Clause 22 (ie 1G) */
-		dev_addr = regnum & 0x1f;
-		mdio_stat &= ~MDIO_STAT_ENC;
-	}
+	mdio_stat |= MDIO_STAT_ENC;
 
 	xgmac_write32(mdio_stat, &regs->mdio_stat, endian);
 
@@ -164,13 +193,11 @@  static int xgmac_mdio_write(struct mii_bus *bus, int phy_id, int regnum, u16 val
 	xgmac_write32(mdio_ctl, &regs->mdio_ctl, endian);
 
 	/* Set the register address */
-	if (regnum & MII_ADDR_C45) {
-		xgmac_write32(regnum & 0xffff, &regs->mdio_addr, endian);
+	xgmac_write32(regnum & 0xffff, &regs->mdio_addr, endian);
 
-		ret = xgmac_wait_until_free(&bus->dev, regs, endian);
-		if (ret)
-			return ret;
-	}
+	ret = xgmac_wait_until_free(&bus->dev, regs, endian);
+	if (ret)
+		return ret;
 
 	/* Write the value to the register */
 	xgmac_write32(MDIO_DATA(value), &regs->mdio_data, endian);
@@ -182,31 +209,84 @@  static int xgmac_mdio_write(struct mii_bus *bus, int phy_id, int regnum, u16 val
 	return 0;
 }
 
-/*
- * Reads from register regnum in the PHY for device dev, returning the value.
+/* Reads from register regnum in the PHY for device dev, returning the value.
  * Clears miimcom first.  All PHY configuration has to be done through the
  * TSEC1 MIIM regs.
  */
-static int xgmac_mdio_read(struct mii_bus *bus, int phy_id, int regnum)
+static int xgmac_mdio_read_c22(struct mii_bus *bus, int phy_id, int regnum)
 {
 	struct mdio_fsl_priv *priv = (struct mdio_fsl_priv *)bus->priv;
 	struct tgec_mdio_controller __iomem *regs = priv->mdio_base;
+	bool endian = priv->is_little_endian;
 	unsigned long flags;
-	uint16_t dev_addr;
 	uint32_t mdio_stat;
 	uint32_t mdio_ctl;
+	u16 dev_addr;
 	int ret;
-	bool endian = priv->is_little_endian;
 
 	mdio_stat = xgmac_read32(&regs->mdio_stat, endian);
-	if (regnum & MII_ADDR_C45) {
-		dev_addr = (regnum >> 16) & 0x1f;
-		mdio_stat |= MDIO_STAT_ENC;
+	dev_addr = regnum & 0x1f;
+	mdio_stat &= ~MDIO_STAT_ENC;
+
+	xgmac_write32(mdio_stat, &regs->mdio_stat, endian);
+
+	ret = xgmac_wait_until_free(&bus->dev, regs, endian);
+	if (ret)
+		return ret;
+
+	/* Set the Port and Device Addrs */
+	mdio_ctl = MDIO_CTL_PORT_ADDR(phy_id) | MDIO_CTL_DEV_ADDR(dev_addr);
+	xgmac_write32(mdio_ctl, &regs->mdio_ctl, endian);
+
+	if (priv->has_a009885)
+		/* Once the operation completes, i.e. MDIO_STAT_BSY clears, we
+		 * must read back the data register within 16 MDC cycles.
+		 */
+		local_irq_save(flags);
+
+	/* Initiate the read */
+	xgmac_write32(mdio_ctl | MDIO_CTL_READ, &regs->mdio_ctl, endian);
+
+	ret = xgmac_wait_until_done(&bus->dev, regs, endian);
+	if (ret)
+		goto irq_restore;
+
+	/* Return all Fs if nothing was there */
+	if ((xgmac_read32(&regs->mdio_stat, endian) & MDIO_STAT_RD_ER) &&
+	    !priv->has_a011043) {
+		dev_dbg(&bus->dev,
+			"Error while reading PHY%d reg at %d.%d\n",
+			phy_id, dev_addr, regnum);
+		ret = 0xffff;
 	} else {
-		dev_addr = regnum & 0x1f;
-		mdio_stat &= ~MDIO_STAT_ENC;
+		ret = xgmac_read32(&regs->mdio_data, endian) & 0xffff;
+		dev_dbg(&bus->dev, "read %04x\n", ret);
 	}
 
+irq_restore:
+	if (priv->has_a009885)
+		local_irq_restore(flags);
+
+	return ret;
+}
+
+/* Reads from register regnum in the PHY for device dev, returning the value.
+ * Clears miimcom first.  All PHY configuration has to be done through the
+ * TSEC1 MIIM regs.
+ */
+static int xgmac_mdio_read_c45(struct mii_bus *bus, int phy_id, int dev_addr,
+			       int regnum)
+{
+	struct mdio_fsl_priv *priv = (struct mdio_fsl_priv *)bus->priv;
+	struct tgec_mdio_controller __iomem *regs = priv->mdio_base;
+	bool endian = priv->is_little_endian;
+	u32 mdio_stat, mdio_ctl;
+	unsigned long flags;
+	int ret;
+
+	mdio_stat = xgmac_read32(&regs->mdio_stat, endian);
+	mdio_stat |= MDIO_STAT_ENC;
+
 	xgmac_write32(mdio_stat, &regs->mdio_stat, endian);
 
 	ret = xgmac_wait_until_free(&bus->dev, regs, endian);
@@ -218,13 +298,11 @@  static int xgmac_mdio_read(struct mii_bus *bus, int phy_id, int regnum)
 	xgmac_write32(mdio_ctl, &regs->mdio_ctl, endian);
 
 	/* Set the register address */
-	if (regnum & MII_ADDR_C45) {
-		xgmac_write32(regnum & 0xffff, &regs->mdio_addr, endian);
+	xgmac_write32(regnum & 0xffff, &regs->mdio_addr, endian);
 
-		ret = xgmac_wait_until_free(&bus->dev, regs, endian);
-		if (ret)
-			return ret;
-	}
+	ret = xgmac_wait_until_free(&bus->dev, regs, endian);
+	if (ret)
+		return ret;
 
 	if (priv->has_a009885)
 		/* Once the operation completes, i.e. MDIO_STAT_BSY clears, we
@@ -326,8 +404,10 @@  static int xgmac_mdio_probe(struct platform_device *pdev)
 		return -ENOMEM;
 
 	bus->name = "Freescale XGMAC MDIO Bus";
-	bus->read = xgmac_mdio_read;
-	bus->write = xgmac_mdio_write;
+	bus->read = xgmac_mdio_read_c22;
+	bus->write = xgmac_mdio_write_c22;
+	bus->read_c45 = xgmac_mdio_read_c45;
+	bus->write_c45 = xgmac_mdio_write_c45;
 	bus->parent = &pdev->dev;
 	bus->probe_capabilities = MDIOBUS_C22_C45;
 	snprintf(bus->id, MII_BUS_ID_SIZE, "%pa", &res->start);