net: dsa: Consolidate phy read and write functions

Move the common code for reading and writing phy registers into the
shared mv88e6xxx.

Signed-off-by: Andrew Lunn <andrew@lunn.ch>
Reviewed-by: Guenter Roeck <linux@roeck-us.net>
Tested-by: Guenter Roeck <linux@roeck-us.net>
Signed-off-by: David S. Miller <davem@davemloft.net>

1 files changed, 99 insertions, 19 deletions

diff --git a/drivers/net/dsa/mv88e6xxx.c b/drivers/net/dsa/mv88e6xxx.c
index df727e1db18d..7c4c04bb0cb0 100644
--- a/drivers/net/dsa/mv88e6xxx.c
+++ b/drivers/net/dsa/mv88e6xxx.c
@@ -214,14 +214,17 @@ int mv88e6xxx_set_addr_indirect(struct dsa_switch *ds, u8 *addr)
 	return 0;
 }
 
-int mv88e6xxx_phy_read(struct dsa_switch *ds, int addr, int regnum)
+/* Must be called with phy mutex held */
+static int _mv88e6xxx_phy_read(struct dsa_switch *ds, int addr, int regnum)
 {
 	if (addr >= 0)
 		return mv88e6xxx_reg_read(ds, addr, regnum);
 	return 0xffff;
 }
 
-int mv88e6xxx_phy_write(struct dsa_switch *ds, int addr, int regnum, u16 val)
+/* Must be called with phy mutex held */
+static int _mv88e6xxx_phy_write(struct dsa_switch *ds, int addr, int regnum,
+				u16 val)
 {
 	if (addr >= 0)
 		return mv88e6xxx_reg_write(ds, addr, regnum, val);
@@ -579,37 +582,37 @@ int  mv88e6xxx_get_temp(struct dsa_switch *ds, int *temp)
 
 	mutex_lock(&ps->phy_mutex);
 
-	ret = mv88e6xxx_phy_write(ds, 0x0, 0x16, 0x6);
+	ret = _mv88e6xxx_phy_write(ds, 0x0, 0x16, 0x6);
 	if (ret < 0)
 		goto error;
 
 	/* Enable temperature sensor */
-	ret = mv88e6xxx_phy_read(ds, 0x0, 0x1a);
+	ret = _mv88e6xxx_phy_read(ds, 0x0, 0x1a);
 	if (ret < 0)
 		goto error;
 
-	ret = mv88e6xxx_phy_write(ds, 0x0, 0x1a, ret | (1 << 5));
+	ret = _mv88e6xxx_phy_write(ds, 0x0, 0x1a, ret | (1 << 5));
 	if (ret < 0)
 		goto error;
 
 	/* Wait for temperature to stabilize */
 	usleep_range(10000, 12000);
 
-	val = mv88e6xxx_phy_read(ds, 0x0, 0x1a);
+	val = _mv88e6xxx_phy_read(ds, 0x0, 0x1a);
 	if (val < 0) {
 		ret = val;
 		goto error;
 	}
 
 	/* Disable temperature sensor */
-	ret = mv88e6xxx_phy_write(ds, 0x0, 0x1a, ret & ~(1 << 5));
+	ret = _mv88e6xxx_phy_write(ds, 0x0, 0x1a, ret & ~(1 << 5));
 	if (ret < 0)
 		goto error;
 
 	*temp = ((val & 0x1f) - 5) * 5;
 
 error:
-	mv88e6xxx_phy_write(ds, 0x0, 0x16, 0x0);
+	_mv88e6xxx_phy_write(ds, 0x0, 0x16, 0x0);
 	mutex_unlock(&ps->phy_mutex);
 	return ret;
 }
@@ -671,7 +674,9 @@ static int _mv88e6xxx_atu_wait(struct dsa_switch *ds)
 	return _mv88e6xxx_wait(ds, REG_GLOBAL, 0x0b, ATU_BUSY);
 }
 
-int mv88e6xxx_phy_read_indirect(struct dsa_switch *ds, int addr, int regnum)
+/* Must be called with phy mutex held */
+static int _mv88e6xxx_phy_read_indirect(struct dsa_switch *ds, int addr,
+					int regnum)
 {
 	int ret;
 
@@ -684,8 +689,9 @@ int mv88e6xxx_phy_read_indirect(struct dsa_switch *ds, int addr, int regnum)
 	return REG_READ(REG_GLOBAL2, 0x19);
 }
 
-int mv88e6xxx_phy_write_indirect(struct dsa_switch *ds, int addr, int regnum,
-				 u16 val)
+/* Must be called with phy mutex held */
+static int _mv88e6xxx_phy_write_indirect(struct dsa_switch *ds, int addr,
+					 int regnum, u16 val)
 {
 	REG_WRITE(REG_GLOBAL2, 0x19, val);
 	REG_WRITE(REG_GLOBAL2, 0x18, 0x9400 | (addr << 5) | regnum);
@@ -715,7 +721,7 @@ static int mv88e6xxx_eee_enable_set(struct dsa_switch *ds, int port,
 {
 	int reg, nreg;
 
-	reg = mv88e6xxx_phy_read_indirect(ds, port, 16);
+	reg = _mv88e6xxx_phy_read_indirect(ds, port, 16);
 	if (reg < 0)
 		return reg;
 
@@ -726,7 +732,7 @@ static int mv88e6xxx_eee_enable_set(struct dsa_switch *ds, int port,
 		nreg |= 0x0100;
 
 	if (nreg != reg)
-		return mv88e6xxx_phy_write_indirect(ds, port, 16, nreg);
+		return _mv88e6xxx_phy_write_indirect(ds, port, 16, nreg);
 
 	return 0;
 }
@@ -1207,12 +1213,12 @@ int mv88e6xxx_phy_page_read(struct dsa_switch *ds, int port, int page, int reg)
 	int ret;
 
 	mutex_lock(&ps->phy_mutex);
-	ret = mv88e6xxx_phy_write_indirect(ds, port, 0x16, page);
+	ret = _mv88e6xxx_phy_write_indirect(ds, port, 0x16, page);
 	if (ret < 0)
 		goto error;
-	ret = mv88e6xxx_phy_read_indirect(ds, port, reg);
+	ret = _mv88e6xxx_phy_read_indirect(ds, port, reg);
 error:
-	mv88e6xxx_phy_write_indirect(ds, port, 0x16, 0x0);
+	_mv88e6xxx_phy_write_indirect(ds, port, 0x16, 0x0);
 	mutex_unlock(&ps->phy_mutex);
 	return ret;
 }
@@ -1224,13 +1230,87 @@ int mv88e6xxx_phy_page_write(struct dsa_switch *ds, int port, int page,
 	int ret;
 
 	mutex_lock(&ps->phy_mutex);
-	ret = mv88e6xxx_phy_write_indirect(ds, port, 0x16, page);
+	ret = _mv88e6xxx_phy_write_indirect(ds, port, 0x16, page);
 	if (ret < 0)
 		goto error;
 
-	ret = mv88e6xxx_phy_write_indirect(ds, port, reg, val);
+	ret = _mv88e6xxx_phy_write_indirect(ds, port, reg, val);
 error:
-	mv88e6xxx_phy_write_indirect(ds, port, 0x16, 0x0);
+	_mv88e6xxx_phy_write_indirect(ds, port, 0x16, 0x0);
+	mutex_unlock(&ps->phy_mutex);
+	return ret;
+}
+
+static int mv88e6xxx_port_to_phy_addr(struct dsa_switch *ds, int port)
+{
+	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
+
+	if (port >= 0 && port < ps->num_ports)
+		return port;
+	return -EINVAL;
+}
+
+int
+mv88e6xxx_phy_read(struct dsa_switch *ds, int port, int regnum)
+{
+	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
+	int addr = mv88e6xxx_port_to_phy_addr(ds, port);
+	int ret;
+
+	if (addr < 0)
+		return addr;
+
+	mutex_lock(&ps->phy_mutex);
+	ret = _mv88e6xxx_phy_read(ds, addr, regnum);
+	mutex_unlock(&ps->phy_mutex);
+	return ret;
+}
+
+int
+mv88e6xxx_phy_write(struct dsa_switch *ds, int port, int regnum, u16 val)
+{
+	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
+	int addr = mv88e6xxx_port_to_phy_addr(ds, port);
+	int ret;
+
+	if (addr < 0)
+		return addr;
+
+	mutex_lock(&ps->phy_mutex);
+	ret = _mv88e6xxx_phy_write(ds, addr, regnum, val);
+	mutex_unlock(&ps->phy_mutex);
+	return ret;
+}
+
+int
+mv88e6xxx_phy_read_indirect(struct dsa_switch *ds, int port, int regnum)
+{
+	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
+	int addr = mv88e6xxx_port_to_phy_addr(ds, port);
+	int ret;
+
+	if (addr < 0)
+		return addr;
+
+	mutex_lock(&ps->phy_mutex);
+	ret = _mv88e6xxx_phy_read_indirect(ds, addr, regnum);
+	mutex_unlock(&ps->phy_mutex);
+	return ret;
+}
+
+int
+mv88e6xxx_phy_write_indirect(struct dsa_switch *ds, int port, int regnum,
+			     u16 val)
+{
+	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
+	int addr = mv88e6xxx_port_to_phy_addr(ds, port);
+	int ret;
+
+	if (addr < 0)
+		return addr;
+
+	mutex_lock(&ps->phy_mutex);
+	ret = _mv88e6xxx_phy_write_indirect(ds, addr, regnum, val);
 	mutex_unlock(&ps->phy_mutex);
 	return ret;
 }