net: stmmac: vlan: Add VLAN S-TAG Insertion Offload support

Originally the VLAN Insertion Offload feature was configured to always
insert S-VLAN type due to the MAC_VLAN_Incl.CSVL flag being always set. So
as long as there was at least one 802.1ad virtual interface created any
other virtual VLAN-interface would have generated S-TAG'ed frames, which
was caused by having both MAC_VLAN_Incl.CSVL and MAC_VLAN_Tag.ESVL flags
set. The former flag wouldn't work without the later one - that's why at
least one 802.1ad virtual interface needed to be created to meet the
problem. That has been fixed in the commit c657f86106c8 ("net: stmmac:
vlan: Disable 802.1AD tag insertion offload") just by removing the VLAN
S-TAG Insertion support from the driver with justification that the
controller doesn't support inserting both VLAN C-TAG and S-TAG at a time.
That is only one VLAN tag type can be enabled at a time.

That's true but it doesn't mean the feature should have been just deleted.
The driver can be enabled to support only one of these tags type insertion
at a time. That can be easily achieved by having the networking core
net_device_ops::{ndo_fix_features(),ndo_set_features()} callbacks
utilized. So let's use them to make sure that only one of VLAN tags type
insertion is enabled at a time - NETIF_F_HW_VLAN_CTAG_TX or
NETIF_F_HW_VLAN_STAG_TX. The MAC_VLAN_Incl.CSVL flag will be enabled only
if the NETIF_F_HW_VLAN_STAG_TX feature is requested.

Please note if there is no 802.1ad virtual interface (basically no S-TAG
filter enabled) it won't be possible to generate S-TAG'ed frames no mater
what due to the VLAN_TAG.ESVL flag cleared. This will be fixed in another
commit sometime later.

Fixes: c657f86106c8 ("net: stmmac: vlan: Disable 802.1AD tag insertion offload")
Signed-off-by: Serge Semin <fancer.lancer@gmail.com>

3 files changed, 27 insertions, 38 deletions

diff --git a/drivers/net/ethernet/stmicro/stmmac/hwif.h b/drivers/net/ethernet/stmicro/stmmac/hwif.h
index e4b28b3a62cc..45111ec4902f 100644
--- a/drivers/net/ethernet/stmicro/stmmac/hwif.h
+++ b/drivers/net/ethernet/stmicro/stmmac/hwif.h
@@ -630,10 +630,9 @@ struct stmmac_vlan_ops {
 	/* VLAN */
 	void (*update_vlan_hash)(struct mac_device_info *hw, u32 hash,
 				 bool is_svlan);
-	void (*enable_vlan)(struct mac_device_info *hw, u32 type);
 	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);
+	void (*set_hw_vlan_mode)(struct mac_device_info *hw, bool tx_stag);
 	int (*add_hw_vlan_rx_fltr)(struct net_device *dev,
 				   struct mac_device_info *hw,
 				   __be16 proto, u16 vid);
@@ -646,8 +645,6 @@ struct stmmac_vlan_ops {
 
 #define stmmac_update_vlan_hash(__priv, __args...) \
 	stmmac_do_void_callback(__priv, vlan, update_vlan_hash, __args)
-#define stmmac_enable_vlan(__priv, __args...) \
-	stmmac_do_void_callback(__priv, vlan, enable_vlan, __args)
 #define stmmac_rx_hw_vlan(__priv, __args...) \
 	stmmac_do_void_callback(__priv, vlan, rx_hw_vlan, __args)
 #define stmmac_set_hw_vlan_mode(__priv, __args...) \
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
index 308800906fbc..0c963a4601f4 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
@@ -3873,10 +3873,6 @@ static int stmmac_hw_setup(struct net_device *dev)
 	for (chan = 0; chan < rx_cnt; chan++)
 		stmmac_enable_sph(priv, priv->ioaddr, priv->sph_active, chan);
 
-	/* VLAN Tag Insertion */
-	if (priv->dma_cap.vlins)
-		stmmac_enable_vlan(priv, priv->hw, STMMAC_VLAN_INSERT);
-
 	/* Enable TSO and/or TBS */
 	for (chan = 0; chan < tx_cnt; chan++) {
 		struct stmmac_tx_queue *tx_q = &priv->dma_conf.tx_queue[chan];
@@ -3896,7 +3892,8 @@ static int stmmac_hw_setup(struct net_device *dev)
 	stmmac_start_all_dma(priv);
 
 	phylink_rx_clk_stop_block(priv->phylink);
-	stmmac_set_hw_vlan_mode(priv, priv->hw);
+	stmmac_set_hw_vlan_mode(priv, priv->hw,
+				dev->features & NETIF_F_HW_VLAN_STAG_TX);
 	phylink_rx_clk_stop_unblock(priv->phylink);
 
 	return 0;
@@ -6469,6 +6466,12 @@ static netdev_features_t stmmac_fix_features(struct net_device *dev,
 			features |= NETIF_F_HW_VLAN_CTAG_RX;
 	}
 
+	/* VLAN type insertion is mutually exclusive: either CTAG or STAG */
+	if (features & NETIF_F_HW_VLAN_CTAG_TX)
+		features &= ~NETIF_F_HW_VLAN_STAG_TX;
+	else if (features & NETIF_F_HW_VLAN_STAG_TX)
+		features &= ~NETIF_F_HW_VLAN_CTAG_TX;
+
 	return features;
 }
 
@@ -6511,7 +6514,8 @@ static int stmmac_set_features(struct net_device *netdev,
 		priv->hw->hw_vlan_en = !!(features & NETIF_F_HW_VLAN_CTAG_RX);
 
 	phylink_rx_clk_stop_block(priv->phylink);
-	stmmac_set_hw_vlan_mode(priv, priv->hw);
+	stmmac_set_hw_vlan_mode(priv, priv->hw,
+				features & NETIF_F_HW_VLAN_STAG_TX);
 	phylink_rx_clk_stop_unblock(priv->phylink);
 
 	return 0;
@@ -8769,8 +8773,13 @@ static int __stmmac_dvr_probe(struct device *device,
 		ndev->features |= NETIF_F_HW_VLAN_CTAG_FILTER;
 		ndev->features |= NETIF_F_HW_VLAN_STAG_FILTER;
 	}
-	if (priv->dma_cap.vlins)
+
+	/* VLAN Insertion feature supports only one type at a time */
+	if (priv->dma_cap.vlins) {
 		ndev->features |= NETIF_F_HW_VLAN_CTAG_TX;
+		ndev->hw_features |= NETIF_F_HW_VLAN_CTAG_TX |
+				     NETIF_F_HW_VLAN_STAG_TX;
+	}
 #endif
 	priv->msg_enable = netif_msg_init(debug, default_msg_level);
 
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_vlan.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_vlan.c
index 24246de499a6..c7cc2ee22366 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_vlan.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_vlan.c
@@ -229,19 +229,6 @@ static void vlan_restore_hw_rx_fltr(struct net_device *dev,
 		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;
-	u32 value;
-
-	value = readl(ioaddr + VLAN_INCL);
-	value |= VLAN_VLTI;
-	value &= ~VLAN_CSVL; /* Only use CVLAN */
-	value &= ~VLAN_VLC;
-	value |= (type << VLAN_VLC_SHIFT) & VLAN_VLC;
-	writel(value, ioaddr + VLAN_INCL);
-}
-
 static void vlan_rx_hw(struct mac_device_info *hw,
 		       struct dma_desc *rx_desc, struct sk_buff *skb)
 {
@@ -252,7 +239,7 @@ static void vlan_rx_hw(struct mac_device_info *hw,
 	}
 }
 
-static void vlan_set_hw_mode(struct mac_device_info *hw)
+static void vlan_set_hw_mode(struct mac_device_info *hw, bool tx_stag)
 {
 	void __iomem *ioaddr = hw->pcsr;
 	u32 value = readl(ioaddr + VLAN_TAG);
@@ -272,22 +259,19 @@ static void vlan_set_hw_mode(struct mac_device_info *hw)
 	/* Enable outer VLAN Tag in Rx DMA descriptor */
 	value |= VLAN_TAG_CTRL_EVLRXS;
 	writel(value, ioaddr + VLAN_TAG);
-}
 
-static void dwxlgmac2_vlan_set_hw_mode(struct mac_device_info *hw)
-{
-	void __iomem *ioaddr = hw->pcsr;
-	u32 value = readl(ioaddr + VLAN_TAG);
-
-	/* Activate Double VLAN for Rx COE */
-	value |= VLAN_EDVLP;
-
-	writel(value, ioaddr + VLAN_TAG);
+	/* Set Tx VLAN Insertion feature (might be unavailable) */
+	value = readl(ioaddr + VLAN_INCL);
+	value |= VLAN_VLTI;
+	if (tx_stag)
+		value |= VLAN_CSVL;
+	else
+		value &= ~VLAN_CSVL;
+	writel(value, ioaddr + VLAN_INCL);
 }
 
 const struct stmmac_vlan_ops dwmac4_vlan_ops = {
 	.update_vlan_hash = vlan_update_hash,
-	.enable_vlan = vlan_enable,
 	.add_hw_vlan_rx_fltr = vlan_add_hw_rx_fltr,
 	.del_hw_vlan_rx_fltr = vlan_del_hw_rx_fltr,
 	.restore_hw_vlan_rx_fltr = vlan_restore_hw_rx_fltr,
@@ -297,8 +281,7 @@ const struct stmmac_vlan_ops dwmac4_vlan_ops = {
 
 const struct stmmac_vlan_ops dwxlgmac2_vlan_ops = {
 	.update_vlan_hash = vlan_update_hash,
-	.set_hw_vlan_mode = dwxlgmac2_vlan_set_hw_mode,
-	.enable_vlan = vlan_enable,
+	.set_hw_vlan_mode = vlan_set_hw_mode,
 };
 
 u32 stmmac_get_num_vlan(void __iomem *ioaddr)