net: dsa: mv88e6xxx: Factor out common initialization code

[linux-drm-fsl-dcu.git] / drivers / net / dsa / mv88e6xxx.c

diff --git a/drivers/net/dsa/mv88e6xxx.c b/drivers/net/dsa/mv88e6xxx.c
index 3e7e31a6abb73f28ea44c9314c4f3628337edab7..6496beb74510ab0d7bedb9dfb7d9fe3fbdcafc19 100644 (file)
--- a/drivers/net/dsa/mv88e6xxx.c
+++ b/drivers/net/dsa/mv88e6xxx.c
@@ -596,6 +596,121 @@ error:
 }
 #endif /* CONFIG_NET_DSA_HWMON */
 
+static int mv88e6xxx_wait(struct dsa_switch *ds, int reg, int offset, u16 mask)
+{
+       unsigned long timeout = jiffies + HZ / 10;
+
+       while (time_before(jiffies, timeout)) {
+               int ret;
+
+               ret = REG_READ(reg, offset);
+               if (!(ret & mask))
+                       return 0;
+
+               usleep_range(1000, 2000);
+       }
+       return -ETIMEDOUT;
+}
+
+int mv88e6xxx_phy_wait(struct dsa_switch *ds)
+{
+       return mv88e6xxx_wait(ds, REG_GLOBAL2, 0x18, 0x8000);
+}
+
+int mv88e6xxx_eeprom_load_wait(struct dsa_switch *ds)
+{
+       return mv88e6xxx_wait(ds, REG_GLOBAL2, 0x14, 0x0800);
+}
+
+int mv88e6xxx_eeprom_busy_wait(struct dsa_switch *ds)
+{
+       return mv88e6xxx_wait(ds, REG_GLOBAL2, 0x14, 0x8000);
+}
+
+int mv88e6xxx_phy_read_indirect(struct dsa_switch *ds, int addr, int regnum)
+{
+       int ret;
+
+       REG_WRITE(REG_GLOBAL2, 0x18, 0x9800 | (addr << 5) | regnum);
+
+       ret = mv88e6xxx_phy_wait(ds);
+       if (ret < 0)
+               return ret;
+
+       return REG_READ(REG_GLOBAL2, 0x19);
+}
+
+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);
+
+       return mv88e6xxx_phy_wait(ds);
+}
+
+int mv88e6xxx_get_eee(struct dsa_switch *ds, int port, struct ethtool_eee *e)
+{
+       int reg;
+
+       reg = mv88e6xxx_phy_read_indirect(ds, port, 16);
+       if (reg < 0)
+               return -EOPNOTSUPP;
+
+       e->eee_enabled = !!(reg & 0x0200);
+       e->tx_lpi_enabled = !!(reg & 0x0100);
+
+       reg = REG_READ(REG_PORT(port), 0);
+       e->eee_active = !!(reg & 0x0040);
+
+       return 0;
+}
+
+static int mv88e6xxx_eee_enable_set(struct dsa_switch *ds, int port,
+                                   bool eee_enabled, bool tx_lpi_enabled)
+{
+       int reg, nreg;
+
+       reg = mv88e6xxx_phy_read_indirect(ds, port, 16);
+       if (reg < 0)
+               return reg;
+
+       nreg = reg & ~0x0300;
+       if (eee_enabled)
+               nreg |= 0x0200;
+       if (tx_lpi_enabled)
+               nreg |= 0x0100;
+
+       if (nreg != reg)
+               return mv88e6xxx_phy_write_indirect(ds, port, 16, nreg);
+
+       return 0;
+}
+
+int mv88e6xxx_set_eee(struct dsa_switch *ds, int port,
+                     struct phy_device *phydev, struct ethtool_eee *e)
+{
+       int ret;
+
+       ret = mv88e6xxx_eee_enable_set(ds, port, e->eee_enabled,
+                                      e->tx_lpi_enabled);
+       if (ret)
+               return -EOPNOTSUPP;
+
+       return 0;
+}
+
+int mv88e6xxx_setup_common(struct dsa_switch *ds)
+{
+       struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
+
+       mutex_init(&ps->smi_mutex);
+       mutex_init(&ps->stats_mutex);
+       mutex_init(&ps->phy_mutex);
+
+       return 0;
+}
+
 static int __init mv88e6xxx_init(void)
 {
 #if IS_ENABLED(CONFIG_NET_DSA_MV88E6131)