2 files changed, 54 insertions, 1 deletions

diff --git a/target/linux/generic/files/drivers/net/phy/mvsw61xx.c b/target/linux/generic/files/drivers/net/phy/mvsw61xx.c
index 4d6dfc52cd..e6074e35f3 100644
--- a/target/linux/generic/files/drivers/net/phy/mvsw61xx.c
+++ b/target/linux/generic/files/drivers/net/phy/mvsw61xx.c
@@ -148,6 +148,31 @@ mvsw61xx_wait_mask_s(struct switch_dev *dev, int addr,
 }
 
 static int
+mvsw61xx_mdio_read(struct switch_dev *dev, int addr, int reg)
+{
+	sw16(dev, MV_GLOBAL2REG(SMI_OP),
+	     MV_INDIRECT_READ | (addr << MV_INDIRECT_ADDR_S) | reg);
+
+	if (mvsw61xx_wait_mask_s(dev,  MV_GLOBAL2REG(SMI_OP),
+				 MV_INDIRECT_INPROGRESS, 0) < 0)
+		return -ETIMEDOUT;
+
+	return sr16(dev, MV_GLOBAL2REG(SMI_DATA));
+}
+
+static int
+mvsw61xx_mdio_write(struct switch_dev *dev, int addr, int reg, u16 val)
+{
+	sw16(dev, MV_GLOBAL2REG(SMI_DATA), val);
+
+	sw16(dev, MV_GLOBAL2REG(SMI_OP),
+	     MV_INDIRECT_WRITE | (addr << MV_INDIRECT_ADDR_S) | reg);
+
+	return mvsw61xx_wait_mask_s(dev,  MV_GLOBAL2REG(SMI_OP),
+				    MV_INDIRECT_INPROGRESS, 0) < 0;
+}
+
+static int
 mvsw61xx_get_port_mask(struct switch_dev *dev,
 		const struct switch_attr *attr, struct switch_val *val)
 {
@@ -566,7 +591,7 @@ static int mvsw61xx_apply(struct switch_dev *dev)
 	return mvsw61xx_update_state(dev);
 }
 
-static int mvsw61xx_reset(struct switch_dev *dev)
+static int _mvsw61xx_reset(struct switch_dev *dev, bool full)
 {
 	struct mvsw61xx_state *state = get_state(dev);
 	int i;
@@ -599,6 +624,17 @@ static int mvsw61xx_reset(struct switch_dev *dev)
 
 		/* Set port association vector */
 		sw16(dev, MV_PORTREG(ASSOC, i), (1 << i));
+
+		/* power up phys */
+		if (full && i < 5) {
+			mvsw61xx_mdio_write(dev, i, MII_MV_SPEC_CTRL,
+					    MV_SPEC_MDI_CROSS_AUTO |
+					    MV_SPEC_ENERGY_DETECT |
+					    MV_SPEC_DOWNSHIFT_COUNTER);
+			mvsw61xx_mdio_write(dev, i, MII_BMCR, BMCR_RESET |
+					    BMCR_ANENABLE | BMCR_FULLDPLX |
+					    BMCR_SPEED1000);
+		}
 	}
 
 	for (i = 0; i < dev->vlans; i++) {
@@ -623,6 +659,11 @@ static int mvsw61xx_reset(struct switch_dev *dev)
 	return 0;
 }
 
+static int mvsw61xx_reset(struct switch_dev *dev)
+{
+	return _mvsw61xx_reset(dev, false);
+}
+
 enum {
 	MVSW61XX_ENABLE_VLAN,
 };
@@ -798,6 +839,8 @@ static int mvsw61xx_probe(struct platform_device *pdev)
 	state->dev.ops = &mvsw61xx_ops;
 	state->dev.alias = dev_name(&pdev->dev);
 
+	_mvsw61xx_reset(&state->dev, true);
+
 	err = register_switch(&state->dev, NULL);
 	if (err < 0)
 		goto out_err;
diff --git a/target/linux/generic/files/drivers/net/phy/mvsw61xx.h b/target/linux/generic/files/drivers/net/phy/mvsw61xx.h
index ae78f5769d..64db6d3aab 100644
--- a/target/linux/generic/files/drivers/net/phy/mvsw61xx.h
+++ b/target/linux/generic/files/drivers/net/phy/mvsw61xx.h
@@ -162,6 +162,8 @@ enum {
 #define MV_GLOBALREG(_type) MV_SWITCH_GLOBAL, MV_GLOBAL_##_type
 
 enum {
+	MV_GLOBAL2_SMI_OP		= 0x18,
+	MV_GLOBAL2_SMI_DATA		= 0x19,
 	MV_GLOBAL2_SDET_POLARITY	= 0x1d,
 };
 #define MV_GLOBAL2REG(_type) MV_SWITCH_GLOBAL2, MV_GLOBAL2_##_type
@@ -229,6 +231,14 @@ enum {
 #define MV_FDB_HI_SHIFT			4
 #define MV_FDB_LO_SHIFT			12
 
+/* Marvell Specific PHY register */
+#define MII_MV_SPEC_CTRL		16
+enum {
+	MV_SPEC_MDI_CROSS_AUTO		= (0x6 << 4),
+	MV_SPEC_ENERGY_DETECT		= (0x3 << 8),
+	MV_SPEC_DOWNSHIFT_COUNTER	= (0x3 << 12),
+};
+
 struct mvsw61xx_state {
 	struct switch_dev dev;
 	struct mii_bus *bus;