[PATCH] net: dsa: mv88e6xxx: Skip cmode writable for mv88e6*41 if unchanged

[Nathan Rossi <nathan@nathanrossi.com>]

The mv88e6341_port_set_cmode function always calls the set writable
regardless of whether the current cmode is different from the desired
cmode. This is problematic for specific configurations of the mv88e6341
and mv88e6141 (in single chip adddressing mode?) where the hidden
registers are not accessible. This causes the set_cmode_writable to
fail, and causes teardown of the switch despite the cmode already being
configured in the correct mode (via external configuration).

This change adds checking of the current cmode compared to the desired
mode and returns if already in the desired mode. This skips the
set_cmode_writable setup if the port is already configured in the
desired mode, avoiding any issues with access of hidden registers.

Signed-off-by: Nathan Rossi <nathan@nathanrossi.com>
---
 drivers/net/dsa/mv88e6xxx/port.c | 18 ++++++++++++++++++
 1 file changed, 18 insertions(+)

diff --git a/drivers/net/dsa/mv88e6xxx/port.c b/drivers/net/dsa/mv88e6xxx/port.c
index 795b312876..f2e9c8cae3 100644
--- a/drivers/net/dsa/mv88e6xxx/port.c
+++ b/drivers/net/dsa/mv88e6xxx/port.c
@@ -713,6 +713,7 @@ int mv88e6341_port_set_cmode(struct mv88e6xxx_chip *chip, int port,
 			     phy_interface_t mode)
 {
 	int err;
+	u8 cmode = chip->ports[port].cmode;
 
 	if (port != 5)
 		return -EOPNOTSUPP;
@@ -724,6 +725,23 @@ int mv88e6341_port_set_cmode(struct mv88e6xxx_chip *chip, int port,
 	case PHY_INTERFACE_MODE_XAUI:
 	case PHY_INTERFACE_MODE_RXAUI:
 		return -EINVAL;
+
+	/* Check before setting writable. Such that on devices that are already
+	 * correctly configured, no attempt is made to make the cmode writable
+	 * as it may fail.
+	 */
+	case PHY_INTERFACE_MODE_1000BASEX:
+		if (cmode == MV88E6XXX_PORT_STS_CMODE_1000BASEX)
+			return 0;
+		break;
+	case PHY_INTERFACE_MODE_SGMII:
+		if (cmode == MV88E6XXX_PORT_STS_CMODE_SGMII)
+			return 0;
+		break;
+	case PHY_INTERFACE_MODE_2500BASEX:
+		if (cmode == MV88E6XXX_PORT_STS_CMODE_2500BASEX)
+			return 0;
+		break;
 	default:
 		break;
 	}
---
2.35.2