2 files changed, 45 insertions, 9 deletions

diff --git a/drivers/net/dsa/bcm_sf2.c b/drivers/net/dsa/bcm_sf2.c
index 45c0e2b97f5f..4daffb284931 100644
--- a/drivers/net/dsa/bcm_sf2.c
+++ b/drivers/net/dsa/bcm_sf2.c
@@ -238,17 +238,28 @@ static void bcm_sf2_gphy_enable_set(struct dsa_switch *ds, bool enable)
         struct bcm_sf2_priv *priv = ds_to_priv(ds);
         u32 reg;
 
-        if (!enable)
-                return;
-
-        reg = reg_readl(priv, REG_SPHY_CNTRL);
-        reg |= PHY_RESET;
-        reg &= ~(EXT_PWR_DOWN | IDDQ_BIAS);
-        reg_writel(priv, reg, REG_SPHY_CNTRL);
-        udelay(21);
         reg = reg_readl(priv, REG_SPHY_CNTRL);
-        reg &= ~PHY_RESET;
+        if (enable) {
+                reg |= PHY_RESET;
+                reg &= ~(EXT_PWR_DOWN | IDDQ_BIAS | CK25_DIS);
+                reg_writel(priv, reg, REG_SPHY_CNTRL);
+                udelay(21);
+                reg = reg_readl(priv, REG_SPHY_CNTRL);
+                reg &= ~PHY_RESET;
+        } else {
+                reg |= EXT_PWR_DOWN | IDDQ_BIAS | PHY_RESET;
+                reg_writel(priv, reg, REG_SPHY_CNTRL);
+                mdelay(1);
+                reg |= CK25_DIS;
+        }
         reg_writel(priv, reg, REG_SPHY_CNTRL);
+
+        /* Use PHY-driven LED signaling */
+        if (!enable) {
+                reg = reg_readl(priv, REG_LED_CNTRL(0));
+                reg |= SPDLNK_SRC_SEL;
+                reg_writel(priv, reg, REG_LED_CNTRL(0));
+        }
 }
 
 static int bcm_sf2_port_setup(struct dsa_switch *ds, int port,
@@ -266,6 +277,24 @@ static int bcm_sf2_port_setup(struct dsa_switch *ds, int port,
         /* Clear the Rx and Tx disable bits and set to no spanning tree */
         core_writel(priv, 0, CORE_G_PCTL_PORT(port));
 
+        /* Re-enable the GPHY and re-apply workarounds */
+        if (port == 0 && priv->hw_params.num_gphy == 1) {
+                bcm_sf2_gphy_enable_set(ds, true);
+                if (phy) {
+                        /* if phy_stop() has been called before, phy
+                         * will be in halted state, and phy_start()
+                         * will call resume.
+                         *
+                         * the resume path does not configure back
+                         * autoneg settings, and since we hard reset
+                         * the phy manually here, we need to reset the
+                         * state machine also.
+                         */
+                        phy->state = PHY_READY;
+                        phy_init_hw(phy);
+                }
+        }
+
         /* Enable port 7 interrupts to get notified */
         if (port == 7)
                 intrl2_1_mask_clear(priv, P_IRQ_MASK(P7_IRQ_OFF));
@@ -299,6 +328,9 @@ static void bcm_sf2_port_disable(struct dsa_switch *ds, int port,
                 intrl2_1_writel(priv, P_IRQ_MASK(P7_IRQ_OFF), INTRL2_CPU_CLEAR);
         }
 
+        if (port == 0 && priv->hw_params.num_gphy == 1)
+                bcm_sf2_gphy_enable_set(ds, false);
+
         if (dsa_is_cpu_port(ds, port))
                 off = CORE_IMP_CTL;
         else
diff --git a/drivers/net/dsa/bcm_sf2_regs.h b/drivers/net/dsa/bcm_sf2_regs.h
index 1bb49cb699ab..cabdfa5e217a 100644
--- a/drivers/net/dsa/bcm_sf2_regs.h
+++ b/drivers/net/dsa/bcm_sf2_regs.h
@@ -61,6 +61,10 @@
 #define  LPI_COUNT_SHIFT                9
 #define  LPI_COUNT_MASK                 0x3F
 
+#define REG_LED_CNTRL_BASE              0x90
+#define REG_LED_CNTRL(x)                (REG_LED_CNTRL_BASE + (x) * 4)
+#define  SPDLNK_SRC_SEL                 (1 << 24)
+
 /* Register set relative to 'INTRL2_0' and 'INTRL2_1' */
 #define INTRL2_CPU_STATUS               0x00
 #define INTRL2_CPU_SET                  0x04