diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c index 01d181c2c68e..6d4ce35e990f 100644 --- a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c +++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c @@ -156,6 +156,7 @@ static void stmmac_tx_timer_arm(struct stmmac_priv *priv, u32 queue); static void stmmac_flush_tx_descriptors(struct stmmac_priv *priv, int queue); static void stmmac_set_dma_operation_mode(struct stmmac_priv *priv, u32 txmode, u32 rxmode, u32 chan); +static int stmmac_vlan_restore(struct stmmac_priv *priv); #ifdef CONFIG_DEBUG_FS static const struct net_device_ops stmmac_netdev_ops; @@ -4107,6 +4108,8 @@ static int __stmmac_open(struct net_device *dev, phylink_start(priv->phylink); + stmmac_vlan_restore(priv); + ret = stmmac_request_irq(dev); if (ret) goto irq_error; @@ -6853,6 +6856,23 @@ del_vlan_error: return ret; } +static int stmmac_vlan_restore(struct stmmac_priv *priv) +{ + int ret; + + if (!(priv->dev->features & NETIF_F_VLAN_FEATURES)) + return 0; + + if (priv->hw->num_vlan) + stmmac_restore_hw_vlan_rx_fltr(priv, priv->dev, priv->hw); + + ret = stmmac_vlan_update(priv, priv->num_double_vlans); + if (ret) + netdev_err(priv->dev, "Failed to restore VLANs\n"); + + return ret; +} + static int stmmac_bpf(struct net_device *dev, struct netdev_bpf *bpf) { struct stmmac_priv *priv = netdev_priv(dev); @@ -8277,10 +8297,10 @@ int stmmac_resume(struct device *dev) stmmac_init_coalesce(priv); phylink_rx_clk_stop_block(priv->phylink); stmmac_set_rx_mode(ndev); - - stmmac_restore_hw_vlan_rx_fltr(priv, ndev, priv->hw); phylink_rx_clk_stop_unblock(priv->phylink); + stmmac_vlan_restore(priv); + stmmac_enable_all_queues(priv); stmmac_enable_all_dma_irq(priv); diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_vlan.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_vlan.c index de1a70e1c86e..fcc34867405e 100644 --- a/drivers/net/ethernet/stmicro/stmmac/stmmac_vlan.c +++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_vlan.c @@ -139,9 +139,6 @@ static int vlan_del_hw_rx_fltr(struct net_device *dev, static void vlan_restore_hw_rx_fltr(struct net_device *dev, struct mac_device_info *hw) { - void __iomem *ioaddr = hw->pcsr; - u32 value; - u32 hash; u32 val; int i; @@ -158,13 +155,6 @@ static void vlan_restore_hw_rx_fltr(struct net_device *dev, vlan_write_filter(dev, hw, i, val); } } - - hash = readl(ioaddr + VLAN_HASH_TABLE); - if (hash & VLAN_VLHT) { - value = readl(ioaddr + VLAN_TAG); - value |= VLAN_VTHM; - writel(value, ioaddr + VLAN_TAG); - } } static void vlan_update_hash(struct mac_device_info *hw, u32 hash,