1 files changed, 34 insertions, 39 deletions

diff --git a/drivers/net/ethernet/marvell/mvneta.c b/drivers/net/ethernet/marvell/mvneta.c
index b248bcbdae63..14786c8bf99e 100644
--- a/drivers/net/ethernet/marvell/mvneta.c
+++ b/drivers/net/ethernet/marvell/mvneta.c
@@ -89,8 +89,9 @@
 #define      MVNETA_TX_IN_PRGRS                  BIT(1)
 #define      MVNETA_TX_FIFO_EMPTY                BIT(8)
 #define MVNETA_RX_MIN_FRAME_SIZE                 0x247c
-#define MVNETA_SGMII_SERDES_CFG                  0x24A0
+#define MVNETA_SERDES_CFG                        0x24A0
 #define      MVNETA_SGMII_SERDES_PROTO           0x0cc7
+#define      MVNETA_QSGMII_SERDES_PROTO          0x0667
 #define MVNETA_TYPE_PRIO                         0x24bc
 #define      MVNETA_FORCE_UNI                    BIT(21)
 #define MVNETA_TXQ_CMD_1                         0x24e4
@@ -711,35 +712,6 @@ static void mvneta_rxq_bm_disable(struct mvneta_port *pp,
         mvreg_write(pp, MVNETA_RXQ_CONFIG_REG(rxq->id), val);
 }
 
-
-
-/* Sets the RGMII Enable bit (RGMIIEn) in port MAC control register */
-static void mvneta_gmac_rgmii_set(struct mvneta_port *pp, int enable)
-{
-        u32  val;
-
-        val = mvreg_read(pp, MVNETA_GMAC_CTRL_2);
-
-        if (enable)
-                val |= MVNETA_GMAC2_PORT_RGMII;
-        else
-                val &= ~MVNETA_GMAC2_PORT_RGMII;
-
-        mvreg_write(pp, MVNETA_GMAC_CTRL_2, val);
-}
-
-/* Config SGMII port */
-static void mvneta_port_sgmii_config(struct mvneta_port *pp)
-{
-        u32 val;
-
-        val = mvreg_read(pp, MVNETA_GMAC_CTRL_2);
-        val |= MVNETA_GMAC2_PCS_ENABLE;
-        mvreg_write(pp, MVNETA_GMAC_CTRL_2, val);
-
-        mvreg_write(pp, MVNETA_SGMII_SERDES_CFG, MVNETA_SGMII_SERDES_PROTO);
-}
-
 /* Start the Ethernet port RX and TX activity */
 static void mvneta_port_up(struct mvneta_port *pp)
 {
@@ -2749,26 +2721,44 @@ static void mvneta_conf_mbus_windows(struct mvneta_port *pp,
 }
 
 /* Power up the port */
-static void mvneta_port_power_up(struct mvneta_port *pp, int phy_mode)
+static int mvneta_port_power_up(struct mvneta_port *pp, int phy_mode)
 {
-        u32 val;
+        u32 ctrl;
 
         /* MAC Cause register should be cleared */
         mvreg_write(pp, MVNETA_UNIT_INTR_CAUSE, 0);
 
-        if (phy_mode == PHY_INTERFACE_MODE_SGMII)
-                mvneta_port_sgmii_config(pp);
+        ctrl = mvreg_read(pp, MVNETA_GMAC_CTRL_2);
 
-        mvneta_gmac_rgmii_set(pp, 1);
+        /* Even though it might look weird, when we're configured in
+         * SGMII or QSGMII mode, the RGMII bit needs to be set.
+         */
+        switch(phy_mode) {
+        case PHY_INTERFACE_MODE_QSGMII:
+                mvreg_write(pp, MVNETA_SERDES_CFG, MVNETA_QSGMII_SERDES_PROTO);
+                ctrl |= MVNETA_GMAC2_PCS_ENABLE | MVNETA_GMAC2_PORT_RGMII;
+                break;
+        case PHY_INTERFACE_MODE_SGMII:
+                mvreg_write(pp, MVNETA_SERDES_CFG, MVNETA_SGMII_SERDES_PROTO);
+                ctrl |= MVNETA_GMAC2_PCS_ENABLE | MVNETA_GMAC2_PORT_RGMII;
+                break;
+        case PHY_INTERFACE_MODE_RGMII:
+        case PHY_INTERFACE_MODE_RGMII_ID:
+                ctrl |= MVNETA_GMAC2_PORT_RGMII;
+                break;
+        default:
+                return -EINVAL;
+        }
 
         /* Cancel Port Reset */
-        val = mvreg_read(pp, MVNETA_GMAC_CTRL_2);
-        val &= ~MVNETA_GMAC2_PORT_RESET;
-        mvreg_write(pp, MVNETA_GMAC_CTRL_2, val);
+        ctrl &= ~MVNETA_GMAC2_PORT_RESET;
+        mvreg_write(pp, MVNETA_GMAC_CTRL_2, ctrl);
 
         while ((mvreg_read(pp, MVNETA_GMAC_CTRL_2) &
                 MVNETA_GMAC2_PORT_RESET) != 0)
                 continue;
+
+        return 0;
 }
 
 /* Device initialization routine */
@@ -2879,7 +2869,12 @@ static int mvneta_probe(struct platform_device *pdev)
                 dev_err(&pdev->dev, "can't init eth hal\n");
                 goto err_free_stats;
         }
-        mvneta_port_power_up(pp, phy_mode);
+
+        err = mvneta_port_power_up(pp, phy_mode);
+        if (err < 0) {
+                dev_err(&pdev->dev, "can't power up port\n");
+                goto err_deinit;
+        }
 
         dram_target_info = mv_mbus_dram_info();
         if (dram_target_info)