diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c index 8e555f4e82d7..3a190cf250e6 100644 --- a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c +++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c @@ -874,60 +874,7 @@ static void stmmac_mac_pcs_get_state(struct phylink_config *config, static void stmmac_mac_config(struct phylink_config *config, unsigned int mode, const struct phylink_link_state *state) { - struct stmmac_priv *priv = netdev_priv(to_net_dev(config->dev)); - u32 ctrl; - - ctrl = readl(priv->ioaddr + MAC_CTRL_REG); - ctrl &= ~priv->hw->link.speed_mask; - - if (state->interface == PHY_INTERFACE_MODE_USXGMII) { - switch (state->speed) { - case SPEED_10000: - ctrl |= priv->hw->link.xgmii.speed10000; - break; - case SPEED_5000: - ctrl |= priv->hw->link.xgmii.speed5000; - break; - case SPEED_2500: - ctrl |= priv->hw->link.xgmii.speed2500; - break; - default: - return; - } - } else { - switch (state->speed) { - case SPEED_2500: - ctrl |= priv->hw->link.speed2500; - break; - case SPEED_1000: - ctrl |= priv->hw->link.speed1000; - break; - case SPEED_100: - ctrl |= priv->hw->link.speed100; - break; - case SPEED_10: - ctrl |= priv->hw->link.speed10; - break; - default: - return; - } - } - - priv->speed = state->speed; - - if (priv->plat->fix_mac_speed) - priv->plat->fix_mac_speed(priv->plat->bsp_priv, state->speed); - - if (!state->duplex) - ctrl &= ~priv->hw->link.duplex; - else - ctrl |= priv->hw->link.duplex; - - /* Flow Control operation */ - if (state->pause) - stmmac_mac_flow_ctrl(priv, state->duplex); - - writel(ctrl, priv->ioaddr + MAC_CTRL_REG); + /* Nothing for now. */ } static void stmmac_mac_an_restart(struct phylink_config *config) @@ -953,6 +900,59 @@ static void stmmac_mac_link_up(struct phylink_config *config, bool tx_pause, bool rx_pause) { struct stmmac_priv *priv = netdev_priv(to_net_dev(config->dev)); + u32 ctrl; + + ctrl = readl(priv->ioaddr + MAC_CTRL_REG); + ctrl &= ~priv->hw->link.speed_mask; + + if (interface == PHY_INTERFACE_MODE_USXGMII) { + switch (speed) { + case SPEED_10000: + ctrl |= priv->hw->link.xgmii.speed10000; + break; + case SPEED_5000: + ctrl |= priv->hw->link.xgmii.speed5000; + break; + case SPEED_2500: + ctrl |= priv->hw->link.xgmii.speed2500; + break; + default: + return; + } + } else { + switch (speed) { + case SPEED_2500: + ctrl |= priv->hw->link.speed2500; + break; + case SPEED_1000: + ctrl |= priv->hw->link.speed1000; + break; + case SPEED_100: + ctrl |= priv->hw->link.speed100; + break; + case SPEED_10: + ctrl |= priv->hw->link.speed10; + break; + default: + return; + } + } + + priv->speed = speed; + + if (priv->plat->fix_mac_speed) + priv->plat->fix_mac_speed(priv->plat->bsp_priv, speed); + + if (!duplex) + ctrl &= ~priv->hw->link.duplex; + else + ctrl |= priv->hw->link.duplex; + + /* Flow Control operation */ + if (tx_pause && rx_pause) + stmmac_mac_flow_ctrl(priv, duplex); + + writel(ctrl, priv->ioaddr + MAC_CTRL_REG); stmmac_mac_set(priv, priv->ioaddr, true); if (phy && priv->dma_cap.eee) {