diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac.h b/drivers/net/ethernet/stmicro/stmmac/stmmac.h index 51c96a738151..33667a26708c 100644 --- a/drivers/net/ethernet/stmicro/stmmac/stmmac.h +++ b/drivers/net/ethernet/stmicro/stmmac/stmmac.h @@ -323,6 +323,7 @@ struct stmmac_priv { void __iomem *ptpaddr; void __iomem *estaddr; unsigned long active_vlans[BITS_TO_LONGS(VLAN_N_VID)]; + unsigned int num_double_vlans; int sfty_irq; int sfty_ce_irq; int sfty_ue_irq; diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c index b6a127316ab5..01d181c2c68e 100644 --- a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c +++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c @@ -6775,6 +6775,7 @@ static int stmmac_vlan_update(struct stmmac_priv *priv, bool is_double) static int stmmac_vlan_rx_add_vid(struct net_device *ndev, __be16 proto, u16 vid) { struct stmmac_priv *priv = netdev_priv(ndev); + unsigned int num_double_vlans; bool is_double = false; int ret; @@ -6786,7 +6787,8 @@ static int stmmac_vlan_rx_add_vid(struct net_device *ndev, __be16 proto, u16 vid is_double = true; set_bit(vid, priv->active_vlans); - ret = stmmac_vlan_update(priv, is_double); + num_double_vlans = priv->num_double_vlans + is_double; + ret = stmmac_vlan_update(priv, num_double_vlans); if (ret) { clear_bit(vid, priv->active_vlans); goto err_pm_put; @@ -6796,11 +6798,13 @@ static int stmmac_vlan_rx_add_vid(struct net_device *ndev, __be16 proto, u16 vid ret = stmmac_add_hw_vlan_rx_fltr(priv, ndev, priv->hw, proto, vid); if (ret) { clear_bit(vid, priv->active_vlans); - stmmac_vlan_update(priv, is_double); + stmmac_vlan_update(priv, priv->num_double_vlans); goto err_pm_put; } } + priv->num_double_vlans = num_double_vlans; + err_pm_put: pm_runtime_put(priv->device); @@ -6813,6 +6817,7 @@ err_pm_put: static int stmmac_vlan_rx_kill_vid(struct net_device *ndev, __be16 proto, u16 vid) { struct stmmac_priv *priv = netdev_priv(ndev); + unsigned int num_double_vlans; bool is_double = false; int ret; @@ -6824,7 +6829,8 @@ static int stmmac_vlan_rx_kill_vid(struct net_device *ndev, __be16 proto, u16 vi is_double = true; clear_bit(vid, priv->active_vlans); - ret = stmmac_vlan_update(priv, is_double); + num_double_vlans = priv->num_double_vlans - is_double; + ret = stmmac_vlan_update(priv, num_double_vlans); if (ret) { set_bit(vid, priv->active_vlans); goto del_vlan_error; @@ -6834,11 +6840,13 @@ static int stmmac_vlan_rx_kill_vid(struct net_device *ndev, __be16 proto, u16 vi ret = stmmac_del_hw_vlan_rx_fltr(priv, ndev, priv->hw, proto, vid); if (ret) { set_bit(vid, priv->active_vlans); - stmmac_vlan_update(priv, is_double); + stmmac_vlan_update(priv, priv->num_double_vlans); goto del_vlan_error; } } + priv->num_double_vlans = num_double_vlans; + del_vlan_error: pm_runtime_put(priv->device); diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_vlan.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_vlan.c index b18404dd5a8b..de1a70e1c86e 100644 --- a/drivers/net/ethernet/stmicro/stmmac/stmmac_vlan.c +++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_vlan.c @@ -183,6 +183,10 @@ static void vlan_update_hash(struct mac_device_info *hw, u32 hash, value |= VLAN_EDVLP; value |= VLAN_ESVL; value |= VLAN_DOVLTC; + } else { + value &= ~VLAN_EDVLP; + value &= ~VLAN_ESVL; + value &= ~VLAN_DOVLTC; } writel(value, ioaddr + VLAN_TAG); @@ -193,6 +197,10 @@ static void vlan_update_hash(struct mac_device_info *hw, u32 hash, value |= VLAN_EDVLP; value |= VLAN_ESVL; value |= VLAN_DOVLTC; + } else { + value &= ~VLAN_EDVLP; + value &= ~VLAN_ESVL; + value &= ~VLAN_DOVLTC; } writel(value | perfect_match, ioaddr + VLAN_TAG);