net: phy: marvell10g: select host interface configuration

Select the host interface configuration according to the capabilities
of the host; this allows the kernel to support SFP modules using the
88x3310.

Signed-off-by: Russell King <rmk+kernel@armlinux.org.uk>

1 files changed, 47 insertions, 5 deletions

diff --git a/drivers/net/phy/marvell10g.c b/drivers/net/phy/marvell10g.c
index f93781f08ecf..ae214c8b8277 100644
--- a/drivers/net/phy/marvell10g.c
+++ b/drivers/net/phy/marvell10g.c
@@ -709,15 +709,41 @@ static int mv3340_init_interface(struct phy_device *phydev, int mactype)
 	return err;
 }
 
+static int mv3310_select_mode(struct phy_device *phydev,
+			      unsigned long *host_interfaces)
+{
+	int mac_type = -1;
+
+	if (test_bit(PHY_INTERFACE_MODE_USXGMII, host_interfaces))
+		mac_type = 7;
+	else if (test_bit(PHY_INTERFACE_MODE_SGMII, host_interfaces) &&
+		 test_bit(PHY_INTERFACE_MODE_10GBASER, host_interfaces))
+		mac_type = 4;
+	else if (test_bit(PHY_INTERFACE_MODE_SGMII, host_interfaces) &&
+		 test_bit(PHY_INTERFACE_MODE_RXAUI, host_interfaces))
+		mac_type = 0;
+	else if (test_bit(PHY_INTERFACE_MODE_10GBASER, host_interfaces))
+		mac_type = 6;
+	else if (test_bit(PHY_INTERFACE_MODE_RXAUI, host_interfaces))
+		mac_type = 2;
+	else if (test_bit(PHY_INTERFACE_MODE_SGMII, host_interfaces))
+		mac_type = 4;
+
+	return mac_type;
+}
+
 static int mv3310_config_init(struct phy_device *phydev)
 {
 	struct mv3310_priv *priv = dev_get_drvdata(&phydev->mdio.dev);
 	const struct mv3310_chip *chip = to_mv3310_chip(phydev);
-	int err, mactype;
+	int val, ret, err, mactype = -1;
 
 	/* Check that the PHY interface type is compatible */
-	if (!test_bit(phydev->interface, priv->supported_interfaces))
+	if (!phy_interface_empty(phydev->host_interfaces)) {
+		mactype = mv3310_select_mode(phydev, phydev->host_interfaces);
+	} else if (!test_bit(phydev->interface, priv->supported_interfaces)) {
 		return -ENODEV;
+	}
 
 	phydev->mdix_ctrl = ETH_TP_MDI_AUTO;
 
@@ -726,9 +752,25 @@ static int mv3310_config_init(struct phy_device *phydev)
 	if (err)
 		return err;
 
-	mactype = chip->get_mactype(phydev);
-	if (mactype < 0)
-		return mactype;
+	if (mactype == -1) {
+		mactype = chip->get_mactype(phydev);
+		if (mactype < 0)
+			return mactype;
+	} else {
+		/* FIXME For 88x2210 */
+		ret = phy_modify_mmd_changed(phydev, MDIO_MMD_VEND2,
+					     MV_V2_PORT_CTRL,
+					     MV_V2_33X0_PORT_CTRL_MACTYPE_MASK,
+					     mactype);
+		if (ret > 0)
+			ret = phy_modify_mmd(phydev, MDIO_MMD_VEND2,
+					     MV_V2_PORT_CTRL,
+					     MV_V2_33X0_PORT_CTRL_SWRST,
+					     MV_V2_33X0_PORT_CTRL_SWRST);
+
+		if (ret < 0)
+			return ret;
+	}
 
 	err = chip->init_interface(phydev, mactype);
 	if (err) {