[PATCH net-next v5 4/4] phy: aquantia: Determine rate adaptation support from registers

[Sean Anderson]

When autonegotiation completes, the phy interface will be set based on
the global config register for that speed. If the SERDES mode is set to
something which the MAC does not support, then the link will not come
up. To avoid this, validate each combination of interface speed and link
speed which might be configured. This way, we ensure that we only
consider rate adaptation in our advertisement when we can actually use
it.

For some firmwares, not all speeds are supported. In this case, the
global config register for that speed will be initialized to zero
(indicating that rate adaptation is not supported). We can detect this
by reading the PMA/PMD speed register to determine which speeds are
supported. This register is read once in probe and cached for later.

Signed-off-by: Sean Anderson <sean.anderson@xxxxxxxx>
---
This commit fixes 3c42563b3041 ("net: phy: aquantia: Add support for
rate matching"). In an effort to avoid backporting of this commit until
it has soaked in master for a while, the fixes tag has been left off.

Changes in v5:
- Don't handle PHY_INTERFACE_MODE_NA, and simplify logic

Changes in v4:
- Fix kerneldoc using - instead of : for parameters

Changes in v3:
- Fix incorrect bits for PMA/PMD speed

Changes in v2:
- Rework to just validate things instead of modifying registers

 drivers/net/phy/aquantia_main.c | 136 ++++++++++++++++++++++++++++++--
 1 file changed, 128 insertions(+), 8 deletions(-)

diff --git a/drivers/net/phy/aquantia_main.c b/drivers/net/phy/aquantia_main.c
index 334a6904ca5a..06078cd2d5b3 100644
--- a/drivers/net/phy/aquantia_main.c
+++ b/drivers/net/phy/aquantia_main.c
@@ -111,6 +111,12 @@
 #define VEND1_GLOBAL_CFG_RATE_ADAPT_NONE	0
 #define VEND1_GLOBAL_CFG_RATE_ADAPT_USX		1
 #define VEND1_GLOBAL_CFG_RATE_ADAPT_PAUSE	2
+#define VEND1_GLOBAL_CFG_SERDES_MODE		GENMASK(2, 0)
+#define VEND1_GLOBAL_CFG_SERDES_MODE_XFI	0
+#define VEND1_GLOBAL_CFG_SERDES_MODE_SGMII	3
+#define VEND1_GLOBAL_CFG_SERDES_MODE_OCSGMII	4
+#define VEND1_GLOBAL_CFG_SERDES_MODE_XFI5G	6
+#define VEND1_GLOBAL_CFG_SERDES_MODE_XFI20G	7
 
 #define VEND1_GLOBAL_RSVD_STAT1			0xc885
 #define VEND1_GLOBAL_RSVD_STAT1_FW_BUILD_ID	GENMASK(7, 4)
@@ -175,6 +181,7 @@ static const struct aqr107_hw_stat aqr107_hw_stats[] = {
 
 struct aqr107_priv {
 	u64 sgmii_stats[AQR107_SGMII_STAT_SZ];
+	int pmapmd_speeds;
 };
 
 static int aqr107_get_sset_count(struct phy_device *phydev)
@@ -677,14 +684,119 @@ static int aqr107_wait_processor_intensive_op(struct phy_device *phydev)
 	return 0;
 }
 
+/**
+ * struct aqr107_link_speed_cfg - Common configuration for link speeds
+ * @speed: The speed of this config
+ * @reg: The global system configuration register for this speed
+ * @speed_bit: The bit in the PMA/PMD speed ability register which determines
+ *             whether this link speed is supported
+ */
+struct aqr107_link_speed_cfg {
+	int speed;
+	u16 reg, speed_bit;
+};
+
+/**
+ * aqr107_rate_adapt_ok() - Validate rate adaptation for a configuration
+ * @phydev: The phy to act on
+ * @serdes_speed: The speed of the serdes (aka the phy interface)
+ * @link_cfg: The config for the link speed
+ *
+ * This function validates whether rate adaptation will work for a particular
+ * combination of @serdes_speed and @link_cfg.
+ *
+ * Return: %true if the @link_cfg.reg is configured for rate adaptation or if
+ *         @link_cfg.speed will not be advertised, %false otherwise.
+ */
+static bool aqr107_rate_adapt_ok(struct phy_device *phydev, int serdes_speed,
+				 const struct aqr107_link_speed_cfg *link_cfg)
+{
+	struct aqr107_priv *priv = phydev->priv;
+	int val;
+
+	phydev_dbg(phydev, "validating link_speed=%d serdes_speed=%d\n",
+		   link_cfg->speed, serdes_speed);
+
+	/* Vacuously OK, since we won't advertise it anyway */
+	if (!(priv->pmapmd_speeds & link_cfg->speed_bit))
+		return true;
+
+	val = phy_read_mmd(phydev, MDIO_MMD_VEND1, link_cfg->reg);
+	if (val < 0) {
+		phydev_warn(phydev, "could not read register %x:%.04x (err = %d)\n",
+			    MDIO_MMD_VEND1, link_cfg->reg, val);
+		return false;
+	}
+
+	phydev_dbg(phydev, "%x:%.04x = %.04x\n", MDIO_MMD_VEND1, link_cfg->reg, val);
+	if (FIELD_GET(VEND1_GLOBAL_CFG_RATE_ADAPT, val) !=
+		VEND1_GLOBAL_CFG_RATE_ADAPT_PAUSE)
+		return false;
+
+	switch (FIELD_GET(VEND1_GLOBAL_CFG_SERDES_MODE, val)) {
+	case VEND1_GLOBAL_CFG_SERDES_MODE_XFI20G:
+		return serdes_speed == SPEED_20000;
+	case VEND1_GLOBAL_CFG_SERDES_MODE_XFI:
+		return serdes_speed == SPEED_10000;
+	case VEND1_GLOBAL_CFG_SERDES_MODE_XFI5G:
+		return serdes_speed == SPEED_5000;
+	case VEND1_GLOBAL_CFG_SERDES_MODE_OCSGMII:
+		return serdes_speed == SPEED_2500;
+	case VEND1_GLOBAL_CFG_SERDES_MODE_SGMII:
+		return serdes_speed == SPEED_1000;
+	default:
+		return false;
+	}
+}
+
 static int aqr107_get_rate_matching(struct phy_device *phydev,
 				    phy_interface_t iface)
 {
-	if (iface == PHY_INTERFACE_MODE_10GBASER ||
-	    iface == PHY_INTERFACE_MODE_2500BASEX ||
-	    iface == PHY_INTERFACE_MODE_NA)
-		return RATE_MATCH_PAUSE;
-	return RATE_MATCH_NONE;
+	static const struct aqr107_link_speed_cfg speed_table[] = {
+		{
+			.speed = SPEED_10,
+			.reg = VEND1_GLOBAL_CFG_10M,
+			.speed_bit = MDIO_PMA_SPEED_10,
+		},
+		{
+			.speed = SPEED_100,
+			.reg = VEND1_GLOBAL_CFG_100M,
+			.speed_bit = MDIO_PMA_SPEED_100,
+		},
+		{
+			.speed = SPEED_1000,
+			.reg = VEND1_GLOBAL_CFG_1G,
+			.speed_bit = MDIO_PMA_SPEED_1000,
+		},
+		{
+			.speed = SPEED_2500,
+			.reg = VEND1_GLOBAL_CFG_2_5G,
+			.speed_bit = MDIO_PMA_SPEED_2_5G,
+		},
+		{
+			.speed = SPEED_5000,
+			.reg = VEND1_GLOBAL_CFG_5G,
+			.speed_bit = MDIO_PMA_SPEED_5G,
+		},
+		{
+			.speed = SPEED_10000,
+			.reg = VEND1_GLOBAL_CFG_10G,
+			.speed_bit = MDIO_PMA_SPEED_10G,
+		},
+	};
+	int speed = phy_interface_max_speed(iface);
+	bool got_one = false;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(speed_table) &&
+		    speed_table[i].speed <= speed; i++) {
+		if (!aqr107_rate_adapt_ok(phydev, speed, &speed_table[i]))
+			return RATE_MATCH_NONE;
+		got_one = true;
+	}
+
+	/* Must match at least one speed */
+	return got_one ? RATE_MATCH_PAUSE : RATE_MATCH_NONE;
 }
 
 static int aqr107_suspend(struct phy_device *phydev)
@@ -713,10 +825,18 @@ static int aqr107_resume(struct phy_device *phydev)
 
 static int aqr107_probe(struct phy_device *phydev)
 {
-	phydev->priv = devm_kzalloc(&phydev->mdio.dev,
-				    sizeof(struct aqr107_priv), GFP_KERNEL);
-	if (!phydev->priv)
+	struct aqr107_priv *priv;
+
+	priv = devm_kzalloc(&phydev->mdio.dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
 		return -ENOMEM;
+	phydev->priv = priv;
+
+	priv->pmapmd_speeds = phy_read_mmd(phydev, MDIO_MMD_PMAPMD, MDIO_SPEED);
+	if (priv->pmapmd_speeds < 0) {
+		phydev_err(phydev, "could not read PMA/PMD speeds\n");
+		return priv->pmapmd_speeds;
+	};
 
 	return aqr_hwmon_probe(phydev);
 }
-- 
2.35.1.1320.gc452695387.dirty