[PATCH net-next 0/2] net: phy: marvell-88x2222: a couple of improvements

[PATCH net-next 0/2] net: phy: marvell-88x2222: a couple of improvements

[Ivan Bornyakov]

First, there are some SFP modules that only uses RX_LOS for link
indication. Add check that SFP link is operational before actual read of
link status.

Second, it is invalid to set 10G speed without autonegotiation,
according to phy_ethtool_ksettings_set(). Implement switching between
10GBase-R and 1000Base-X/SGMII if autonegotiation can't complete but
there is signal in line.

Ivan Bornyakov (2):
  net: phy: marvell-88x2222: check that link is operational
  net: phy: marvell-88x2222: swap 1G/10G modes on autoneg

 drivers/net/phy/marvell-88x2222.c | 296 +++++++++++++++++++-----------
 1 file changed, 191 insertions(+), 105 deletions(-)

-- 
2.26.3

[PATCH net-next 1/2] net: phy: marvell-88x2222: check that link is operational

[Ivan Bornyakov]

Some SFP modules uses RX_LOS for link indication. In such cases link
will be always up, even without cable connected. RX_LOS changes will
trigger link_up()/link_down() upstream operations. Thus, check that SFP
link is operational before actual read link status.

Signed-off-by: Ivan Bornyakov <i.bornyakov@metrotek.ru>
---
 drivers/net/phy/marvell-88x2222.c | 26 ++++++++++++++++++++++++++
 1 file changed, 26 insertions(+)

diff --git a/drivers/net/phy/marvell-88x2222.c b/drivers/net/phy/marvell-88x2222.c
index eca8c2f20684..fb285ac741b2 100644
--- a/drivers/net/phy/marvell-88x2222.c
+++ b/drivers/net/phy/marvell-88x2222.c
@@ -51,6 +51,7 @@
 struct mv2222_data {
 	phy_interface_t line_interface;
 	__ETHTOOL_DECLARE_LINK_MODE_MASK(supported);
+	bool sfp_link;
 };
 
 /* SFI PMA transmit enable */
@@ -148,6 +149,9 @@ static int mv2222_read_status(struct phy_device *phydev)
 	phydev->speed = SPEED_UNKNOWN;
 	phydev->duplex = DUPLEX_UNKNOWN;
 
+	if (!priv->sfp_link)
+		return 0;
+
 	if (priv->line_interface == PHY_INTERFACE_MODE_10GBASER)
 		link = mv2222_read_status_10g(phydev);
 	else
@@ -446,9 +450,31 @@ static void mv2222_sfp_remove(void *upstream)
 	linkmode_zero(priv->supported);
 }
 
+static void mv2222_sfp_link_up(void *upstream)
+{
+	struct phy_device *phydev = upstream;
+	struct mv2222_data *priv;
+
+	priv = (struct mv2222_data *)phydev->priv;
+
+	priv->sfp_link = true;
+}
+
+static void mv2222_sfp_link_down(void *upstream)
+{
+	struct phy_device *phydev = upstream;
+	struct mv2222_data *priv;
+
+	priv = (struct mv2222_data *)phydev->priv;
+
+	priv->sfp_link = false;
+}
+
 static const struct sfp_upstream_ops sfp_phy_ops = {
 	.module_insert = mv2222_sfp_insert,
 	.module_remove = mv2222_sfp_remove,
+	.link_up = mv2222_sfp_link_up,
+	.link_down = mv2222_sfp_link_down,
 	.attach = phy_sfp_attach,
 	.detach = phy_sfp_detach,
 };
-- 
2.26.3

Re: [PATCH net-next 1/2] net: phy: marvell-88x2222: check that link is operational

[Marek Behún]

On Mon, 12 Apr 2021 15:16:59 +0300
Ivan Bornyakov <i.bornyakov@metrotek.ru> wrote:

> Some SFP modules uses RX_LOS for link indication. In such cases link
> will be always up, even without cable connected. RX_LOS changes will
> trigger link_up()/link_down() upstream operations. Thus, check that SFP
> link is operational before actual read link status.
> 
> Signed-off-by: Ivan Bornyakov <i.bornyakov@metrotek.ru>
> ---
>  drivers/net/phy/marvell-88x2222.c | 26 ++++++++++++++++++++++++++
>  1 file changed, 26 insertions(+)
> 
> diff --git a/drivers/net/phy/marvell-88x2222.c b/drivers/net/phy/marvell-88x2222.c
> index eca8c2f20684..fb285ac741b2 100644
> --- a/drivers/net/phy/marvell-88x2222.c
> +++ b/drivers/net/phy/marvell-88x2222.c
> @@ -51,6 +51,7 @@
>  struct mv2222_data {
>  	phy_interface_t line_interface;
>  	__ETHTOOL_DECLARE_LINK_MODE_MASK(supported);
> +	bool sfp_link;
>  };
>  
>  /* SFI PMA transmit enable */
> @@ -148,6 +149,9 @@ static int mv2222_read_status(struct phy_device *phydev)
>  	phydev->speed = SPEED_UNKNOWN;
>  	phydev->duplex = DUPLEX_UNKNOWN;
>  
> +	if (!priv->sfp_link)
> +		return 0;
> +

So if SFP is not used at all, if this PHY is used in a different
usecase, this function will always return 0? Is this correct?

>  	if (priv->line_interface == PHY_INTERFACE_MODE_10GBASER)
>  		link = mv2222_read_status_10g(phydev);
>  	else
> @@ -446,9 +450,31 @@ static void mv2222_sfp_remove(void *upstream)
>  	linkmode_zero(priv->supported);
>  }
>  
> +static void mv2222_sfp_link_up(void *upstream)
> +{
> +	struct phy_device *phydev = upstream;
> +	struct mv2222_data *priv;
> +
> +	priv = (struct mv2222_data *)phydev->priv;
> +
> +	priv->sfp_link = true;
> +}
> +
> +static void mv2222_sfp_link_down(void *upstream)
> +{
> +	struct phy_device *phydev = upstream;
> +	struct mv2222_data *priv;
> +
> +	priv = (struct mv2222_data *)phydev->priv;

This cast is redundant since the phydev->priv is (void*). You can cast
(void*) to (struct ... *).

You can also just use
	struct mv2222_data *priv = phydev->priv;

> +
> +	priv->sfp_link = false;
> +}
> +
>  static const struct sfp_upstream_ops sfp_phy_ops = {
>  	.module_insert = mv2222_sfp_insert,
>  	.module_remove = mv2222_sfp_remove,
> +	.link_up = mv2222_sfp_link_up,
> +	.link_down = mv2222_sfp_link_down,
>  	.attach = phy_sfp_attach,
>  	.detach = phy_sfp_detach,
>  };

Re: [PATCH net-next 1/2] net: phy: marvell-88x2222: check that link is operational

[Ivan Bornyakov]

On Tue, Apr 13, 2021 at 01:32:12AM +0200, Marek Behún wrote:
> On Mon, 12 Apr 2021 15:16:59 +0300
> Ivan Bornyakov <i.bornyakov@metrotek.ru> wrote:
> 
> > Some SFP modules uses RX_LOS for link indication. In such cases link
> > will be always up, even without cable connected. RX_LOS changes will
> > trigger link_up()/link_down() upstream operations. Thus, check that SFP
> > link is operational before actual read link status.
> > 
> > Signed-off-by: Ivan Bornyakov <i.bornyakov@metrotek.ru>
> > ---
> >  drivers/net/phy/marvell-88x2222.c | 26 ++++++++++++++++++++++++++
> >  1 file changed, 26 insertions(+)
> > 
> > diff --git a/drivers/net/phy/marvell-88x2222.c b/drivers/net/phy/marvell-88x2222.c
> > index eca8c2f20684..fb285ac741b2 100644
> > --- a/drivers/net/phy/marvell-88x2222.c
> > +++ b/drivers/net/phy/marvell-88x2222.c
> > @@ -51,6 +51,7 @@
> >  struct mv2222_data {
> >  	phy_interface_t line_interface;
> >  	__ETHTOOL_DECLARE_LINK_MODE_MASK(supported);
> > +	bool sfp_link;
> >  };
> >  
> >  /* SFI PMA transmit enable */
> > @@ -148,6 +149,9 @@ static int mv2222_read_status(struct phy_device *phydev)
> >  	phydev->speed = SPEED_UNKNOWN;
> >  	phydev->duplex = DUPLEX_UNKNOWN;
> >  
> > +	if (!priv->sfp_link)
> > +		return 0;
> > +
> 
> So if SFP is not used at all, if this PHY is used in a different
> usecase, this function will always return 0? Is this correct?
> 

Yes, probably. The thing is that I only have hardware with SFP cages, so
I realy don't know if this driver work in other usecases. The good thing
about open source is that other developers with different hardware
configurations can rework here and there and contribute back. Right?

> >  	if (priv->line_interface == PHY_INTERFACE_MODE_10GBASER)
> >  		link = mv2222_read_status_10g(phydev);
> >  	else
> > @@ -446,9 +450,31 @@ static void mv2222_sfp_remove(void *upstream)
> >  	linkmode_zero(priv->supported);
> >  }
> >  
> > +static void mv2222_sfp_link_up(void *upstream)
> > +{
> > +	struct phy_device *phydev = upstream;
> > +	struct mv2222_data *priv;
> > +
> > +	priv = (struct mv2222_data *)phydev->priv;
> > +
> > +	priv->sfp_link = true;
> > +}
> > +
> > +static void mv2222_sfp_link_down(void *upstream)
> > +{
> > +	struct phy_device *phydev = upstream;
> > +	struct mv2222_data *priv;
> > +
> > +	priv = (struct mv2222_data *)phydev->priv;
> 
> This cast is redundant since the phydev->priv is (void*). You can cast
> (void*) to (struct ... *).
> 
> You can also just use
> 	struct mv2222_data *priv = phydev->priv;
>

Yeah, I know, but reverse XMAS tree wouldn't line up.

> > +
> > +	priv->sfp_link = false;
> > +}
> > +
> >  static const struct sfp_upstream_ops sfp_phy_ops = {
> >  	.module_insert = mv2222_sfp_insert,
> >  	.module_remove = mv2222_sfp_remove,
> > +	.link_up = mv2222_sfp_link_up,
> > +	.link_down = mv2222_sfp_link_down,
> >  	.attach = phy_sfp_attach,
> >  	.detach = phy_sfp_detach,
> >  };
> 

Re: [PATCH net-next 1/2] net: phy: marvell-88x2222: check that link is operational

[Andrew Lunn]

On Tue, Apr 13, 2021 at 10:13:49AM +0300, Ivan Bornyakov wrote:
> On Tue, Apr 13, 2021 at 01:32:12AM +0200, Marek Behún wrote:
> > On Mon, 12 Apr 2021 15:16:59 +0300
> > Ivan Bornyakov <i.bornyakov@metrotek.ru> wrote:
> > 
> > > Some SFP modules uses RX_LOS for link indication. In such cases link
> > > will be always up, even without cable connected. RX_LOS changes will
> > > trigger link_up()/link_down() upstream operations. Thus, check that SFP
> > > link is operational before actual read link status.
> > > 
> > > Signed-off-by: Ivan Bornyakov <i.bornyakov@metrotek.ru>
> > > ---
> > >  drivers/net/phy/marvell-88x2222.c | 26 ++++++++++++++++++++++++++
> > >  1 file changed, 26 insertions(+)
> > > 
> > > diff --git a/drivers/net/phy/marvell-88x2222.c b/drivers/net/phy/marvell-88x2222.c
> > > index eca8c2f20684..fb285ac741b2 100644
> > > --- a/drivers/net/phy/marvell-88x2222.c
> > > +++ b/drivers/net/phy/marvell-88x2222.c
> > > @@ -51,6 +51,7 @@
> > >  struct mv2222_data {
> > >  	phy_interface_t line_interface;
> > >  	__ETHTOOL_DECLARE_LINK_MODE_MASK(supported);
> > > +	bool sfp_link;
> > >  };
> > >  
> > >  /* SFI PMA transmit enable */
> > > @@ -148,6 +149,9 @@ static int mv2222_read_status(struct phy_device *phydev)
> > >  	phydev->speed = SPEED_UNKNOWN;
> > >  	phydev->duplex = DUPLEX_UNKNOWN;
> > >  
> > > +	if (!priv->sfp_link)
> > > +		return 0;
> > > +
> > 
> > So if SFP is not used at all, if this PHY is used in a different
> > usecase, this function will always return 0? Is this correct?
> > 
> 
> Yes, probably. The thing is that I only have hardware with SFP cages, so
> I realy don't know if this driver work in other usecases.

It is O.K, to say you don't know if this will work for other setups,
but it is different thing to do something which could potentially
break those other setup. Somebody trying to use this without an SFP is
going to have a bad experience because of this change. And then they
are going to have to try to fix this, potentially breaking your setup.

if you truly need this, make it conditional on that you know you have
an SFP cage connected.

> > > +static void mv2222_sfp_link_down(void *upstream)
> > > +{
> > > +	struct phy_device *phydev = upstream;
> > > +	struct mv2222_data *priv;
> > > +
> > > +	priv = (struct mv2222_data *)phydev->priv;
> > 
> > This cast is redundant since the phydev->priv is (void*). You can cast
> > (void*) to (struct ... *).
> > 
> > You can also just use
> > 	struct mv2222_data *priv = phydev->priv;
> >
> 
> Yeah, I know, but reverse XMAS tree wouldn't line up.

Please move the assignment into the body of the function.

       Andrew

Re: [PATCH net-next 1/2] net: phy: marvell-88x2222: check that link is operational

[Andrew Lunn]

On Mon, Apr 12, 2021 at 03:16:59PM +0300, Ivan Bornyakov wrote:
> Some SFP modules uses RX_LOS for link indication. In such cases link
> will be always up, even without cable connected. RX_LOS changes will
> trigger link_up()/link_down() upstream operations. Thus, check that SFP
> link is operational before actual read link status.

Sorry, but this is not making much sense to me.

LOS just indicates some sort of light is coming into the device. You
have no idea what sort of light. The transceiver might be able to
decode that light and get sync, it might not. It is important that
mv2222_read_status() returns the line side status. Has it been able to
achieve sync? That should be independent of LOS. Or are you saying the
transceiver is reporting sync, despite no light coming in?

	Andrew

Re: [PATCH net-next 1/2] net: phy: marvell-88x2222: check that link is operational

[Ivan Bornyakov]

On Tue, Apr 13, 2021 at 01:40:32AM +0200, Andrew Lunn wrote:
> On Mon, Apr 12, 2021 at 03:16:59PM +0300, Ivan Bornyakov wrote:
> > Some SFP modules uses RX_LOS for link indication. In such cases link
> > will be always up, even without cable connected. RX_LOS changes will
> > trigger link_up()/link_down() upstream operations. Thus, check that SFP
> > link is operational before actual read link status.
> 
> Sorry, but this is not making much sense to me.
> 
> LOS just indicates some sort of light is coming into the device. You
> have no idea what sort of light. The transceiver might be able to
> decode that light and get sync, it might not. It is important that
> mv2222_read_status() returns the line side status. Has it been able to
> achieve sync? That should be independent of LOS. Or are you saying the
> transceiver is reporting sync, despite no light coming in?
> 
> 	Andrew

Yes, with some SFP modules transceiver is reporting sync despite no
light coming in. So, the idea is to check that link is somewhat
operational before determing line-side status. 

Re: [PATCH net-next 1/2] net: phy: marvell-88x2222: check that link is operational

[Russell King - ARM Linux admin]

On Tue, Apr 13, 2021 at 10:19:30AM +0300, Ivan Bornyakov wrote:
> On Tue, Apr 13, 2021 at 01:40:32AM +0200, Andrew Lunn wrote:
> > On Mon, Apr 12, 2021 at 03:16:59PM +0300, Ivan Bornyakov wrote:
> > > Some SFP modules uses RX_LOS for link indication. In such cases link
> > > will be always up, even without cable connected. RX_LOS changes will
> > > trigger link_up()/link_down() upstream operations. Thus, check that SFP
> > > link is operational before actual read link status.
> > 
> > Sorry, but this is not making much sense to me.
> > 
> > LOS just indicates some sort of light is coming into the device. You
> > have no idea what sort of light. The transceiver might be able to
> > decode that light and get sync, it might not. It is important that
> > mv2222_read_status() returns the line side status. Has it been able to
> > achieve sync? That should be independent of LOS. Or are you saying the
> > transceiver is reporting sync, despite no light coming in?
> > 
> > 	Andrew
> 
> Yes, with some SFP modules transceiver is reporting sync despite no
> light coming in. So, the idea is to check that link is somewhat
> operational before determing line-side status. 

Indeed - it should be a logical and operation - there is light present
_and_ the PHY recognises the signal. This is what the commit achieves,
although (iirc) doesn't cater for the case where there is no SFP cage
attached.

-- 
RMK's Patch system: https://www.armlinux.org.uk/developer/patches/
FTTP is here! 40Mbps down 10Mbps up. Decent connectivity at last!

Re: [PATCH net-next 1/2] net: phy: marvell-88x2222: check that link is operational

[Ivan Bornyakov]

On Tue, Apr 13, 2021 at 10:23:48AM +0100, Russell King - ARM Linux admin wrote:
> On Tue, Apr 13, 2021 at 10:19:30AM +0300, Ivan Bornyakov wrote:
> > On Tue, Apr 13, 2021 at 01:40:32AM +0200, Andrew Lunn wrote:
> > > On Mon, Apr 12, 2021 at 03:16:59PM +0300, Ivan Bornyakov wrote:
> > > > Some SFP modules uses RX_LOS for link indication. In such cases link
> > > > will be always up, even without cable connected. RX_LOS changes will
> > > > trigger link_up()/link_down() upstream operations. Thus, check that SFP
> > > > link is operational before actual read link status.
> > > 
> > > Sorry, but this is not making much sense to me.
> > > 
> > > LOS just indicates some sort of light is coming into the device. You
> > > have no idea what sort of light. The transceiver might be able to
> > > decode that light and get sync, it might not. It is important that
> > > mv2222_read_status() returns the line side status. Has it been able to
> > > achieve sync? That should be independent of LOS. Or are you saying the
> > > transceiver is reporting sync, despite no light coming in?
> > > 
> > > 	Andrew
> > 
> > Yes, with some SFP modules transceiver is reporting sync despite no
> > light coming in. So, the idea is to check that link is somewhat
> > operational before determing line-side status. 
> 
> Indeed - it should be a logical and operation - there is light present
> _and_ the PHY recognises the signal. This is what the commit achieves,
> although (iirc) doesn't cater for the case where there is no SFP cage
> attached.
> 
> -- 
> RMK's Patch system: https://www.armlinux.org.uk/developer/patches/
> FTTP is here! 40Mbps down 10Mbps up. Decent connectivity at last!

Correct, it does not, I only have HW with SFP cage attached.

Re: [PATCH net-next 1/2] net: phy: marvell-88x2222: check that link is operational

[Andrew Lunn]

> Indeed - it should be a logical and operation - there is light present
> _and_ the PHY recognises the signal. This is what the commit achieves,
> although (iirc) doesn't cater for the case where there is no SFP cage
> attached.

Hi Russell

Is there something like this in the marvell10 driver?

Also, do you know when there is an SFP cage? Do we need a standardised
DT property for this?

   Andrew

Re: [PATCH net-next 1/2] net: phy: marvell-88x2222: check that link is operational

[Russell King - ARM Linux admin]

Hi Andrew,

On Tue, Apr 13, 2021 at 03:12:05PM +0200, Andrew Lunn wrote:
> Is there something like this in the marvell10 driver?

No, it doesn't seem to be necessary there - I haven't seen spontaneous
link-ups with the 88x3310 there. Even if we did, that would cause other
issues beyond a nusience link-up event, as the PHY selects the first
media that has link between copper and fiber (and both are present on
Macchiatobin platforms.)

If the fiber indicates link up, it would prevent a copper connection
being established.

> Also, do you know when there is an SFP cage? Do we need a standardised
> DT property for this?

phydev->sfp_bus being non-NULL?

-- 
RMK's Patch system: https://www.armlinux.org.uk/developer/patches/
FTTP is here! 40Mbps down 10Mbps up. Decent connectivity at last!

[PATCH net-next 2/2] net: phy: marvell-88x2222: swap 1G/10G modes on autoneg

[Ivan Bornyakov]

Setting 10G without autonegotiation is invalid according to
phy_ethtool_ksettings_set(). Thus, if autonegotiation can't complete for
quite a time, but there is signal in line, switch line interface type to
10GBase-R, if supported, in hope for link to be established. And vice
versa.

Signed-off-by: Ivan Bornyakov <i.bornyakov@metrotek.ru>
---
 drivers/net/phy/marvell-88x2222.c | 276 ++++++++++++++++++------------
 1 file changed, 168 insertions(+), 108 deletions(-)

diff --git a/drivers/net/phy/marvell-88x2222.c b/drivers/net/phy/marvell-88x2222.c
index fb285ac741b2..d16c81f08334 100644
--- a/drivers/net/phy/marvell-88x2222.c
+++ b/drivers/net/phy/marvell-88x2222.c
@@ -48,6 +48,8 @@
 #define	MV_1GBX_PHY_STAT_SPEED100	BIT(14)
 #define	MV_1GBX_PHY_STAT_SPEED1000	BIT(15)
 
+#define	AUTONEG_TIMEOUT	3
+
 struct mv2222_data {
 	phy_interface_t line_interface;
 	__ETHTOOL_DECLARE_LINK_MODE_MASK(supported);
@@ -82,89 +84,6 @@ static int mv2222_soft_reset(struct phy_device *phydev)
 					 5000, 1000000, true);
 }
 
-/* Returns negative on error, 0 if link is down, 1 if link is up */
-static int mv2222_read_status_10g(struct phy_device *phydev)
-{
-	int val, link = 0;
-
-	val = phy_read_mmd(phydev, MDIO_MMD_PCS, MDIO_STAT1);
-	if (val < 0)
-		return val;
-
-	if (val & MDIO_STAT1_LSTATUS) {
-		link = 1;
-
-		/* 10GBASE-R do not support auto-negotiation */
-		phydev->autoneg = AUTONEG_DISABLE;
-		phydev->speed = SPEED_10000;
-		phydev->duplex = DUPLEX_FULL;
-	}
-
-	return link;
-}
-
-/* Returns negative on error, 0 if link is down, 1 if link is up */
-static int mv2222_read_status_1g(struct phy_device *phydev)
-{
-	int val, link = 0;
-
-	val = phy_read_mmd(phydev, MDIO_MMD_PCS, MV_1GBX_STAT);
-	if (val < 0)
-		return val;
-
-	if (!(val & BMSR_LSTATUS) ||
-	    (phydev->autoneg == AUTONEG_ENABLE &&
-	     !(val & BMSR_ANEGCOMPLETE)))
-		return 0;
-
-	link = 1;
-
-	val = phy_read_mmd(phydev, MDIO_MMD_PCS, MV_1GBX_PHY_STAT);
-	if (val < 0)
-		return val;
-
-	if (val & MV_1GBX_PHY_STAT_AN_RESOLVED) {
-		if (val & MV_1GBX_PHY_STAT_DUPLEX)
-			phydev->duplex = DUPLEX_FULL;
-		else
-			phydev->duplex = DUPLEX_HALF;
-
-		if (val & MV_1GBX_PHY_STAT_SPEED1000)
-			phydev->speed = SPEED_1000;
-		else if (val & MV_1GBX_PHY_STAT_SPEED100)
-			phydev->speed = SPEED_100;
-		else
-			phydev->speed = SPEED_10;
-	}
-
-	return link;
-}
-
-static int mv2222_read_status(struct phy_device *phydev)
-{
-	struct mv2222_data *priv = phydev->priv;
-	int link;
-
-	phydev->link = 0;
-	phydev->speed = SPEED_UNKNOWN;
-	phydev->duplex = DUPLEX_UNKNOWN;
-
-	if (!priv->sfp_link)
-		return 0;
-
-	if (priv->line_interface == PHY_INTERFACE_MODE_10GBASER)
-		link = mv2222_read_status_10g(phydev);
-	else
-		link = mv2222_read_status_1g(phydev);
-
-	if (link < 0)
-		return link;
-
-	phydev->link = link;
-
-	return 0;
-}
-
 static int mv2222_disable_aneg(struct phy_device *phydev)
 {
 	int ret = phy_clear_bits_mmd(phydev, MDIO_MMD_PCS, MV_1GBX_CTRL,
@@ -252,6 +171,24 @@ static bool mv2222_is_1gbx_capable(struct phy_device *phydev)
 				 priv->supported);
 }
 
+static bool mv2222_is_sgmii_capable(struct phy_device *phydev)
+{
+	struct mv2222_data *priv = phydev->priv;
+
+	return (linkmode_test_bit(ETHTOOL_LINK_MODE_1000baseT_Full_BIT,
+				  priv->supported) ||
+		linkmode_test_bit(ETHTOOL_LINK_MODE_1000baseT_Half_BIT,
+				  priv->supported) ||
+		linkmode_test_bit(ETHTOOL_LINK_MODE_100baseT_Full_BIT,
+				  priv->supported) ||
+		linkmode_test_bit(ETHTOOL_LINK_MODE_100baseT_Half_BIT,
+				  priv->supported) ||
+		linkmode_test_bit(ETHTOOL_LINK_MODE_10baseT_Full_BIT,
+				  priv->supported) ||
+		linkmode_test_bit(ETHTOOL_LINK_MODE_10baseT_Half_BIT,
+				  priv->supported));
+}
+
 static int mv2222_config_line(struct phy_device *phydev)
 {
 	struct mv2222_data *priv = phydev->priv;
@@ -271,7 +208,7 @@ static int mv2222_config_line(struct phy_device *phydev)
 	}
 }
 
-static int mv2222_setup_forced(struct phy_device *phydev)
+static int mv2222_swap_line_type(struct phy_device *phydev)
 {
 	struct mv2222_data *priv = phydev->priv;
 	bool changed = false;
@@ -279,25 +216,23 @@ static int mv2222_setup_forced(struct phy_device *phydev)
 
 	switch (priv->line_interface) {
 	case PHY_INTERFACE_MODE_10GBASER:
-		if (phydev->speed == SPEED_1000 &&
-		    mv2222_is_1gbx_capable(phydev)) {
+		if (mv2222_is_1gbx_capable(phydev)) {
 			priv->line_interface = PHY_INTERFACE_MODE_1000BASEX;
 			changed = true;
 		}
 
-		break;
-	case PHY_INTERFACE_MODE_1000BASEX:
-		if (phydev->speed == SPEED_10000 &&
-		    mv2222_is_10g_capable(phydev)) {
-			priv->line_interface = PHY_INTERFACE_MODE_10GBASER;
+		if (mv2222_is_sgmii_capable(phydev)) {
+			priv->line_interface = PHY_INTERFACE_MODE_SGMII;
 			changed = true;
 		}
 
 		break;
+	case PHY_INTERFACE_MODE_1000BASEX:
 	case PHY_INTERFACE_MODE_SGMII:
-		ret = mv2222_set_sgmii_speed(phydev);
-		if (ret < 0)
-			return ret;
+		if (mv2222_is_10g_capable(phydev)) {
+			priv->line_interface = PHY_INTERFACE_MODE_10GBASER;
+			changed = true;
+		}
 
 		break;
 	default:
@@ -310,6 +245,29 @@ static int mv2222_setup_forced(struct phy_device *phydev)
 			return ret;
 	}
 
+	return 0;
+}
+
+static int mv2222_setup_forced(struct phy_device *phydev)
+{
+	struct mv2222_data *priv = phydev->priv;
+	int ret;
+
+	if (priv->line_interface == PHY_INTERFACE_MODE_10GBASER) {
+		if (phydev->speed < SPEED_10000 &&
+		    phydev->speed != SPEED_UNKNOWN) {
+			ret = mv2222_swap_line_type(phydev);
+			if (ret < 0)
+				return ret;
+		}
+	}
+
+	if (priv->line_interface == PHY_INTERFACE_MODE_SGMII) {
+		ret = mv2222_set_sgmii_speed(phydev);
+		if (ret < 0)
+			return ret;
+	}
+
 	return mv2222_disable_aneg(phydev);
 }
 
@@ -323,17 +281,9 @@ static int mv2222_config_aneg(struct phy_device *phydev)
 		return 0;
 
 	if (phydev->autoneg == AUTONEG_DISABLE ||
-	    phydev->speed == SPEED_10000)
+	    priv->line_interface == PHY_INTERFACE_MODE_10GBASER)
 		return mv2222_setup_forced(phydev);
 
-	if (priv->line_interface == PHY_INTERFACE_MODE_10GBASER &&
-	    mv2222_is_1gbx_capable(phydev)) {
-		priv->line_interface = PHY_INTERFACE_MODE_1000BASEX;
-		ret = mv2222_config_line(phydev);
-		if (ret < 0)
-			return ret;
-	}
-
 	adv = linkmode_adv_to_mii_adv_x(priv->supported,
 					ETHTOOL_LINK_MODE_1000baseX_Full_BIT);
 
@@ -367,6 +317,120 @@ static int mv2222_aneg_done(struct phy_device *phydev)
 	return (ret & BMSR_ANEGCOMPLETE);
 }
 
+/* Returns negative on error, 0 if link is down, 1 if link is up */
+static int mv2222_read_status_10g(struct phy_device *phydev)
+{
+	static int timeout;
+	int val, link = 0;
+
+	val = phy_read_mmd(phydev, MDIO_MMD_PCS, MDIO_STAT1);
+	if (val < 0)
+		return val;
+
+	if (val & MDIO_STAT1_LSTATUS) {
+		link = 1;
+
+		/* 10GBASE-R do not support auto-negotiation */
+		phydev->autoneg = AUTONEG_DISABLE;
+		phydev->speed = SPEED_10000;
+		phydev->duplex = DUPLEX_FULL;
+	} else {
+		if (phydev->autoneg == AUTONEG_ENABLE) {
+			timeout++;
+
+			if (timeout > AUTONEG_TIMEOUT) {
+				timeout = 0;
+
+				val = mv2222_swap_line_type(phydev);
+				if (val < 0)
+					return val;
+
+				return mv2222_config_aneg(phydev);
+			}
+		}
+	}
+
+	return link;
+}
+
+/* Returns negative on error, 0 if link is down, 1 if link is up */
+static int mv2222_read_status_1g(struct phy_device *phydev)
+{
+	static int timeout;
+	int val, link = 0;
+
+	val = phy_read_mmd(phydev, MDIO_MMD_PCS, MV_1GBX_STAT);
+	if (val < 0)
+		return val;
+
+	if ((phydev->autoneg == AUTONEG_ENABLE &&
+	     !(val & BMSR_ANEGCOMPLETE))) {
+		timeout++;
+
+		if (timeout > AUTONEG_TIMEOUT) {
+			timeout = 0;
+
+			val = mv2222_swap_line_type(phydev);
+			if (val < 0)
+				return val;
+
+			return mv2222_config_aneg(phydev);
+		}
+
+		return 0;
+	}
+
+	if (!(val & BMSR_LSTATUS))
+		return 0;
+
+	link = 1;
+
+	val = phy_read_mmd(phydev, MDIO_MMD_PCS, MV_1GBX_PHY_STAT);
+	if (val < 0)
+		return val;
+
+	if (val & MV_1GBX_PHY_STAT_AN_RESOLVED) {
+		if (val & MV_1GBX_PHY_STAT_DUPLEX)
+			phydev->duplex = DUPLEX_FULL;
+		else
+			phydev->duplex = DUPLEX_HALF;
+
+		if (val & MV_1GBX_PHY_STAT_SPEED1000)
+			phydev->speed = SPEED_1000;
+		else if (val & MV_1GBX_PHY_STAT_SPEED100)
+			phydev->speed = SPEED_100;
+		else
+			phydev->speed = SPEED_10;
+	}
+
+	return link;
+}
+
+static int mv2222_read_status(struct phy_device *phydev)
+{
+	struct mv2222_data *priv = phydev->priv;
+	int link;
+
+	phydev->link = 0;
+	phydev->speed = SPEED_UNKNOWN;
+	phydev->duplex = DUPLEX_UNKNOWN;
+
+	if (!priv->sfp_link)
+		return 0;
+
+	if (priv->line_interface == PHY_INTERFACE_MODE_10GBASER)
+		link = mv2222_read_status_10g(phydev);
+	else
+		link = mv2222_read_status_1g(phydev);
+
+	if (link < 0)
+		return link;
+
+	phydev->link = link;
+
+	return 0;
+}
+
 static int mv2222_resume(struct phy_device *phydev)
 {
 	return mv2222_tx_enable(phydev);
@@ -428,11 +492,7 @@ static int mv2222_sfp_insert(void *upstream, const struct sfp_eeprom_id *id)
 		return ret;
 
 	if (mutex_trylock(&phydev->lock)) {
-		if (priv->line_interface == PHY_INTERFACE_MODE_10GBASER)
-			ret = mv2222_setup_forced(phydev);
-		else
-			ret = mv2222_config_aneg(phydev);
-
+		ret = mv2222_config_aneg(phydev);
 		mutex_unlock(&phydev->lock);
 	}
 
-- 
2.26.3