net: stmmac: vlan: Enable S-VLAN feature on device open

The MAC_VLAN_Tag.ESVL flag is responsible for the S-VLAN feature
activation in the MAC. Here is the bit description:

> When this bit is set, the MAC transmitter and receiver consider the
> S-VLAN packets (Type = 0x88A8) as valid VLAN tagged packets.

From that perspective the flag must be set if S-TAG is supposed to be met
in the incoming or outcoming traffic. Thus it must be set if the
HW-offloaded S-VLAN is required on any path. But currently it is set on
the Rx S-VLAN filter activation only. It's not that a big problem, since
normally the S-TAG-ed frames are expected in both directions. Thus if the
S-TAG filter feature isn't enabled no S-TAGed outbound frames will be
generated. But from the scalability and maintainability perspectives it's
better to set the flag in a centralized way if any of the HW-offloaded
S-VLAN feature is requested. Let's do that by moving the bit handling to
the stmmac_set_hw_vlan_mode() method.

Signed-off-by: Serge Semin <fancer.lancer@gmail.com>

3 files changed, 10 insertions, 5 deletions

diff --git a/drivers/net/ethernet/stmicro/stmmac/hwif.h b/drivers/net/ethernet/stmicro/stmmac/hwif.h
index 6996fe2e1336..a46e819aec29 100644
--- a/drivers/net/ethernet/stmicro/stmmac/hwif.h
+++ b/drivers/net/ethernet/stmicro/stmmac/hwif.h
@@ -633,7 +633,7 @@ struct stmmac_vlan_ops {
 	void (*rx_hw_vlan)(struct mac_device_info *hw, struct dma_desc *rx_desc,
 			   struct sk_buff *skb);
 	void (*set_hw_vlan_mode)(struct mac_device_info *hw, bool rx_strip,
-				 bool tx_stag);
+				 bool rx_stag, bool tx_stag);
 	int (*add_hw_vlan_rx_fltr)(struct net_device *dev,
 				   struct mac_device_info *hw,
 				   __be16 proto, u16 vid);
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
index 47de06bbde2f..1a6d2f6335b5 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
@@ -3895,6 +3895,7 @@ static int stmmac_hw_setup(struct net_device *dev)
 	stmmac_set_hw_vlan_mode(priv, priv->hw,
 				dev->features & (NETIF_F_HW_VLAN_CTAG_RX |
 						 NETIF_F_HW_VLAN_STAG_RX),
+				dev->features & NETIF_F_HW_VLAN_STAG_FILTER,
 				dev->features & NETIF_F_HW_VLAN_STAG_TX);
 	phylink_rx_clk_stop_unblock(priv->phylink);
 
@@ -6521,6 +6522,7 @@ static int stmmac_set_features(struct net_device *netdev,
 	stmmac_set_hw_vlan_mode(priv, priv->hw,
 				features & (NETIF_F_HW_VLAN_CTAG_RX |
 					    NETIF_F_HW_VLAN_STAG_RX),
+				features & NETIF_F_HW_VLAN_STAG_FILTER,
 				features & NETIF_F_HW_VLAN_STAG_TX);
 	phylink_rx_clk_stop_unblock(priv->phylink);
 
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_vlan.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_vlan.c
index bb0521978453..64270920de91 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_vlan.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_vlan.c
@@ -190,17 +190,14 @@ static void vlan_update_hash(struct mac_device_info *hw, u32 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;
 	}
 
@@ -240,7 +237,7 @@ static void vlan_rx_hw(struct mac_device_info *hw,
 }
 
 static void vlan_set_hw_mode(struct mac_device_info *hw, bool rx_strip,
-			     bool tx_stag)
+			     bool rx_stag, bool tx_stag)
 {
 	void __iomem *ioaddr = hw->pcsr;
 	u32 value = readl(ioaddr + VLAN_TAG);
@@ -254,6 +251,12 @@ static void vlan_set_hw_mode(struct mac_device_info *hw, bool rx_strip,
 		/* Do not strip VLAN on Receive */
 		value |= VLAN_TAG_STRIP_NONE;
 
+	/* Activate S-VLAN feature on MAC Tx and Rx */
+	if (rx_strip || rx_stag || tx_stag)
+		value |= VLAN_ESVL;
+	else
+		value &= ~VLAN_ESVL;
+
 	/* Activate Double VLAN for Rx COE */
 	value |= VLAN_EDVLP;