1 files changed, 217 insertions, 86 deletions

diff --git a/drivers/net/phy/marvell10g.c b/drivers/net/phy/marvell10g.c
index b6fea119fe13..07524046531e 100644
--- a/drivers/net/phy/marvell10g.c
+++ b/drivers/net/phy/marvell10g.c
@@ -27,6 +27,7 @@
 #include <linux/delay.h>
 #include <linux/hwmon.h>
 #include <linux/marvell_phy.h>
+#include <linux/of.h>
 #include <linux/phy.h>
 #include <linux/sfp.h>
 #include <linux/netdevice.h>
@@ -82,6 +83,8 @@ enum {
 	MV_PCS_CSSR1_SPD1_10	= 0x0000,
 	MV_PCS_CSSR1_DUPLEX_FULL= BIT(13),
 	MV_PCS_CSSR1_RESOLVED	= BIT(11),
+	MV_PCS_CSSR1_TX_PAUSE	= BIT(9),
+	MV_PCS_CSSR1_RX_PAUSE	= BIT(8),
 	MV_PCS_CSSR1_MDIX	= BIT(6),
 	MV_PCS_CSSR1_SPD2_MASK	= 0x000c,
 	MV_PCS_CSSR1_SPD2_5000	= 0x0008,
@@ -136,11 +139,19 @@ enum {
 	MV_V2_TEMP_UNKNOWN	= 0x9600, /* unknown function */
 };
 
+struct mv3310_mactype {
+	bool valid;
+	bool fixed_interface;
+	phy_interface_t interface_10g;
+};
+
 struct mv3310_chip {
 	bool (*has_downshift)(struct phy_device *phydev);
 	void (*init_supported_interfaces)(unsigned long *mask);
 	int (*get_mactype)(struct phy_device *phydev);
-	int (*init_interface)(struct phy_device *phydev, int mactype);
+
+	const struct mv3310_mactype *mactypes;
+	size_t n_mactypes;
 
 #ifdef CONFIG_HWMON
 	int (*hwmon_read_temp_reg)(struct phy_device *phydev);
@@ -149,14 +160,16 @@ struct mv3310_chip {
 
 struct mv3310_priv {
 	DECLARE_BITMAP(supported_interfaces, PHY_INTERFACE_MODE_MAX);
+	const struct mv3310_mactype *mactype;
 
 	u32 firmware_ver;
 	bool has_downshift;
-	bool rate_match;
-	phy_interface_t const_interface;
+	bool firmware_failed;
 
 	struct device *hwmon_dev;
 	char *hwmon_name;
+	u8 num_leds;
+	u16 led_mode[4];
 };
 
 static const struct mv3310_chip *to_mv3310_chip(struct phy_device *phydev)
@@ -466,9 +479,10 @@ static int mv3310_sfp_insert(void *upstream, const struct sfp_eeprom_id *id)
 {
 	struct phy_device *phydev = upstream;
 	__ETHTOOL_DECLARE_LINK_MODE_MASK(support) = { 0, };
+	DECLARE_PHY_INTERFACE_MASK(interfaces);
 	phy_interface_t iface;
 
-	sfp_parse_support(phydev->sfp_bus, id, support);
+	sfp_parse_support(phydev->sfp_bus, id, support, interfaces);
 	iface = sfp_select_interface(phydev->sfp_bus, support);
 
 	if (iface != PHY_INTERFACE_MODE_10GBASER) {
@@ -484,6 +498,43 @@ static const struct sfp_upstream_ops mv3310_sfp_ops = {
 	.module_insert = mv3310_sfp_insert,
 };
 
+static int mv3310_leds_write(struct phy_device *phydev)
+{
+	struct mv3310_priv *priv = dev_get_drvdata(&phydev->mdio.dev);
+	int i, ret;
+
+	for (i = 0; i < priv->num_leds; i++) {
+		ret = phy_write_mmd(phydev, MDIO_MMD_VEND2, 0xf020 + i,
+				    priv->led_mode[i]);
+		if (ret < 0)
+			return ret;
+	}
+
+	return 0;
+}
+
+static int mv3310_fw_config(struct phy_device *phydev)
+{
+	struct mv3310_priv *priv = dev_get_drvdata(&phydev->mdio.dev);
+	struct device_node *node;
+	int ret;
+
+	node = phydev->mdio.dev.of_node;
+	if (!node)
+		return 0;
+
+	ret = of_property_read_variable_u16_array(node, "marvell,led-mode",
+				priv->led_mode, 1, ARRAY_SIZE(priv->led_mode));
+	if (ret == -EINVAL)
+		ret = 0;
+	if (ret < 0)
+		return ret;
+
+	priv->num_leds = ret;
+
+	return 0;
+}
+
 static int mv3310_probe(struct phy_device *phydev)
 {
 	const struct mv3310_chip *chip = to_mv3310_chip(phydev);
@@ -495,6 +546,24 @@ static int mv3310_probe(struct phy_device *phydev)
 	    (phydev->c45_ids.devices_in_package & mmd_mask) != mmd_mask)
 		return -ENODEV;
 
+	__set_bit(PHY_INTERFACE_MODE_SGMII, phydev->supported_interfaces);
+	__set_bit(PHY_INTERFACE_MODE_2500BASEX, phydev->supported_interfaces);
+	__set_bit(PHY_INTERFACE_MODE_10GBASER, phydev->supported_interfaces);
+
+	priv = devm_kzalloc(&phydev->mdio.dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	dev_set_drvdata(&phydev->mdio.dev, priv);
+
+	ret = mv3310_fw_config(phydev);
+	if (ret < 0)
+		return ret;
+
+	ret = mv3310_leds_write(phydev);
+	if (ret < 0)
+		return ret;
+
 	ret = phy_read_mmd(phydev, MDIO_MMD_PMAPMD, MV_PMA_BOOT);
 	if (ret < 0)
 		return ret;
@@ -502,15 +571,9 @@ static int mv3310_probe(struct phy_device *phydev)
 	if (ret & MV_PMA_BOOT_FATAL) {
 		dev_warn(&phydev->mdio.dev,
 			 "PHY failed to boot firmware, status=%04x\n", ret);
-		return -ENODEV;
+		priv->firmware_failed = true;
 	}
 
-	priv = devm_kzalloc(&phydev->mdio.dev, sizeof(*priv), GFP_KERNEL);
-	if (!priv)
-		return -ENOMEM;
-
-	dev_set_drvdata(&phydev->mdio.dev, priv);
-
 	ret = phy_read_mmd(phydev, MDIO_MMD_PMAPMD, MV_PMA_FW_VER0);
 	if (ret < 0)
 		return ret;
@@ -565,6 +628,19 @@ static int mv3310_resume(struct phy_device *phydev)
 	return mv3310_hwmon_config(phydev, true);
 }
 
+static int mv3310_start(struct phy_device *phydev)
+{
+	struct mv3310_priv *priv = dev_get_drvdata(&phydev->mdio.dev);
+
+	if (priv->firmware_failed) {
+		dev_warn(&phydev->mdio.dev,
+			 "PHY firmware failure: PHY not starting");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
 /* Some PHYs in the Alaska family such as the 88X3310 and the 88E2010
  * don't set bit 14 in PMA Extended Abilities (1.11), although they do
  * support 2.5GBASET and 5GBASET. For these models, we can still read their
@@ -604,76 +680,105 @@ static int mv3310_get_mactype(struct phy_device *phydev)
 	return mactype & MV_V2_33X0_PORT_CTRL_MACTYPE_MASK;
 }
 
-static int mv2110_init_interface(struct phy_device *phydev, int mactype)
-{
-	struct mv3310_priv *priv = dev_get_drvdata(&phydev->mdio.dev);
-
-	priv->rate_match = false;
-
-	if (mactype == MV_PMA_21X0_PORT_CTRL_MACTYPE_10GBASER_RATE_MATCH)
-		priv->rate_match = true;
-
-	if (mactype == MV_PMA_21X0_PORT_CTRL_MACTYPE_USXGMII)
-		priv->const_interface = PHY_INTERFACE_MODE_USXGMII;
-	else if (mactype == MV_PMA_21X0_PORT_CTRL_MACTYPE_10GBASER_RATE_MATCH)
-		priv->const_interface = PHY_INTERFACE_MODE_10GBASER;
-	else if (mactype == MV_PMA_21X0_PORT_CTRL_MACTYPE_5GBASER ||
-		 mactype == MV_PMA_21X0_PORT_CTRL_MACTYPE_5GBASER_NO_SGMII_AN)
-		priv->const_interface = PHY_INTERFACE_MODE_NA;
-	else
-		return -EINVAL;
-
-	return 0;
-}
-
-static int mv3310_init_interface(struct phy_device *phydev, int mactype)
-{
-	struct mv3310_priv *priv = dev_get_drvdata(&phydev->mdio.dev);
-
-	priv->rate_match = false;
-
-	if (mactype == MV_V2_33X0_PORT_CTRL_MACTYPE_10GBASER_RATE_MATCH ||
-	    mactype == MV_V2_33X0_PORT_CTRL_MACTYPE_RXAUI_RATE_MATCH ||
-	    mactype == MV_V2_3310_PORT_CTRL_MACTYPE_XAUI_RATE_MATCH)
-		priv->rate_match = true;
-
-	if (mactype == MV_V2_33X0_PORT_CTRL_MACTYPE_USXGMII)
-		priv->const_interface = PHY_INTERFACE_MODE_USXGMII;
-	else if (mactype == MV_V2_33X0_PORT_CTRL_MACTYPE_10GBASER_RATE_MATCH ||
-		 mactype == MV_V2_33X0_PORT_CTRL_MACTYPE_10GBASER_NO_SGMII_AN ||
-		 mactype == MV_V2_33X0_PORT_CTRL_MACTYPE_10GBASER)
-		priv->const_interface = PHY_INTERFACE_MODE_10GBASER;
-	else if (mactype == MV_V2_33X0_PORT_CTRL_MACTYPE_RXAUI_RATE_MATCH ||
-		 mactype == MV_V2_33X0_PORT_CTRL_MACTYPE_RXAUI)
-		priv->const_interface = PHY_INTERFACE_MODE_RXAUI;
-	else if (mactype == MV_V2_3310_PORT_CTRL_MACTYPE_XAUI_RATE_MATCH ||
-		 mactype == MV_V2_3310_PORT_CTRL_MACTYPE_XAUI)
-		priv->const_interface = PHY_INTERFACE_MODE_XAUI;
-	else
-		return -EINVAL;
-
-	return 0;
-}
-
-static int mv3340_init_interface(struct phy_device *phydev, int mactype)
-{
-	struct mv3310_priv *priv = dev_get_drvdata(&phydev->mdio.dev);
-	int err = 0;
-
-	priv->rate_match = false;
+static const struct mv3310_mactype mv2110_mactypes[] = {
+	[MV_PMA_21X0_PORT_CTRL_MACTYPE_USXGMII] = {
+		.valid = true,
+		.fixed_interface = true,
+		.interface_10g = PHY_INTERFACE_MODE_USXGMII,
+	},
+	[MV_PMA_21X0_PORT_CTRL_MACTYPE_5GBASER] = {
+		.valid = true,
+		.interface_10g = PHY_INTERFACE_MODE_NA,
+	},
+	[MV_PMA_21X0_PORT_CTRL_MACTYPE_5GBASER_NO_SGMII_AN] = {
+		.valid = true,
+		.interface_10g = PHY_INTERFACE_MODE_NA,
+	},
+	[MV_PMA_21X0_PORT_CTRL_MACTYPE_10GBASER_RATE_MATCH] = {
+		.valid = true,
+		.fixed_interface = true,
+		.interface_10g = PHY_INTERFACE_MODE_10GBASER,
+	},
+};
 
-	if (mactype == MV_V2_3340_PORT_CTRL_MACTYPE_RXAUI_NO_SGMII_AN)
-		priv->const_interface = PHY_INTERFACE_MODE_RXAUI;
-	else
-		err = mv3310_init_interface(phydev, mactype);
+static const struct mv3310_mactype mv3310_mactypes[] = {
+	[MV_V2_33X0_PORT_CTRL_MACTYPE_RXAUI] = {
+		.valid = true,
+		.interface_10g = PHY_INTERFACE_MODE_RXAUI,
+	},
+	[MV_V2_3310_PORT_CTRL_MACTYPE_XAUI_RATE_MATCH] = {
+		.valid = true,
+		.fixed_interface = true,
+		.interface_10g = PHY_INTERFACE_MODE_XAUI,
+	},
+	[MV_V2_33X0_PORT_CTRL_MACTYPE_RXAUI_RATE_MATCH] = {
+		.valid = true,
+		.fixed_interface = true,
+		.interface_10g = PHY_INTERFACE_MODE_RXAUI,
+	},
+	[MV_V2_3310_PORT_CTRL_MACTYPE_XAUI] = {
+		.valid = true,
+		.interface_10g = PHY_INTERFACE_MODE_XAUI,
+	},
+	[MV_V2_33X0_PORT_CTRL_MACTYPE_10GBASER] = {
+		.valid = true,
+		.interface_10g = PHY_INTERFACE_MODE_10GBASER,
+	},
+	[MV_V2_33X0_PORT_CTRL_MACTYPE_10GBASER_NO_SGMII_AN] = {
+		.valid = true,
+		.interface_10g = PHY_INTERFACE_MODE_10GBASER,
+	},
+	[MV_V2_33X0_PORT_CTRL_MACTYPE_10GBASER_RATE_MATCH] = {
+		.valid = true,
+		.fixed_interface = true,
+		.interface_10g = PHY_INTERFACE_MODE_10GBASER,
+	},
+	[MV_V2_33X0_PORT_CTRL_MACTYPE_USXGMII] = {
+		.valid = true,
+		.fixed_interface = true,
+		.interface_10g = PHY_INTERFACE_MODE_USXGMII,
+	},
+};
 
-	return err;
-}
+static const struct mv3310_mactype mv3340_mactypes[] = {
+	[MV_V2_33X0_PORT_CTRL_MACTYPE_RXAUI] = {
+		.valid = true,
+		.interface_10g = PHY_INTERFACE_MODE_RXAUI,
+	},
+	[MV_V2_3340_PORT_CTRL_MACTYPE_RXAUI_NO_SGMII_AN] = {
+		.valid = true,
+		.interface_10g = PHY_INTERFACE_MODE_RXAUI,
+	},
+	[MV_V2_33X0_PORT_CTRL_MACTYPE_RXAUI_RATE_MATCH] = {
+		.valid = true,
+		.fixed_interface = true,
+		.interface_10g = PHY_INTERFACE_MODE_RXAUI,
+	},
+	[MV_V2_33X0_PORT_CTRL_MACTYPE_10GBASER] = {
+		.valid = true,
+		.interface_10g = PHY_INTERFACE_MODE_10GBASER,
+	},
+	[MV_V2_33X0_PORT_CTRL_MACTYPE_10GBASER_NO_SGMII_AN] = {
+		.valid = true,
+		.interface_10g = PHY_INTERFACE_MODE_10GBASER,
+	},
+	[MV_V2_33X0_PORT_CTRL_MACTYPE_10GBASER_RATE_MATCH] = {
+		.valid = true,
+		.fixed_interface = true,
+		.interface_10g = PHY_INTERFACE_MODE_10GBASER,
+	},
+	[MV_V2_33X0_PORT_CTRL_MACTYPE_USXGMII] = {
+		.valid = true,
+		.fixed_interface = true,
+		.interface_10g = PHY_INTERFACE_MODE_USXGMII,
+	},
+};
 
 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);
+	unsigned long *possible;
 	int err, mactype;
 
 	/* Check that the PHY interface type is compatible */
@@ -691,10 +796,23 @@ static int mv3310_config_init(struct phy_device *phydev)
 	if (mactype < 0)
 		return mactype;
 
-	err = chip->init_interface(phydev, mactype);
-	if (err) {
+	if (mactype >= chip->n_mactypes || !chip->mactypes[mactype].valid) {
 		phydev_err(phydev, "MACTYPE configuration invalid\n");
-		return err;
+		return -EINVAL;
+	}
+
+	priv->mactype = &chip->mactypes[mactype];
+
+	possible = phydev->possible_interfaces;
+	phy_interface_zero(possible);
+
+	if (priv->mactype->interface_10g != PHY_INTERFACE_MODE_NA)
+		__set_bit(priv->mactype->interface_10g, possible);
+
+	if (!priv->mactype->fixed_interface) {
+		__set_bit(PHY_INTERFACE_MODE_5GBASER, possible);
+		__set_bit(PHY_INTERFACE_MODE_2500BASEX, possible);
+		__set_bit(PHY_INTERFACE_MODE_SGMII, possible);
 	}
 
 	/* Enable EDPD mode - saving 600mW */
@@ -707,7 +825,7 @@ static int mv3310_config_init(struct phy_device *phydev)
 	if (err && err != -EOPNOTSUPP)
 		return err;
 
-	return 0;
+	return mv3310_leds_write(phydev);
 }
 
 static int mv3310_get_features(struct phy_device *phydev)
@@ -823,9 +941,8 @@ static void mv3310_update_interface(struct phy_device *phydev)
 	 *
 	 * In USXGMII mode the PHY interface mode is also fixed.
 	 */
-	if (priv->rate_match ||
-	    priv->const_interface == PHY_INTERFACE_MODE_USXGMII) {
-		phydev->interface = priv->const_interface;
+	if (priv->mactype->fixed_interface) {
+		phydev->interface = priv->mactype->interface_10g;
 		return;
 	}
 
@@ -837,7 +954,7 @@ static void mv3310_update_interface(struct phy_device *phydev)
 	 */
 	switch (phydev->speed) {
 	case SPEED_10000:
-		phydev->interface = priv->const_interface;
+		phydev->interface = priv->mactype->interface_10g;
 		break;
 	case SPEED_5000:
 		phydev->interface = PHY_INTERFACE_MODE_5GBASER;
@@ -925,6 +1042,10 @@ static int mv3310_read_status_copper(struct phy_device *phydev)
 	phydev->mdix = cssr1 & MV_PCS_CSSR1_MDIX ?
 		       ETH_TP_MDI_X : ETH_TP_MDI;
 
+	phydev->resolved_tx_pause = !!(cssr1 & MV_PCS_CSSR1_TX_PAUSE);
+	phydev->resolved_rx_pause = !!(cssr1 & MV_PCS_CSSR1_RX_PAUSE);
+	phydev->resolved_pause_valid = true;
+
 	if (val & MDIO_AN_STAT1_COMPLETE) {
 		val = genphy_c45_read_lpa(phydev);
 		if (val < 0)
@@ -1049,7 +1170,9 @@ static const struct mv3310_chip mv3310_type = {
 	.has_downshift = mv3310_has_downshift,
 	.init_supported_interfaces = mv3310_init_supported_interfaces,
 	.get_mactype = mv3310_get_mactype,
-	.init_interface = mv3310_init_interface,
+
+	.mactypes = mv3310_mactypes,
+	.n_mactypes = ARRAY_SIZE(mv3310_mactypes),
 
 #ifdef CONFIG_HWMON
 	.hwmon_read_temp_reg = mv3310_hwmon_read_temp_reg,
@@ -1060,7 +1183,9 @@ static const struct mv3310_chip mv3340_type = {
 	.has_downshift = mv3310_has_downshift,
 	.init_supported_interfaces = mv3340_init_supported_interfaces,
 	.get_mactype = mv3310_get_mactype,
-	.init_interface = mv3340_init_interface,
+
+	.mactypes = mv3340_mactypes,
+	.n_mactypes = ARRAY_SIZE(mv3340_mactypes),
 
 #ifdef CONFIG_HWMON
 	.hwmon_read_temp_reg = mv3310_hwmon_read_temp_reg,
@@ -1070,7 +1195,9 @@ static const struct mv3310_chip mv3340_type = {
 static const struct mv3310_chip mv2110_type = {
 	.init_supported_interfaces = mv2110_init_supported_interfaces,
 	.get_mactype = mv2110_get_mactype,
-	.init_interface = mv2110_init_interface,
+
+	.mactypes = mv2110_mactypes,
+	.n_mactypes = ARRAY_SIZE(mv2110_mactypes),
 
 #ifdef CONFIG_HWMON
 	.hwmon_read_temp_reg = mv2110_hwmon_read_temp_reg,
@@ -1080,7 +1207,9 @@ static const struct mv3310_chip mv2110_type = {
 static const struct mv3310_chip mv2111_type = {
 	.init_supported_interfaces = mv2111_init_supported_interfaces,
 	.get_mactype = mv2110_get_mactype,
-	.init_interface = mv2110_init_interface,
+
+	.mactypes = mv2110_mactypes,
+	.n_mactypes = ARRAY_SIZE(mv2110_mactypes),
 
 #ifdef CONFIG_HWMON
 	.hwmon_read_temp_reg = mv2110_hwmon_read_temp_reg,
@@ -1251,6 +1380,7 @@ static struct phy_driver mv3310_drivers[] = {
 		.probe		= mv3310_probe,
 		.suspend	= mv3310_suspend,
 		.resume		= mv3310_resume,
+		.start		= mv3310_start,
 		.config_aneg	= mv3310_config_aneg,
 		.aneg_done	= mv3310_aneg_done,
 		.read_status	= mv3310_read_status,
@@ -1288,6 +1418,7 @@ static struct phy_driver mv3310_drivers[] = {
 		.probe		= mv3310_probe,
 		.suspend	= mv3310_suspend,
 		.resume		= mv3310_resume,
+		.start		= mv3310_start,
 		.config_init	= mv3310_config_init,
 		.config_aneg	= mv3310_config_aneg,
 		.aneg_done	= mv3310_aneg_done,