net: phy: marvell-88x2222: check that link is operational

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.

If there is no SFP cage connected to the tranciever, check only PMD
Recieve Signal Detect register.

Signed-off-by: Ivan Bornyakov <i.bornyakov@metrotek.ru>
Signed-off-by: David S. Miller <davem@davemloft.net>

drivers/net/phy/marvell-88x2222.c

@@ -32,6 +32,10 @@
 #define	MV_HOST_RST_SW	BIT(7)
 #define	MV_PORT_RST_SW	(MV_LINE_RST_SW | MV_HOST_RST_SW)
 
+/* PMD Receive Signal Detect */
+#define	MV_RX_SIGNAL_DETECT		0x000A
+#define	MV_RX_SIGNAL_DETECT_GLOBAL	BIT(0)
+
 /* 1000Base-X/SGMII Control Register */
 #define	MV_1GBX_CTRL		(0x2000 + MII_BMCR)
 
@@ -51,6 +55,7 @@
 struct mv2222_data {
 	phy_interface_t line_interface;
 	__ETHTOOL_DECLARE_LINK_MODE_MASK(supported);
+	bool sfp_link;
 };
 
 /* SFI PMA transmit enable */
@@ -139,6 +144,21 @@ static int mv2222_read_status_1g(struct phy_device *phydev)
 	return link;
 }
 
+static bool mv2222_link_is_operational(struct phy_device *phydev)
+{
+	struct mv2222_data *priv = phydev->priv;
+	int val;
+
+	val = phy_read_mmd(phydev, MDIO_MMD_PMAPMD, MV_RX_SIGNAL_DETECT);
+	if (val < 0 || !(val & MV_RX_SIGNAL_DETECT_GLOBAL))
+		return false;
+
+	if (phydev->sfp_bus && !priv->sfp_link)
+		return false;
+
+	return true;
+}
+
 static int mv2222_read_status(struct phy_device *phydev)
 {
 	struct mv2222_data *priv = phydev->priv;
@@ -148,6 +168,9 @@ static int mv2222_read_status(struct phy_device *phydev)
 	phydev->speed = SPEED_UNKNOWN;
 	phydev->duplex = DUPLEX_UNKNOWN;
 
+	if (!mv2222_link_is_operational(phydev))
+		return 0;
+
 	if (priv->line_interface == PHY_INTERFACE_MODE_10GBASER)
 		link = mv2222_read_status_10g(phydev);
 	else
@@ -446,9 +469,29 @@ 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 = phydev->priv;
+	priv->sfp_link = true;
+}
+
+static void mv2222_sfp_link_down(void *upstream)
+{
+	struct phy_device *phydev = upstream;
+	struct mv2222_data *priv;
+
+	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,
 };