mvsw61xx: enable SerDes on 6176 if required

[openwrt/staging/rmilecki.git] / target / linux / generic / files / drivers / net / phy / mvsw61xx.c

diff --git a/target/linux/generic/files/drivers/net/phy/mvsw61xx.c b/target/linux/generic/files/drivers/net/phy/mvsw61xx.c
index e6074e35f308863b961aafdbfa9bf0bcfb855b84..6bd112b00d325f1e2324d8acb60331dbbea32261 100644 (file)
--- a/target/linux/generic/files/drivers/net/phy/mvsw61xx.c
+++ b/target/linux/generic/files/drivers/net/phy/mvsw61xx.c
@@ -172,6 +172,27 @@ mvsw61xx_mdio_write(struct switch_dev *dev, int addr, int reg, u16 val)
                                    MV_INDIRECT_INPROGRESS, 0) < 0;
 }
 
+static int
+mvsw61xx_mdio_page_read(struct switch_dev *dev, int port, int page, int reg)
+{
+       int ret;
+
+       mvsw61xx_mdio_write(dev, port, MII_MV_PAGE, page);
+       ret = mvsw61xx_mdio_read(dev, port, reg);
+       mvsw61xx_mdio_write(dev, port, MII_MV_PAGE, 0);
+
+       return ret;
+}
+
+static void
+mvsw61xx_mdio_page_write(struct switch_dev *dev, int port, int page, int reg,
+                        u16 val)
+{
+       mvsw61xx_mdio_write(dev, port, MII_MV_PAGE, page);
+       mvsw61xx_mdio_write(dev, port, reg, val);
+       mvsw61xx_mdio_write(dev, port, MII_MV_PAGE, 0);
+}
+
 static int
 mvsw61xx_get_port_mask(struct switch_dev *dev,
                const struct switch_attr *attr, struct switch_val *val)
@@ -591,6 +612,19 @@ static int mvsw61xx_apply(struct switch_dev *dev)
        return mvsw61xx_update_state(dev);
 }
 
+static void mvsw61xx_enable_serdes(struct switch_dev *dev)
+{
+       int bmcr = mvsw61xx_mdio_page_read(dev, MV_REG_FIBER_SERDES,
+                                          MV_PAGE_FIBER_SERDES, MII_BMCR);
+       if (bmcr < 0)
+               return;
+
+       if (bmcr & BMCR_PDOWN)
+               mvsw61xx_mdio_page_write(dev, MV_REG_FIBER_SERDES,
+                                        MV_PAGE_FIBER_SERDES, MII_BMCR,
+                                        bmcr & ~BMCR_PDOWN);
+}
+
 static int _mvsw61xx_reset(struct switch_dev *dev, bool full)
 {
        struct mvsw61xx_state *state = get_state(dev);
@@ -635,6 +669,18 @@ static int _mvsw61xx_reset(struct switch_dev *dev, bool full)
                                            BMCR_ANENABLE | BMCR_FULLDPLX |
                                            BMCR_SPEED1000);
                }
+
+               /* enable SerDes if necessary */
+               if (full && i >= 5 && state->model == MV_IDENT_VALUE_6176) {
+                       u16 sts = sr16(dev, MV_PORTREG(STATUS, i));
+                       u16 mode = sts & MV_PORT_STATUS_CMODE_MASK;
+
+                       if (mode == MV_PORT_STATUS_CMODE_100BASE_X ||
+                           mode == MV_PORT_STATUS_CMODE_1000BASE_X ||
+                           mode == MV_PORT_STATUS_CMODE_SGMII) {
+                               mvsw61xx_enable_serdes(dev);
+                       }
+               }
        }
 
        for (i = 0; i < dev->vlans; i++) {