net: ethernet: mvneta: Fix Serdes configuration for SoCs without comphy

[platform/kernel/linux-starfive.git] / drivers / net / ethernet / marvell / mvneta.c

diff --git a/drivers/net/ethernet/marvell/mvneta.c b/drivers/net/ethernet/marvell/mvneta.c
index 946925b..987aaaa 100644 (file)
--- a/drivers/net/ethernet/marvell/mvneta.c
+++ b/drivers/net/ethernet/marvell/mvneta.c
@@ -106,6 +106,7 @@
 #define      MVNETA_TX_IN_PRGRS                  BIT(1)
 #define      MVNETA_TX_FIFO_EMPTY                BIT(8)
 #define MVNETA_RX_MIN_FRAME_SIZE                 0x247c
+/* Only exists on Armada XP and Armada 370 */
 #define MVNETA_SERDES_CFG                       0x24A0
 #define      MVNETA_SGMII_SERDES_PROTO          0x0cc7
 #define      MVNETA_QSGMII_SERDES_PROTO                 0x0667
@@ -3529,26 +3530,55 @@ static int mvneta_setup_txqs(struct mvneta_port *pp)
        return 0;
 }
 
-static int mvneta_comphy_init(struct mvneta_port *pp)
+static int mvneta_comphy_init(struct mvneta_port *pp, phy_interface_t interface)
 {
        int ret;
 
-       if (!pp->comphy)
-               return 0;
-
-       ret = phy_set_mode_ext(pp->comphy, PHY_MODE_ETHERNET,
-                              pp->phy_interface);
+       ret = phy_set_mode_ext(pp->comphy, PHY_MODE_ETHERNET, interface);
        if (ret)
                return ret;
 
        return phy_power_on(pp->comphy);
 }
 
+static int mvneta_config_interface(struct mvneta_port *pp,
+                                  phy_interface_t interface)
+{
+       int ret = 0;
+
+       if (pp->comphy) {
+               if (interface == PHY_INTERFACE_MODE_SGMII ||
+                   interface == PHY_INTERFACE_MODE_1000BASEX ||
+                   interface == PHY_INTERFACE_MODE_2500BASEX) {
+                       ret = mvneta_comphy_init(pp, interface);
+               }
+       } else {
+               switch (interface) {
+               case PHY_INTERFACE_MODE_QSGMII:
+                       mvreg_write(pp, MVNETA_SERDES_CFG,
+                                   MVNETA_QSGMII_SERDES_PROTO);
+                       break;
+
+               case PHY_INTERFACE_MODE_SGMII:
+               case PHY_INTERFACE_MODE_1000BASEX:
+                       mvreg_write(pp, MVNETA_SERDES_CFG,
+                                   MVNETA_SGMII_SERDES_PROTO);
+                       break;
+               default:
+                       return -EINVAL;
+               }
+       }
+
+       pp->phy_interface = interface;
+
+       return ret;
+}
+
 static void mvneta_start_dev(struct mvneta_port *pp)
 {
        int cpu;
 
-       WARN_ON(mvneta_comphy_init(pp));
+       WARN_ON(mvneta_config_interface(pp, pp->phy_interface));
 
        mvneta_max_rx_size_set(pp, pp->pkt_size);
        mvneta_txq_max_tx_size_set(pp, pp->pkt_size);
@@ -3926,14 +3956,10 @@ static void mvneta_mac_config(struct phylink_config *config, unsigned int mode,
        if (state->speed == SPEED_2500)
                new_ctrl4 |= MVNETA_GMAC4_SHORT_PREAMBLE_ENABLE;
 
-       if (pp->comphy && pp->phy_interface != state->interface &&
-           (state->interface == PHY_INTERFACE_MODE_SGMII ||
-            state->interface == PHY_INTERFACE_MODE_1000BASEX ||
-            state->interface == PHY_INTERFACE_MODE_2500BASEX)) {
-               pp->phy_interface = state->interface;
-
-               WARN_ON(phy_power_off(pp->comphy));
-               WARN_ON(mvneta_comphy_init(pp));
+       if (pp->phy_interface != state->interface) {
+               if (pp->comphy)
+                       WARN_ON(phy_power_off(pp->comphy));
+               WARN_ON(mvneta_config_interface(pp, state->interface));
        }
 
        if (new_ctrl0 != gmac_ctrl0)
@@ -4977,20 +5003,10 @@ static void mvneta_conf_mbus_windows(struct mvneta_port *pp,
 }
 
 /* Power up the port */
-static int mvneta_port_power_up(struct mvneta_port *pp, int phy_mode)
+static void mvneta_port_power_up(struct mvneta_port *pp, int phy_mode)
 {
        /* MAC Cause register should be cleared */
        mvreg_write(pp, MVNETA_UNIT_INTR_CAUSE, 0);
-
-       if (phy_mode == PHY_INTERFACE_MODE_QSGMII)
-               mvreg_write(pp, MVNETA_SERDES_CFG, MVNETA_QSGMII_SERDES_PROTO);
-       else if (phy_mode == PHY_INTERFACE_MODE_SGMII ||
-                phy_interface_mode_is_8023z(phy_mode))
-               mvreg_write(pp, MVNETA_SERDES_CFG, MVNETA_SGMII_SERDES_PROTO);
-       else if (!phy_interface_mode_is_rgmii(phy_mode))
-               return -EINVAL;
-
-       return 0;
 }
 
 /* Device initialization routine */
@@ -5176,11 +5192,7 @@ static int mvneta_probe(struct platform_device *pdev)
        if (err < 0)
                goto err_netdev;
 
-       err = mvneta_port_power_up(pp, phy_mode);
-       if (err < 0) {
-               dev_err(&pdev->dev, "can't power up port\n");
-               goto err_netdev;
-       }
+       mvneta_port_power_up(pp, phy_mode);
 
        /* Armada3700 network controller does not support per-cpu
         * operation, so only single NAPI should be initialized.
@@ -5334,11 +5346,7 @@ static int mvneta_resume(struct device *device)
                }
        }
        mvneta_defaults_set(pp);
-       err = mvneta_port_power_up(pp, pp->phy_interface);
-       if (err < 0) {
-               dev_err(device, "can't power up port\n");
-               return err;
-       }
+       mvneta_port_power_up(pp, pp->phy_interface);
 
        netif_device_attach(dev);