net: phy: broadcom: add support for BCM5221 phy

Message ID 20230811215322.8679-1-giulio.benetti@benettiengineering.com
State New
Headers
Series net: phy: broadcom: add support for BCM5221 phy |

Commit Message

Giulio Benetti Aug. 11, 2023, 9:53 p.m. UTC
  This patch adds the BCM5221 PHY support by reusing
brcm_fet_config_intr() and brcm_fet_handle_interrupt() and
implementing config_init()/suspend()/resume().

Sponsored by: Tekvox Inc.
Cc: Jim Reinhart <jimr@tekvox.com>
Cc: James Autry <jautry@tekvox.com>
Cc: Matthew Maron <matthewm@tekvox.com>
Signed-off-by: Giulio Benetti <giulio.benetti@benettiengineering.com>
---
 drivers/net/phy/broadcom.c | 144 +++++++++++++++++++++++++++++++++++++
 include/linux/brcmphy.h    |   8 +++
 2 files changed, 152 insertions(+)
  

Comments

Florian Fainelli Aug. 11, 2023, 10:39 p.m. UTC | #1
On 8/11/23 15:16, Andrew Lunn wrote:
> On Fri, Aug 11, 2023 at 11:53:22PM +0200, Giulio Benetti wrote:
>> This patch adds the BCM5221 PHY support by reusing
>> brcm_fet_config_intr() and brcm_fet_handle_interrupt() and
>> implementing config_init()/suspend()/resume().
>>
>> Sponsored by: Tekvox Inc.
> 
> That is a new tag. Maybe you should update
> Documentation/process/submitting-patches.rst ?
> 
>> +static int bcm5221_config_init(struct phy_device *phydev)
>> +{
>> +	int reg, err, err2, brcmtest;
>> +
>> +	/* Reset the PHY to bring it to a known state. */
>> +	err = phy_write(phydev, MII_BMCR, BMCR_RESET);
>> +	if (err < 0)
>> +		return err;
>> +
>> +	/* The datasheet indicates the PHY needs up to 1us to complete a reset,
>> +	 * build some slack here.
>> +	 */
>> +	usleep_range(1000, 2000);
>> +
>> +	/* The PHY requires 65 MDC clock cycles to complete a write operation
>> +	 * and turnaround the line properly.
>> +	 *
>> +	 * We ignore -EIO here as the MDIO controller (e.g.: mdio-bcm-unimac)
>> +	 * may flag the lack of turn-around as a read failure. This is
>> +	 * particularly true with this combination since the MDIO controller
>> +	 * only used 64 MDC cycles. This is not a critical failure in this
>> +	 * specific case and it has no functional impact otherwise, so we let
>> +	 * that one go through. If there is a genuine bus error, the next read
>> +	 * of MII_BRCM_FET_INTREG will error out.
>> +	 */
>> +	err = phy_read(phydev, MII_BMCR);
>> +	if (err < 0 && err != -EIO)
>> +		return err;
> 
> It is pretty normal to check the value of MII_BMCR and ensure that
> BMCR_RESET has cleared. See phy_poll_reset(). It might not be needed,
> if you trust the datasheet, but 802.3 C22 says it should clear.
> 
>> +	/* Enable auto MDIX */
>> +	err = phy_clear_bits(phydev, BCM5221_AEGSR, BCM5221_AEGSR_MDIX_DIS);
>> +	if (err < 0)
>> +		return err;
> 
> It is better to set it based on phydev->mdix_ctrl.
> 
>> @@ -1288,6 +1431,7 @@ static struct mdio_device_id __maybe_unused broadcom_tbl[] = {
>>   	{ PHY_ID_BCM53125, 0xfffffff0 },
>>   	{ PHY_ID_BCM53128, 0xfffffff0 },
>>   	{ PHY_ID_BCM89610, 0xfffffff0 },
>> +	{ PHY_ID_BCM5221, 0xfffffff0 },
> 
> This table has some sort of sorting. I would put this new entry before
> PHY_ID_BCM5241.
> 
>>   #define PHY_ID_BCM50610			0x0143bd60
>>   #define PHY_ID_BCM50610M		0x0143bd70
>>   #define PHY_ID_BCM5241			0x0143bc30
>> +#define PHY_ID_BCM5221			0x004061e0
> 
> The value looks odd. Is the OUI correct? Is that a broadcom OUI?

Yes it is correct and yes it is assigned to Broadcom.
  
Florian Fainelli Aug. 11, 2023, 11:52 p.m. UTC | #2
On 8/11/2023 2:53 PM, Giulio Benetti wrote:
> This patch adds the BCM5221 PHY support by reusing
> brcm_fet_config_intr() and brcm_fet_handle_interrupt() and
> implementing config_init()/suspend()/resume().
> 
> Sponsored by: Tekvox Inc.
> Cc: Jim Reinhart <jimr@tekvox.com>
> Cc: James Autry <jautry@tekvox.com>
> Cc: Matthew Maron <matthewm@tekvox.com>
> Signed-off-by: Giulio Benetti <giulio.benetti@benettiengineering.com>

Looks good, few comments below.

> ---
>   drivers/net/phy/broadcom.c | 144 +++++++++++++++++++++++++++++++++++++
>   include/linux/brcmphy.h    |   8 +++
>   2 files changed, 152 insertions(+)
> 
> diff --git a/drivers/net/phy/broadcom.c b/drivers/net/phy/broadcom.c
> index 59cae0d808aa..99f6c0485f01 100644
> --- a/drivers/net/phy/broadcom.c
> +++ b/drivers/net/phy/broadcom.c
> @@ -754,6 +754,84 @@ static int brcm_fet_config_init(struct phy_device *phydev)
>   	return err;
>   }
>   
> +static int bcm5221_config_init(struct phy_device *phydev)
> +{

Very similar to brcm_fet_config_init() except that you configure fewer 
interrupt sources, do not have the LED mode programming and your MDI-X 
programming is done via a 10BaseT specific register rather than the shadow.

Can you consider adding parameters to brcm_fet_config_init() such that 
you can specify the 5221 specific settings such as the interrupt mask 
for instance?

This should help address the locking that Russell suggested.

[snip]
>   
> +static int bcm5221_suspend(struct phy_device *phydev)
> +{
> +	int reg, err, err2, brcmtest;
> +
> +	/* Enable shadow register access */
> +	brcmtest = phy_read(phydev, MII_BRCM_FET_BRCMTEST);
> +	if (brcmtest < 0)
> +		return brcmtest;
> +
> +	reg = brcmtest | MII_BRCM_FET_BT_SRE;
> +
> +	err = phy_write(phydev, MII_BRCM_FET_BRCMTEST, reg);
> +	if (err < 0)
> +		return err;
> +
> +	/* Force Low Power Mode with clock enabled */
> +	err = phy_set_bits(phydev, MII_BRCM_FET_SHDW_AUXMODE4,
> +			   BCM5221_SHDW_AM4_EN_CLK_LPM |
> +			   BCM5221_SHDW_AM4_FORCE_LPM);
> +
> +	/* Disable shadow register access */
> +	err2 = phy_write(phydev, MII_BRCM_FET_BRCMTEST, brcmtest);
> +	if (!err)
> +		err = err2;
> +
> +	return err;
> +}

bcm5221_suspend() is essentially brcm_fet_suspend() minus the setting of 
the BMCR.PDOWN bit, can you consider factoring brcm_fet_suspend() into a 
helper that can decide whether to set the power down bit or not? Does 
not brcm_fet_suspend() work as-is?

> +
> +static int bcm5221_resume(struct phy_device *phydev)
> +{

This should really be calling bcm5221_config_init() here, if the PHY was 
on a power island that is powered off during system suspend, 
bcm5221_resume() would not be restoring the auto-power down that is set 
during config_init(), probably not desired because that means you will 
burn power when the cable is disconnected...
  
Andrew Lunn Aug. 12, 2023, 2:19 p.m. UTC | #3
> > This table has some sort of sorting. I would put this new entry before
> > PHY_ID_BCM5241.
> > 
> > >   #define PHY_ID_BCM50610			0x0143bd60
> > >   #define PHY_ID_BCM50610M		0x0143bd70
> > >   #define PHY_ID_BCM5241			0x0143bc30
> > > +#define PHY_ID_BCM5221			0x004061e0
> > 
> > The value looks odd. Is the OUI correct? Is that a broadcom OUI?
> 
> Yes it is correct and yes it is assigned to Broadcom.

I found a datasheet for this PHY and it does list this OUI. I just
noticed it is the only device supported by this driver which uses this
OUI, so i just wanted to check.

     Andrew
  
Giulio Benetti Aug. 12, 2023, 5:12 p.m. UTC | #4
Hello Andrew,

thank you for reviewing,

On 12/08/23 00:16, Andrew Lunn wrote:
> On Fri, Aug 11, 2023 at 11:53:22PM +0200, Giulio Benetti wrote:
>> This patch adds the BCM5221 PHY support by reusing
>> brcm_fet_config_intr() and brcm_fet_handle_interrupt() and
>> implementing config_init()/suspend()/resume().
>>
>> Sponsored by: Tekvox Inc.
> 
> That is a new tag. Maybe you should update
> Documentation/process/submitting-patches.rst ?

I've picked this tag from commit:
https://git.kernel.org/pub/scm/linux/kernel/git/torvalds/linux.git/commit/?id=73c289bac05919286f8c7e1660fcaf6ec0468363

it's not "Sponsored-by:", do you think it's better using this last form?

I can send a patch along with V2 of this one for that.

>> +static int bcm5221_config_init(struct phy_device *phydev)
>> +{
>> +	int reg, err, err2, brcmtest;
>> +
>> +	/* Reset the PHY to bring it to a known state. */
>> +	err = phy_write(phydev, MII_BMCR, BMCR_RESET);
>> +	if (err < 0)
>> +		return err;
>> +
>> +	/* The datasheet indicates the PHY needs up to 1us to complete a reset,
>> +	 * build some slack here.
>> +	 */
>> +	usleep_range(1000, 2000);
>> +
>> +	/* The PHY requires 65 MDC clock cycles to complete a write operation
>> +	 * and turnaround the line properly.
>> +	 *
>> +	 * We ignore -EIO here as the MDIO controller (e.g.: mdio-bcm-unimac)
>> +	 * may flag the lack of turn-around as a read failure. This is
>> +	 * particularly true with this combination since the MDIO controller
>> +	 * only used 64 MDC cycles. This is not a critical failure in this
>> +	 * specific case and it has no functional impact otherwise, so we let
>> +	 * that one go through. If there is a genuine bus error, the next read
>> +	 * of MII_BRCM_FET_INTREG will error out.
>> +	 */
>> +	err = phy_read(phydev, MII_BMCR);
>> +	if (err < 0 && err != -EIO)
>> +		return err;
> 
> It is pretty normal to check the value of MII_BMCR and ensure that
> BMCR_RESET has cleared. See phy_poll_reset(). It might not be needed,
> if you trust the datasheet, but 802.3 C22 says it should clear.

oh ok, I'll do that on V2

> 
>> +	/* Enable auto MDIX */
>> +	err = phy_clear_bits(phydev, BCM5221_AEGSR, BCM5221_AEGSR_MDIX_DIS);
>> +	if (err < 0)
>> +		return err;
> 
> It is better to set it based on phydev->mdix_ctrl.

ok,

> 
>> @@ -1288,6 +1431,7 @@ static struct mdio_device_id __maybe_unused broadcom_tbl[] = {
>>   	{ PHY_ID_BCM53125, 0xfffffff0 },
>>   	{ PHY_ID_BCM53128, 0xfffffff0 },
>>   	{ PHY_ID_BCM89610, 0xfffffff0 },
>> +	{ PHY_ID_BCM5221, 0xfffffff0 },
> 
> This table has some sort of sorting. I would put this new entry before
> PHY_ID_BCM5241.

right, this and even the struct phy_driver driver entry too then,

> 
>>   #define PHY_ID_BCM50610			0x0143bd60
>>   #define PHY_ID_BCM50610M		0x0143bd70
>>   #define PHY_ID_BCM5241			0x0143bc30
>> +#define PHY_ID_BCM5221			0x004061e0
> 
> The value looks odd. Is the OUI correct? Is that a broadcom OUI?

Yes, you can check it in the pdf page 37:
https://docs.broadcom.com/doc/5221-DS07-405-RDS.pdf
and yes, it's odd.

Thanks again for the review, I'll do the changes here and the others
suggested by the other and send V2 patch.

Best regards
  
Giulio Benetti Aug. 12, 2023, 5:15 p.m. UTC | #5
Hello Russell,

thanks for reviewing,

On 12/08/23 00:22, Russell King (Oracle) wrote:
> On Fri, Aug 11, 2023 at 11:53:22PM +0200, Giulio Benetti wrote:
>> +	reg = phy_read(phydev, MII_BRCM_FET_INTREG);
>> +	if (reg < 0)
>> +		return reg;
>> +
>> +	/* Unmask events we are interested in and mask interrupts globally. */
>> +	reg = MII_BRCM_FET_IR_ENABLE |
>> +	      MII_BRCM_FET_IR_MASK;
>> +
>> +	err = phy_write(phydev, MII_BRCM_FET_INTREG, reg);
>> +	if (err < 0)
>> +		return err;
> 
> Please explain why you read MII_BRCM_FET_INTREG, then discard its value
> and write a replacement value.

ok, yes, as it is it doesn't look self-explanatory at all

>> +
>> +	/* Enable auto MDIX */
>> +	err = phy_clear_bits(phydev, BCM5221_AEGSR, BCM5221_AEGSR_MDIX_DIS);
>> +	if (err < 0)
>> +		return err;
>> +
>> +	/* Enable shadow register access */
>> +	brcmtest = phy_read(phydev, MII_BRCM_FET_BRCMTEST);
>> +	if (brcmtest < 0)
>> +		return brcmtest;
>> +
>> +	reg = brcmtest | MII_BRCM_FET_BT_SRE;
>> +
>> +	err = phy_write(phydev, MII_BRCM_FET_BRCMTEST, reg);
>> +	if (err < 0)
>> +		return err;
> 
> I think you should consider locking the MDIO bus while the device is
> switched to the shadow register set, so that other accesses don't happen
> that may interfere with this.

oh, I haven't considered this, you're totally right,

>> +static int bcm5221_suspend(struct phy_device *phydev)
>> +{
>> +	int reg, err, err2, brcmtest;
>> +
>> +	/* Enable shadow register access */
>> +	brcmtest = phy_read(phydev, MII_BRCM_FET_BRCMTEST);
>> +	if (brcmtest < 0)
>> +		return brcmtest;
>> +
>> +	reg = brcmtest | MII_BRCM_FET_BT_SRE;
>> +
>> +	err = phy_write(phydev, MII_BRCM_FET_BRCMTEST, reg);
>> +	if (err < 0)
>> +		return err;
>> +
>> +	/* Force Low Power Mode with clock enabled */
>> +	err = phy_set_bits(phydev, MII_BRCM_FET_SHDW_AUXMODE4,
>> +			   BCM5221_SHDW_AM4_EN_CLK_LPM |
>> +			   BCM5221_SHDW_AM4_FORCE_LPM);
>> +
>> +	/* Disable shadow register access */
>> +	err2 = phy_write(phydev, MII_BRCM_FET_BRCMTEST, brcmtest);
>> +	if (!err)
>> +		err = err2;
> 
> Same here.

ok,

>> +
>> +	return err;
>> +}
>> +
>> +static int bcm5221_resume(struct phy_device *phydev)
>> +{
>> +	int reg, err, err2, brcmtest;
>> +
>> +	/* Enable shadow register access */
>> +	brcmtest = phy_read(phydev, MII_BRCM_FET_BRCMTEST);
>> +	if (brcmtest < 0)
>> +		return brcmtest;
>> +
>> +	reg = brcmtest | MII_BRCM_FET_BT_SRE;
>> +
>> +	err = phy_write(phydev, MII_BRCM_FET_BRCMTEST, reg);
>> +	if (err < 0)
>> +		return err;
>> +
>> +	/* Exit Low Power Mode with clock enabled */
>> +	err = phy_clear_bits(phydev, MII_BRCM_FET_SHDW_AUXMODE4,
>> +			     BCM5221_SHDW_AM4_FORCE_LPM);
>> +
>> +	/* Disable shadow register access */
>> +	err2 = phy_write(phydev, MII_BRCM_FET_BRCMTEST, brcmtest);
>> +	if (!err)
>> +		err = err2;
> 
> And, of course, same here.

ok, will do on V2 along with the other point from Andrew and Florian.

Thanks again for the review,
Best regards
  
Giulio Benetti Aug. 12, 2023, 5:27 p.m. UTC | #6
Hello Florian,

thanks for reviewing,

On 12/08/23 01:52, Florian Fainelli wrote:
> 
> 
> On 8/11/2023 2:53 PM, Giulio Benetti wrote:
>> This patch adds the BCM5221 PHY support by reusing
>> brcm_fet_config_intr() and brcm_fet_handle_interrupt() and
>> implementing config_init()/suspend()/resume().
>>
>> Sponsored by: Tekvox Inc.
>> Cc: Jim Reinhart <jimr@tekvox.com>
>> Cc: James Autry <jautry@tekvox.com>
>> Cc: Matthew Maron <matthewm@tekvox.com>
>> Signed-off-by: Giulio Benetti <giulio.benetti@benettiengineering.com>
> 
> Looks good, few comments below.
> 
>> ---
>>   drivers/net/phy/broadcom.c | 144 +++++++++++++++++++++++++++++++++++++
>>   include/linux/brcmphy.h    |   8 +++
>>   2 files changed, 152 insertions(+)
>>
>> diff --git a/drivers/net/phy/broadcom.c b/drivers/net/phy/broadcom.c
>> index 59cae0d808aa..99f6c0485f01 100644
>> --- a/drivers/net/phy/broadcom.c
>> +++ b/drivers/net/phy/broadcom.c
>> @@ -754,6 +754,84 @@ static int brcm_fet_config_init(struct phy_device 
>> *phydev)
>>       return err;
>>   }
>> +static int bcm5221_config_init(struct phy_device *phydev)
>> +{
> 
> Very similar to brcm_fet_config_init() except that you configure fewer 
> interrupt sources, do not have the LED mode programming and your MDI-X 
> programming is done via a 10BaseT specific register rather than the shadow.
> 
> Can you consider adding parameters to brcm_fet_config_init() such that 
> you can specify the 5221 specific settings such as the interrupt mask 
> for instance?

So if I've understood correctly I should drop bcm5221_config_init() and
use brcm_fet_config_init() by checking the struct phy_driver .phy_id to
be PHY_ID_BCM5221.

> This should help address the locking that Russell suggested.

But I don't understand how this can help to prevent the locking Russell
suggested, can you please elaborate more or point me and example?
As far as I've understood I should take care to lock sections to prevent
from interrupts to occur during shadow mode switching, is it correct?
So maybe you mean that if I do this in brcm_fet_config_init() it will be
fixed for the other phys using brcm_fet_config_init(), correct?

> [snip]
>> +static int bcm5221_suspend(struct phy_device *phydev)
>> +{
>> +    int reg, err, err2, brcmtest;
>> +
>> +    /* Enable shadow register access */
>> +    brcmtest = phy_read(phydev, MII_BRCM_FET_BRCMTEST);
>> +    if (brcmtest < 0)
>> +        return brcmtest;
>> +
>> +    reg = brcmtest | MII_BRCM_FET_BT_SRE;
>> +
>> +    err = phy_write(phydev, MII_BRCM_FET_BRCMTEST, reg);
>> +    if (err < 0)
>> +        return err;
>> +
>> +    /* Force Low Power Mode with clock enabled */
>> +    err = phy_set_bits(phydev, MII_BRCM_FET_SHDW_AUXMODE4,
>> +               BCM5221_SHDW_AM4_EN_CLK_LPM |
>> +               BCM5221_SHDW_AM4_FORCE_LPM);
>> +
>> +    /* Disable shadow register access */
>> +    err2 = phy_write(phydev, MII_BRCM_FET_BRCMTEST, brcmtest);
>> +    if (!err)
>> +        err = err2;
>> +
>> +    return err;
>> +}
> 
> bcm5221_suspend() is essentially brcm_fet_suspend() minus the setting of 
> the BMCR.PDOWN bit, can you consider factoring brcm_fet_suspend() into a 
> helper that can decide whether to set the power down bit or not? Does 
> not brcm_fet_suspend() work as-is?

Sure, I will,

>> +
>> +static int bcm5221_resume(struct phy_device *phydev)
>> +{
> 
> This should really be calling bcm5221_config_init() here, if the PHY was 
> on a power island that is powered off during system suspend, 
> bcm5221_resume() would not be restoring the auto-power down that is set 
> during config_init(), probably not desired because that means you will 
> burn power when the cable is disconnected...

oh yes! You're right, I will then drop bcm5221_resume() in favor of 
bcm5221_config_init(), so brcm_fet_config_init() with phy_id == 
PHY_ID_BCM5221 checks.

Thanks again
Best regards
  

Patch

diff --git a/drivers/net/phy/broadcom.c b/drivers/net/phy/broadcom.c
index 59cae0d808aa..99f6c0485f01 100644
--- a/drivers/net/phy/broadcom.c
+++ b/drivers/net/phy/broadcom.c
@@ -754,6 +754,84 @@  static int brcm_fet_config_init(struct phy_device *phydev)
 	return err;
 }
 
+static int bcm5221_config_init(struct phy_device *phydev)
+{
+	int reg, err, err2, brcmtest;
+
+	/* Reset the PHY to bring it to a known state. */
+	err = phy_write(phydev, MII_BMCR, BMCR_RESET);
+	if (err < 0)
+		return err;
+
+	/* The datasheet indicates the PHY needs up to 1us to complete a reset,
+	 * build some slack here.
+	 */
+	usleep_range(1000, 2000);
+
+	/* The PHY requires 65 MDC clock cycles to complete a write operation
+	 * and turnaround the line properly.
+	 *
+	 * We ignore -EIO here as the MDIO controller (e.g.: mdio-bcm-unimac)
+	 * may flag the lack of turn-around as a read failure. This is
+	 * particularly true with this combination since the MDIO controller
+	 * only used 64 MDC cycles. This is not a critical failure in this
+	 * specific case and it has no functional impact otherwise, so we let
+	 * that one go through. If there is a genuine bus error, the next read
+	 * of MII_BRCM_FET_INTREG will error out.
+	 */
+	err = phy_read(phydev, MII_BMCR);
+	if (err < 0 && err != -EIO)
+		return err;
+
+	reg = phy_read(phydev, MII_BRCM_FET_INTREG);
+	if (reg < 0)
+		return reg;
+
+	/* Unmask events we are interested in and mask interrupts globally. */
+	reg = MII_BRCM_FET_IR_ENABLE |
+	      MII_BRCM_FET_IR_MASK;
+
+	err = phy_write(phydev, MII_BRCM_FET_INTREG, reg);
+	if (err < 0)
+		return err;
+
+	/* Enable auto MDIX */
+	err = phy_clear_bits(phydev, BCM5221_AEGSR, BCM5221_AEGSR_MDIX_DIS);
+	if (err < 0)
+		return err;
+
+	/* Enable shadow register access */
+	brcmtest = phy_read(phydev, MII_BRCM_FET_BRCMTEST);
+	if (brcmtest < 0)
+		return brcmtest;
+
+	reg = brcmtest | MII_BRCM_FET_BT_SRE;
+
+	err = phy_write(phydev, MII_BRCM_FET_BRCMTEST, reg);
+	if (err < 0)
+		return err;
+
+        /* Exit low power mode */
+	err = phy_clear_bits(phydev, MII_BRCM_FET_SHDW_AUXMODE4,
+			 BCM5221_SHDW_AM4_FORCE_LPM);
+	if (err < 0)
+		goto done;
+
+	if (phydev->dev_flags & PHY_BRCM_AUTO_PWRDWN_ENABLE) {
+		/* Enable auto power down */
+		err = phy_set_bits(phydev, MII_BRCM_FET_SHDW_AUXSTAT2,
+				   MII_BRCM_FET_SHDW_AS2_APDE);
+	}
+
+done:
+	/* Disable shadow register access */
+	err2 = phy_write(phydev, MII_BRCM_FET_BRCMTEST, brcmtest);
+	if (!err)
+		err = err2;
+
+	return err;
+}
+
 static int brcm_fet_ack_interrupt(struct phy_device *phydev)
 {
 	int reg;
@@ -882,6 +960,61 @@  static int bcm54xx_phy_set_wol(struct phy_device *phydev,
 	return 0;
 }
 
+static int bcm5221_suspend(struct phy_device *phydev)
+{
+	int reg, err, err2, brcmtest;
+
+	/* Enable shadow register access */
+	brcmtest = phy_read(phydev, MII_BRCM_FET_BRCMTEST);
+	if (brcmtest < 0)
+		return brcmtest;
+
+	reg = brcmtest | MII_BRCM_FET_BT_SRE;
+
+	err = phy_write(phydev, MII_BRCM_FET_BRCMTEST, reg);
+	if (err < 0)
+		return err;
+
+	/* Force Low Power Mode with clock enabled */
+	err = phy_set_bits(phydev, MII_BRCM_FET_SHDW_AUXMODE4,
+			   BCM5221_SHDW_AM4_EN_CLK_LPM |
+			   BCM5221_SHDW_AM4_FORCE_LPM);
+
+	/* Disable shadow register access */
+	err2 = phy_write(phydev, MII_BRCM_FET_BRCMTEST, brcmtest);
+	if (!err)
+		err = err2;
+
+	return err;
+}
+
+static int bcm5221_resume(struct phy_device *phydev)
+{
+	int reg, err, err2, brcmtest;
+
+	/* Enable shadow register access */
+	brcmtest = phy_read(phydev, MII_BRCM_FET_BRCMTEST);
+	if (brcmtest < 0)
+		return brcmtest;
+
+	reg = brcmtest | MII_BRCM_FET_BT_SRE;
+
+	err = phy_write(phydev, MII_BRCM_FET_BRCMTEST, reg);
+	if (err < 0)
+		return err;
+
+	/* Exit Low Power Mode with clock enabled */
+	err = phy_clear_bits(phydev, MII_BRCM_FET_SHDW_AUXMODE4,
+			     BCM5221_SHDW_AM4_FORCE_LPM);
+
+	/* Disable shadow register access */
+	err2 = phy_write(phydev, MII_BRCM_FET_BRCMTEST, brcmtest);
+	if (!err)
+		err = err2;
+
+	return err;
+}
+
 static int bcm54xx_phy_probe(struct phy_device *phydev)
 {
 	struct bcm54xx_phy_priv *priv;
@@ -1208,6 +1341,16 @@  static struct phy_driver broadcom_drivers[] = {
 	.handle_interrupt = brcm_fet_handle_interrupt,
 	.suspend	= brcm_fet_suspend,
 	.resume		= brcm_fet_config_init,
+}, {
+	.phy_id		= PHY_ID_BCM5221,
+	.phy_id_mask	= 0xfffffff0,
+	.name		= "Broadcom BCM5221",
+	/* PHY_BASIC_FEATURES */
+	.config_init	= bcm5221_config_init,
+	.config_intr	= brcm_fet_config_intr,
+	.handle_interrupt = brcm_fet_handle_interrupt,
+	.suspend	= bcm5221_suspend,
+	.resume		= bcm5221_resume,
 }, {
 	.phy_id		= PHY_ID_BCM5395,
 	.phy_id_mask	= 0xfffffff0,
@@ -1288,6 +1431,7 @@  static struct mdio_device_id __maybe_unused broadcom_tbl[] = {
 	{ PHY_ID_BCM53125, 0xfffffff0 },
 	{ PHY_ID_BCM53128, 0xfffffff0 },
 	{ PHY_ID_BCM89610, 0xfffffff0 },
+	{ PHY_ID_BCM5221, 0xfffffff0 },
 	{ }
 };
 
diff --git a/include/linux/brcmphy.h b/include/linux/brcmphy.h
index 5d732f48f787..3d7786cc997d 100644
--- a/include/linux/brcmphy.h
+++ b/include/linux/brcmphy.h
@@ -12,6 +12,7 @@ 
 #define PHY_ID_BCM50610			0x0143bd60
 #define PHY_ID_BCM50610M		0x0143bd70
 #define PHY_ID_BCM5241			0x0143bc30
+#define PHY_ID_BCM5221			0x004061e0
 #define PHY_ID_BCMAC131			0x0143bc70
 #define PHY_ID_BCM5481			0x0143bca0
 #define PHY_ID_BCM5395			0x0143bcf0
@@ -330,6 +331,13 @@ 
 
 #define BCM54XX_WOL_INT_STATUS		(MII_BCM54XX_EXP_SEL_WOL + 0x94)
 
+/* BCM5221 Registers */
+#define BCM5221_AEGSR			0x1C
+#define BCM5221_AEGSR_MDIX_DIS		BIT(11)
+
+#define BCM5221_SHDW_AM4_EN_CLK_LPM	BIT(2)
+#define BCM5221_SHDW_AM4_FORCE_LPM	BIT(1)
+
 /*****************************************************************************/
 /* Fast Ethernet Transceiver definitions. */
 /*****************************************************************************/