[net-next PATCH v3 13/13] drivers: net: dsa: qca8k: set internal delay also for sgmii

[Ansuel Smith]

QCA original code report port instability and sa that SGMII also require
to set internal delay. Generalize the rgmii delay function and apply the
advised value if they are not defined in DT.

Signed-off-by: Ansuel Smith <ansuelsmth@xxxxxxxxx>
---
 drivers/net/dsa/qca8k.c | 102 +++++++++++++++++++++++++---------------
 drivers/net/dsa/qca8k.h |   2 +
 2 files changed, 67 insertions(+), 37 deletions(-)

diff --git a/drivers/net/dsa/qca8k.c b/drivers/net/dsa/qca8k.c
index 7c68c272ce3a..21776826bf2e 100644
--- a/drivers/net/dsa/qca8k.c
+++ b/drivers/net/dsa/qca8k.c
@@ -1164,13 +1164,67 @@ qca8k_setup(struct dsa_switch *ds)
 	return 0;
 }
 
+static void
+qca8k_mac_config_setup_internal_delay(struct qca8k_priv *priv, struct dsa_port *dp,
+				      u32 reg, const struct phylink_link_state *state)
+{
+	u32 delay, val = 0;
+	int ret;
+
+	if (state->interface == PHY_INTERFACE_MODE_RGMII_ID ||
+	    state->interface == PHY_INTERFACE_MODE_RGMII_TXID ||
+	    state->interface == PHY_INTERFACE_MODE_SGMII) {
+		if (of_property_read_u32(dp->dn, "tx-internal-delay-ps", &delay))
+			delay = 1;
+		else
+			/* Switch regs accept value in ns, convert ps to ns */
+			delay = delay / 1000;
+
+		if (delay > QCA8K_MAX_DELAY) {
+			dev_err(priv->dev, "rgmii tx delay is limited to a max value of 3ns, setting to the max value");
+			delay = 3;
+		}
+
+		val |= QCA8K_PORT_PAD_RGMII_TX_DELAY(delay) |
+			QCA8K_PORT_PAD_RGMII_TX_DELAY_EN;
+	}
+
+	if (state->interface == PHY_INTERFACE_MODE_RGMII_ID ||
+	    state->interface == PHY_INTERFACE_MODE_RGMII_RXID ||
+	    state->interface == PHY_INTERFACE_MODE_SGMII) {
+		if (of_property_read_u32(dp->dn, "rx-internal-delay-ps", &delay))
+			delay = 2;
+		else
+			/* Switch regs accept value in ns, convert ps to ns */
+			delay = delay / 1000;
+
+		if (delay > QCA8K_MAX_DELAY) {
+			dev_err(priv->dev, "rgmii rx delay is limited to a max value of 3ns, setting to the max value");
+			delay = 3;
+		}
+
+		val |= QCA8K_PORT_PAD_RGMII_RX_DELAY(delay) |
+			QCA8K_PORT_PAD_RGMII_RX_DELAY_EN;
+	}
+
+	/* Set RGMII delay based on the selected values */
+	ret = qca8k_rmw(priv, reg,
+			QCA8K_PORT_PAD_RGMII_TX_DELAY_MASK |
+			QCA8K_PORT_PAD_RGMII_TX_DELAY_MASK |
+			QCA8K_PORT_PAD_RGMII_RX_DELAY_MASK |
+			QCA8K_PORT_PAD_RGMII_RX_DELAY_EN,
+			val);
+	if (ret)
+		dev_err(priv->dev, "Failed to set internal delay for CPU port %d", dp->index);
+}
+
 static void
 qca8k_phylink_mac_config(struct dsa_switch *ds, int port, unsigned int mode,
 			 const struct phylink_link_state *state)
 {
 	struct qca8k_priv *priv = ds->priv;
 	struct dsa_port *dp;
-	u32 reg, val, delay;
+	u32 reg, val;
 	int ret;
 
 	switch (port) {
@@ -1222,44 +1276,11 @@ qca8k_phylink_mac_config(struct dsa_switch *ds, int port, unsigned int mode,
 	case PHY_INTERFACE_MODE_RGMII_TXID:
 	case PHY_INTERFACE_MODE_RGMII_RXID:
 		dp = dsa_to_port(ds, port);
-		val = QCA8K_PORT_PAD_RGMII_EN;
-
-		if (state->interface == PHY_INTERFACE_MODE_RGMII_ID ||
-		    state->interface == PHY_INTERFACE_MODE_RGMII_TXID) {
-			if (of_property_read_u32(dp->dn, "tx-internal-delay-ps", &delay))
-				delay = 1;
-			else
-				/* Switch regs accept value in ns, convert ps to ns */
-				delay = delay / 1000;
-
-			if (delay > QCA8K_MAX_DELAY) {
-				dev_err(priv->dev, "rgmii tx delay is limited to a max value of 3ns, setting to the max value");
-				delay = 3;
-			}
-
-			val |= QCA8K_PORT_PAD_RGMII_TX_DELAY(delay) |
-			       QCA8K_PORT_PAD_RGMII_TX_DELAY_EN;
-		}
 
-		if (state->interface == PHY_INTERFACE_MODE_RGMII_ID ||
-		    state->interface == PHY_INTERFACE_MODE_RGMII_RXID) {
-			if (of_property_read_u32(dp->dn, "rx-internal-delay-ps", &delay))
-				delay = 2;
-			else
-				/* Switch regs accept value in ns, convert ps to ns */
-				delay = delay / 1000;
-
-			if (delay > QCA8K_MAX_DELAY) {
-				dev_err(priv->dev, "rgmii rx delay is limited to a max value of 3ns, setting to the max value");
-				delay = 3;
-			}
-
-			val |= QCA8K_PORT_PAD_RGMII_RX_DELAY(delay) |
-			       QCA8K_PORT_PAD_RGMII_RX_DELAY_EN;
-		}
+		qca8k_write(priv, reg, QCA8K_PORT_PAD_RGMII_EN);
 
-		/* Set RGMII delay based on the selected values */
-		qca8k_write(priv, reg, val);
+		/* Configure rgmii delay from dp or taking advised values */
+		qca8k_mac_config_setup_internal_delay(priv, dp, reg, state);
 
 		/* QCA8337 requires to set rgmii rx delay for all ports.
 		 * This is enabled through PORT5_PAD_CTRL for all ports,
@@ -1341,6 +1362,13 @@ qca8k_phylink_mac_config(struct dsa_switch *ds, int port, unsigned int mode,
 					QCA8K_PORT0_PAD_SGMII_RXCLK_FALLING_EDGE |
 					QCA8K_PORT0_PAD_SGMII_TXCLK_FALLING_EDGE,
 					val);
+
+		/* From original code is reported port instability as SGMII also
+		 * require delay set. Apply advised values here or take them from DT.
+		 */
+		if (state->interface == PHY_INTERFACE_MODE_SGMII)
+			qca8k_mac_config_setup_internal_delay(priv, dp, reg, state);
+
 		break;
 	default:
 		dev_err(ds->dev, "xMII mode %s not supported for port %d\n",
diff --git a/drivers/net/dsa/qca8k.h b/drivers/net/dsa/qca8k.h
index c032db5e0d41..92867001cc34 100644
--- a/drivers/net/dsa/qca8k.h
+++ b/drivers/net/dsa/qca8k.h
@@ -39,7 +39,9 @@
 #define QCA8K_REG_PORT5_PAD_CTRL			0x008
 #define QCA8K_REG_PORT6_PAD_CTRL			0x00c
 #define   QCA8K_PORT_PAD_RGMII_EN			BIT(26)
+#define   QCA8K_PORT_PAD_RGMII_TX_DELAY_MASK		GENMASK(23, 22)
 #define   QCA8K_PORT_PAD_RGMII_TX_DELAY(x)		((x) << 22)
+#define   QCA8K_PORT_PAD_RGMII_RX_DELAY_MASK		GENMASK(21, 20)
 #define   QCA8K_PORT_PAD_RGMII_RX_DELAY(x)		((x) << 20)
 #define	  QCA8K_PORT_PAD_RGMII_TX_DELAY_EN		BIT(25)
 #define   QCA8K_PORT_PAD_RGMII_RX_DELAY_EN		BIT(24)
-- 
2.32.0