net: stmmac: vlan: Restore VLAN filters in set_rx_mode()

The commit bd7ad51253a7 ("net: stmmac: Fix VLAN HW state restore") fixed
the problem with the VLAN filters not being restored if the device was
closed or after resume. That could have been done in a clearer and more
coherent way just by calling the stmmac_restore_hw_vlan_rx_fltr() method
in the stmmac_set_filter() method. The later in its turn is called in the
net_device_ops::ndo_set_rx_mode() callback of the driver, which in its
turn is called on device open and resumes. The only
problematic part is to store the HASH/perfect filters context. But it can
be done by adding two more mac_device_info fields aside with the Extended
VLAN perfect filter settings. A similar approach with cacheing and using
ndo_set_rx_mode() to restore VLAN-filters is implemented in some another
network drivers.

Let's do that then. As a nice side-effect the stmmac_vlan_restore() will
be no longer needed. Also the stmmac_ops::set_filter() methods now look
more coherent setting up the all MAC and VLAN filters.

Note the vlan_write_filter() method must be altered to call the
atomic-version of the CSR polling function, since it's caller -
vlan_restore_hw_vlan_rx_fltr() is now called from the BH-disabled context
(see dev_set_rx_mode()).

bd7ad51253a7 ("net: stmmac: Fix VLAN HW state restore")
Signed-off-by: Serge Semin <fancer.lancer@gmail.com>

5 files changed, 38 insertions, 39 deletions

diff --git a/drivers/net/ethernet/stmicro/stmmac/common.h b/drivers/net/ethernet/stmicro/stmmac/common.h
index 7b86414006d1..8403a3e17211 100644
--- a/drivers/net/ethernet/stmicro/stmmac/common.h
+++ b/drivers/net/ethernet/stmicro/stmmac/common.h
@@ -656,6 +656,8 @@ struct mac_device_info {
 	unsigned int pcs;
 	unsigned int xlgmac;
 	unsigned int num_vlan;
+	u16 vlan_hash;
+	bool vlan_stag;
 	u32 vlan_filter[32];
 	bool vlan_fail_q_en;
 	u8 vlan_fail_q;
diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac4_core.c b/drivers/net/ethernet/stmicro/stmmac/dwmac4_core.c
index 4d07f421e7af..c7a3097472e7 100644
--- a/drivers/net/ethernet/stmicro/stmmac/dwmac4_core.c
+++ b/drivers/net/ethernet/stmicro/stmmac/dwmac4_core.c
@@ -511,6 +511,7 @@ static void dwmac4_set_filter(struct mac_device_info *hw,
 {
 	void __iomem *ioaddr = (void __iomem *)dev->base_addr;
 	int numhashregs = (hw->multicast_filter_bins >> 5);
+	struct stmmac_priv *priv = netdev_priv(dev);
 	int mcbitslog2 = hw->mcast_bits_log2;
 	unsigned int value;
 	u32 mc_filter[8];
@@ -593,10 +594,12 @@ static void dwmac4_set_filter(struct mac_device_info *hw,
 	}
 
 	/* VLAN filtering */
-	if (dev->flags & IFF_PROMISC && !hw->vlan_fail_q_en)
+	if (dev->flags & IFF_PROMISC && !hw->vlan_fail_q_en) {
 		value &= ~GMAC_PACKET_FILTER_VTFE;
-	else if (dev->features & NETIF_F_HW_VLAN_CTAG_FILTER)
+	} else if (dev->features & NETIF_F_HW_VLAN_CTAG_FILTER) {
 		value |= GMAC_PACKET_FILTER_VTFE;
+		stmmac_restore_hw_vlan_rx_fltr(priv, dev, hw);
+	}
 
 	writel(value, ioaddr + GMAC_PACKET_FILTER);
 }
diff --git a/drivers/net/ethernet/stmicro/stmmac/dwxgmac2_core.c b/drivers/net/ethernet/stmicro/stmmac/dwxgmac2_core.c
index 7a7c3c91fc52..9cd58b0d8306 100644
--- a/drivers/net/ethernet/stmicro/stmmac/dwxgmac2_core.c
+++ b/drivers/net/ethernet/stmicro/stmmac/dwxgmac2_core.c
@@ -572,6 +572,7 @@ static void dwxgmac2_set_filter(struct mac_device_info *hw,
 				struct net_device *dev)
 {
 	void __iomem *ioaddr = (void __iomem *)dev->base_addr;
+	struct stmmac_priv *priv = netdev_priv(dev);
 	int mcbitslog2 = hw->mcast_bits_log2;
 	u32 value, mc_filter[8];
 
@@ -625,10 +626,12 @@ static void dwxgmac2_set_filter(struct mac_device_info *hw,
 	}
 
 	/* VLAN filtering */
-	if (dev->flags & IFF_PROMISC)
+	if (dev->flags & IFF_PROMISC) {
 		value &= ~XGMAC_FILTER_VTFE;
-	else if (dev->features & NETIF_F_HW_VLAN_CTAG_FILTER)
+	} else if (dev->features & NETIF_F_HW_VLAN_CTAG_FILTER) {
 		value |= XGMAC_FILTER_VTFE;
+		stmmac_restore_hw_vlan_rx_fltr(priv, dev, hw);
+	}
 
 	writel(value, ioaddr + XGMAC_PACKET_FILTER);
 }
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
index d746064011ab..308800906fbc 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
@@ -160,7 +160,6 @@ static void stmmac_reset_tx_queue(struct stmmac_priv *priv, u32 queue);
 static void stmmac_reset_queues_param(struct stmmac_priv *priv);
 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_vlan_restore(struct stmmac_priv *priv);
 
 #ifdef CONFIG_DEBUG_FS
 static const struct net_device_ops stmmac_netdev_ops;
@@ -4268,8 +4267,6 @@ 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;
@@ -7225,17 +7222,6 @@ del_vlan_error:
 	return ret;
 }
 
-static void stmmac_vlan_restore(struct stmmac_priv *priv)
-{
-	if (!(priv->dev->features & NETIF_F_VLAN_FEATURES))
-		return;
-
-	if (priv->hw->num_vlan)
-		stmmac_restore_hw_vlan_rx_fltr(priv, priv->dev, priv->hw);
-
-	stmmac_vlan_update(priv, priv->num_svlans);
-}
-
 static int stmmac_bpf(struct net_device *dev, struct netdev_bpf *bpf)
 {
 	struct stmmac_priv *priv = netdev_priv(dev);
@@ -9124,8 +9110,6 @@ int stmmac_resume(struct device *dev)
 	stmmac_set_rx_mode(ndev);
 	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 293589addf2d..24246de499a6 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_vlan.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_vlan.c
@@ -51,9 +51,9 @@ static int vlan_write_filter(struct net_device *dev,
 
 	writel(val, ioaddr + VLAN_TAG);
 
-	ret = readl_poll_timeout(ioaddr + VLAN_TAG, val,
-				 !(val & VLAN_TAG_CTRL_OB),
-				 1000, 500000);
+	ret = readl_poll_timeout_atomic(ioaddr + VLAN_TAG, val,
+					!(val & VLAN_TAG_CTRL_OB),
+					1000, 500000);
 	if (ret) {
 		netdev_err(dev, "Timeout accessing MAC_VLAN_Tag_Filter\n");
 		return -EBUSY;
@@ -175,28 +175,13 @@ static int vlan_del_hw_rx_fltr(struct net_device *dev,
 	return 0;
 }
 
-static void vlan_restore_hw_rx_fltr(struct net_device *dev,
-				    struct mac_device_info *hw)
-{
-	int i;
-
-	/* Single Rx VLAN Filter */
-	if (hw->num_vlan == 1) {
-		vlan_write_single(dev, hw->vlan_filter[0]);
-		return;
-	}
-
-	/* Extended Rx VLAN Filter Enable */
-	for (i = 0; i < hw->num_vlan; i++)
-		vlan_write_filter(dev, hw, i, hw->vlan_filter[i]);
-}
-
 static void vlan_update_hash(struct mac_device_info *hw, u32 hash,
 			     bool is_svlan)
 {
 	void __iomem *ioaddr = hw->pcsr;
 	u32 value;
 
+	hw->vlan_hash = hash;
 	writel(hash, ioaddr + VLAN_HASH_TABLE);
 
 	value = readl(ioaddr + VLAN_TAG);
@@ -204,13 +189,16 @@ static void vlan_update_hash(struct mac_device_info *hw, u32 hash,
 	if (hash) {
 		value |= VLAN_VTHM | VLAN_ETV;
 		if (is_svlan) {
+			hw->vlan_stag = true;
 			value |= VLAN_ESVL;
 			value |= VLAN_DOVLTC;
 		} else {
+			hw->vlan_stag = false;
 			value &= ~VLAN_ESVL;
 			value &= ~VLAN_DOVLTC;
 		}
 	} else {
+		hw->vlan_stag = false;
 		value &= ~(VLAN_VTHM | VLAN_ETV);
 		value &= ~VLAN_ESVL;
 		value &= ~VLAN_DOVLTC;
@@ -222,6 +210,25 @@ static void vlan_update_hash(struct mac_device_info *hw, u32 hash,
 	writel(value, ioaddr + VLAN_TAG);
 }
 
+static void vlan_restore_hw_rx_fltr(struct net_device *dev,
+				    struct mac_device_info *hw)
+{
+	int i;
+
+	/* Hash-based Rx VLAN Filter */
+	vlan_update_hash(hw, hw->vlan_hash, hw->vlan_stag);
+
+	/* Single Rx VLAN Filter */
+	if (hw->num_vlan == 1) {
+		vlan_write_single(dev, hw->vlan_filter[0]);
+		return;
+	}
+
+	/* Extended Rx VLAN Filter Enable */
+	for (i = 0; i < hw->num_vlan; i++)
+		vlan_write_filter(dev, hw, i, hw->vlan_filter[i]);
+}
+
 static void vlan_enable(struct mac_device_info *hw, u32 type)
 {
 	void __iomem *ioaddr = hw->pcsr;