net: stmmac: vlan: Add VLAN S-TAG perfect filtering

The blamed commit added the Extended VLAN Tag Perfect filtering support to
the DW Ether QoS part of the driver. Then the implementation has been
reused for DW XGMACs in commit 534df0c1724b ("net: stmmac: dwxgmac2: Add
support for HW-accelerated VLAN stripping").

Originally it was intended for the VLAN TAG perfect filters to be used
orthogonally with the VLAN Tag Hash filtering. So one functionality
would work irrespective to another one being available. Well, with some
flaws but it has worked. The problem comes when you get to have a device
with no VLAN Tag Hash filter but with the Extended VLAN Tag filter
support. In such case by default the S-TAG filter won't be activated
causing C-TAG frames filtering instead, because no ERSVLM or DOVLTC flag
set in the MAC_VLAN_Tag_Filter register. These flags are responsible for
VLAN S-TAG tag activating or disabling VLAN tag type checking. Anyway in
case if the VLAN Tag Hash filtering is supported it will be _responsible_
for permitting both C- and S-Tags of the same VID, which at least isn't
secure. But the VLAN perfect filter S-TAG entries will be misconfigured.

Let's fix the problem denoted above by setting up the ERSVLM flag in case
if the 802.1ad VLAN-protocol requested. The flag will be preserved in the
private-data VIDs cache so to be properly restored after system resume and
to differentiate the tags with the same VIDs but different types.

Fixes: ed64639bc1e0 ("net: stmmac: Add support for VLAN Rx filtering")
Signed-off-by: Serge Semin <fancer.lancer@gmail.com>

2 files changed, 49 insertions, 21 deletions

diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_vlan.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_vlan.c
index 59469805edc2..1550843446a7 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_vlan.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_vlan.c
@@ -7,14 +7,15 @@
 #include "stmmac.h"
 #include "stmmac_vlan.h"
 
-static void vlan_write_single(struct net_device *dev, u16 vid)
+static void vlan_write_single(struct net_device *dev, u32 data)
 {
 	void __iomem *ioaddr = (void __iomem *)dev->base_addr;
-	u32 val;
+	u32 val, mask;
 
-	val = readl(ioaddr + VLAN_TAG);
-	val &= ~VLAN_TAG_VID;
-	val |= VLAN_TAG_ETV | vid;
+	mask = VLAN_TAG_ERSVLM | VLAN_TAG_VID;
+
+	val = readl(ioaddr + VLAN_TAG) & ~mask;
+	val |= VLAN_TAG_ETV | (data & mask);
 
 	writel(val, ioaddr + VLAN_TAG);
 }
@@ -56,12 +57,15 @@ static int vlan_add_hw_rx_fltr(struct net_device *dev,
 			       __be16 proto, u16 vid)
 {
 	int index = -1;
+	bool is_stag;
 	u32 val = 0;
 	int i, ret;
 
 	if (vid > 4095)
 		return -EINVAL;
 
+	is_stag = proto == htons(ETH_P_8021AD);
+
 	/* Single Rx VLAN Filter */
 	if (hw->num_vlan == 1) {
 		/* For single VLAN filter, VID 0 means VLAN promiscuous */
@@ -70,21 +74,30 @@ static int vlan_add_hw_rx_fltr(struct net_device *dev,
 			return -EPERM;
 		}
 
+		val = vid;
+		if (is_stag)
+			val |= VLAN_TAG_ERSVLM;
+
+		if (hw->vlan_filter[0] == val)
+			return 0;
+
 		if (hw->vlan_filter[0] & VLAN_TAG_VID) {
 			netdev_err(dev, "Only single VLAN ID supported\n");
 			return -EPERM;
 		}
 
-		hw->vlan_filter[0] = vid;
-
 		if (netif_running(dev))
-			vlan_write_single(dev, vid);
+			vlan_write_single(dev, val);
+
+		hw->vlan_filter[0] = val;
 
 		return 0;
 	}
 
 	/* Extended Rx VLAN Filter Enable */
 	val |= VLAN_TAG_DATA_ETV | VLAN_TAG_DATA_VEN | vid;
+	if (is_stag)
+		val |= VLAN_TAG_DATA_ERSVLM;
 
 	for (i = 0; i < hw->num_vlan; i++) {
 		if (hw->vlan_filter[i] == val)
@@ -115,31 +128,44 @@ static int vlan_del_hw_rx_fltr(struct net_device *dev,
 			       __be16 proto, u16 vid)
 {
 	int i, ret = 0;
+	bool is_stag;
+
+	is_stag = proto == htons(ETH_P_8021AD);
 
 	/* Single Rx VLAN Filter */
 	if (hw->num_vlan == 1) {
-		if ((hw->vlan_filter[0] & VLAN_TAG_VID) == vid) {
-			hw->vlan_filter[0] = 0;
+		if ((hw->vlan_filter[0] & VLAN_TAG_VID) != vid)
+			return 0;
+
+		if (is_stag != !!(hw->vlan_filter[0] & VLAN_TAG_ERSVLM))
+			return 0;
+
+		if (netif_running(dev))
+			vlan_write_single(dev, 0);
+
+		hw->vlan_filter[0] = 0;
 
-			if (netif_running(dev))
-				vlan_write_single(dev, 0);
-		}
 		return 0;
 	}
 
 	/* Extended Rx VLAN Filter Enable */
 	for (i = 0; i < hw->num_vlan; i++) {
-		if ((hw->vlan_filter[i] & VLAN_TAG_DATA_VEN) &&
-		    ((hw->vlan_filter[i] & VLAN_TAG_DATA_VID) == vid)) {
+		if (!(hw->vlan_filter[i] & VLAN_TAG_DATA_VEN))
+			continue;
 
-			if (netif_running(dev)) {
-				ret = vlan_write_filter(dev, hw, i, 0);
-				if (ret)
-					return ret;
-			}
+		if ((hw->vlan_filter[i] & VLAN_TAG_DATA_VID) != vid)
+			continue;
 
-			hw->vlan_filter[i] = 0;
+		if (is_stag != !!(hw->vlan_filter[i] & VLAN_TAG_DATA_ERSVLM))
+			continue;
+
+		if (netif_running(dev)) {
+			ret = vlan_write_filter(dev, hw, i, 0);
+			if (ret)
+				return ret;
 		}
+
+		hw->vlan_filter[i] = 0;
 	}
 
 	return 0;
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_vlan.h b/drivers/net/ethernet/stmicro/stmmac/stmmac_vlan.h
index 514f28f246d9..5143c0fbda26 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_vlan.h
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_vlan.h
@@ -30,6 +30,7 @@
 /* MAC VLAN Tag */
 #define VLAN_TAG_VID			GENMASK(15, 0)
 #define VLAN_TAG_ETV			BIT(16)
+#define VLAN_TAG_ERSVLM			BIT(19)
 
 /* MAC VLAN Tag Control */
 #define VLAN_TAG_CTRL_OB		BIT(0)
@@ -49,6 +50,7 @@
 #define VLAN_TAG_DATA_VID		GENMASK(15, 0)
 #define VLAN_TAG_DATA_VEN		BIT(16)
 #define VLAN_TAG_DATA_ETV		BIT(17)
+#define VLAN_TAG_DATA_ERSVLM		BIT(19)
 
 /* MAC VLAN HW FEAT */
 #define HW_FEATURE3			0x00000128