net: stmmac: vlan: Improve hash and perfect VLAN filters security

By default all DW GMAC v3.70, DW QoS Ether and DW XGMAC IP-cores are
equipped with a single perfect VLAN-tag filter tunable to filter C- and/or
S-Tags. Optionally a hash-based VLAN-filter can be synthesized into the
IP-cores. But both the basic perfect filter and the hash-based filter are
configured by using the same CSR - MAC_VLAN_Tag. So if S-VLAN type tags
filtering enabled, it will be applicable for both basic and hash-based
filters. From that perspective it would be better to synchronize both
types of the filters utilization.

Moreover currently the filters configuration is someway insecure. If at
least one S-VLAN tags is added to the hash-based filter, then the VLAN
type checking will be fully disabled. Thus both C- and S-VLAN tags would
be permitted even if S-VLAN tags filtering is only requested. In addition
to that the hash-based filter implementation keeps track a united list of
the C- and S- tags. So if a tag of both of these tags added and some of
the them is removed, then both of the tags filtering will be disabled.
Even though it seems unlikely to have both C- and S-VLANs on the same
wire, it will be still inappropriate behaviour.

So let's fix all the misbehaviours above by improving the basic perfect
and hash-based VLAN-filters configuration procedure. First the
added/removed C- and S-Tags must be kept tracked in order to properly
setup the filters. Second the hash-based filter configuration method must
be altered to take these counters state into account. Third both the
filters re-configuration must be done synchronously in case if any basic
perfect or hash-based state is updated. Finally the core driver VLANs
bitmap must be extended to preserve S-Tags too so not to disable the tags
pair on one of the tags removal. Thus this shall provide the most optimal
and secure basic and hash-based filters utilization.

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

6 files changed, 84 insertions, 62 deletions

diff --git a/drivers/net/ethernet/stmicro/stmmac/common.h b/drivers/net/ethernet/stmicro/stmmac/common.h
index dff7a12047a2..4a811516474b 100644
--- a/drivers/net/ethernet/stmicro/stmmac/common.h
+++ b/drivers/net/ethernet/stmicro/stmmac/common.h
@@ -657,7 +657,8 @@ struct mac_device_info {
 	unsigned int xlgmac;
 	unsigned int num_vlan;
 	u16 vlan_hash;
-	bool vlan_stag;
+	int vlan_ctags;
+	int vlan_stags;
 	u32 vlan_filter[32];
 	bool vlan_fail_q_en;
 	u8 vlan_fail_q;
diff --git a/drivers/net/ethernet/stmicro/stmmac/hwif.h b/drivers/net/ethernet/stmicro/stmmac/hwif.h
index a46e819aec29..b8fd4b527393 100644
--- a/drivers/net/ethernet/stmicro/stmmac/hwif.h
+++ b/drivers/net/ethernet/stmicro/stmmac/hwif.h
@@ -629,7 +629,7 @@ struct stmmac_est_ops {
 struct stmmac_vlan_ops {
 	/* VLAN */
 	void (*update_vlan_hash)(struct mac_device_info *hw, u32 hash,
-				 bool is_svlan);
+				 int add_ctags, int add_stags);
 	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,
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac.h b/drivers/net/ethernet/stmicro/stmmac/stmmac.h
index 7ac58e01d846..2053f62c67f2 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac.h
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac.h
@@ -375,8 +375,10 @@ struct stmmac_priv {
 	void __iomem *mmcaddr;
 	void __iomem *ptpaddr;
 	void __iomem *estaddr;
-	unsigned long active_vlans[BITS_TO_LONGS(VLAN_N_VID)];
-	unsigned int num_svlans;
+
+	DECLARE_BITMAP(active_cvlans, VLAN_N_VID);
+	DECLARE_BITMAP(active_svlans, VLAN_N_VID);
+
 	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 9dad81d3ffea..d2006d90e7e9 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
@@ -7144,18 +7144,56 @@ static u32 stmmac_vid_crc32_le(__le16 vid_le)
 	return crc;
 }
 
-static int stmmac_vlan_update(struct stmmac_priv *priv, bool is_svlan)
+static int stmmac_update_hw_vlan_rx_hash(struct stmmac_priv *priv, bool add,
+					 __be16 proto, u16 vid)
 {
+	int add_ctags = 0, add_stags = 0;
+	unsigned long *active_vlans;
 	u32 crc, hash = 0;
-	u16 vid = 0;
+	u16 vid_cpu;
+	int ret;
+
+	if (proto == htons(ETH_P_8021Q)) {
+		active_vlans = priv->active_cvlans;
+		add_ctags = add ? 1 : -1;
+	} else {
+		active_vlans = priv->active_svlans;
+		add_stags = add ? 1 : -1;
+	}
+
+	if (add)
+		ret = test_and_set_bit(vid, active_vlans);
+	else
+		ret = test_and_clear_bit(vid, active_vlans);
+
+	if (ret == add)
+		return 0;
 
-	for_each_set_bit(vid, priv->active_vlans, VLAN_N_VID) {
+	for_each_set_bit(vid, priv->active_cvlans, VLAN_N_VID) {
 		__le16 vid_le = cpu_to_le16(vid);
 		crc = bitrev32(~stmmac_vid_crc32_le(vid_le)) >> 28;
 		hash |= (1 << crc);
 	}
 
-	return stmmac_update_vlan_hash(priv, priv->hw, hash, is_svlan);
+	for_each_set_bit(vid_cpu, priv->active_svlans, VLAN_N_VID) {
+		__le16 vid_le = cpu_to_le16(vid_cpu);
+		crc = bitrev32(~stmmac_vid_crc32_le(vid_le)) >> 28;
+		hash |= (1 << crc);
+        }
+
+	ret = stmmac_update_vlan_hash(priv, priv->hw, hash, add_ctags, add_stags);
+	if (ret)
+		goto err_revert_bit;
+
+	return 0;
+
+err_revert_bit:
+	if (add)
+		clear_bit(vid, active_vlans);
+	else
+		set_bit(vid, active_vlans);
+
+	return ret;
 }
 
 /* FIXME: This may need RXC to be running, but it may be called with BH
@@ -7164,33 +7202,19 @@ static int stmmac_vlan_update(struct stmmac_priv *priv, bool is_svlan)
 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_svlans;
-	bool is_svlan = false;
 	int ret;
 
 	ret = pm_runtime_resume_and_get(priv->device);
 	if (ret < 0)
 		return ret;
 
-	if (be16_to_cpu(proto) == ETH_P_8021AD)
-		is_svlan = true;
-
-	set_bit(vid, priv->active_vlans);
-	num_svlans = priv->num_svlans + is_svlan;
-	ret = stmmac_vlan_update(priv, num_svlans);
-	if (ret) {
-		clear_bit(vid, priv->active_vlans);
+	ret = stmmac_update_hw_vlan_rx_hash(priv, true, proto, vid);
+	if (ret)
 		goto err_pm_put;
-	}
 
 	ret = stmmac_add_hw_vlan_rx_fltr(priv, ndev, priv->hw, proto, vid);
-	if (ret && ret != -EINVAL) {
-		clear_bit(vid, priv->active_vlans);
-		stmmac_vlan_update(priv, priv->num_svlans);
-		goto err_pm_put;
-	}
-
-	priv->num_svlans = num_svlans;
+	if (ret && ret != -EINVAL)
+		stmmac_update_hw_vlan_rx_hash(priv, false, proto, vid);
 
 err_pm_put:
 	pm_runtime_put(priv->device);
@@ -7204,33 +7228,19 @@ 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_svlans;
-	bool is_svlan = false;
 	int ret;
 
 	ret = pm_runtime_resume_and_get(priv->device);
 	if (ret < 0)
 		return ret;
 
-	if (be16_to_cpu(proto) == ETH_P_8021AD)
-		is_svlan = true;
-
-	clear_bit(vid, priv->active_vlans);
-	num_svlans = priv->num_svlans - is_double;
-	ret = stmmac_vlan_update(priv, num_svlans);
-	if (ret) {
-		set_bit(vid, priv->active_vlans);
-		goto del_vlan_error;
-	}
-
 	ret = stmmac_del_hw_vlan_rx_fltr(priv, ndev, priv->hw, proto, vid);
-	if (ret && ret != -EINVAL) {
-		set_bit(vid, priv->active_vlans);
-		stmmac_vlan_update(priv, priv->num_svlans);
+	if (ret && ret != -EINVAL)
 		goto del_vlan_error;
-	}
 
-	priv->num_svlans = num_svlans;
+	ret = stmmac_update_hw_vlan_rx_hash(priv, false, proto, vid);
+	if (ret)
+		stmmac_add_hw_vlan_rx_fltr(priv, ndev, priv->hw, proto, vid);
 
 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 64270920de91..b6d4b2c7a2d3 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_vlan.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_vlan.c
@@ -7,17 +7,19 @@
 #include "stmmac.h"
 #include "stmmac_vlan.h"
 
+static void vlan_update_hash(struct mac_device_info *hw, u32 hash,
+			     int add_ctags, int add_stags);
+
 static void vlan_write_single(struct net_device *dev, u32 data)
 {
 	void __iomem *ioaddr = (void __iomem *)dev->base_addr;
-	u32 val, mask;
+	u32 val;
 
-	mask = VLAN_TAG_ERSVLM | VLAN_TAG_VID;
 	if (!(data & VLAN_TAG_VID))
 		data |= VLAN_TAG_VID;
 
-	val = readl(ioaddr + VLAN_TAG) & ~mask;
-	val |= VLAN_TAG_ETV | (data & mask);
+	val = readl(ioaddr + VLAN_TAG) & ~VLAN_TAG_VID;
+	val |= VLAN_TAG_ETV | (data & VLAN_TAG_VID);
 
 	writel(val, ioaddr + VLAN_TAG);
 }
@@ -96,6 +98,8 @@ static int vlan_add_hw_rx_fltr(struct net_device *dev,
 			return -EPERM;
 		}
 
+		vlan_update_hash(hw, hw->vlan_hash, !is_stag, is_stag);
+
 		vlan_write_single(dev, val);
 
 		hw->vlan_filter[0] = val;
@@ -147,6 +151,8 @@ static int vlan_del_hw_rx_fltr(struct net_device *dev,
 		if (is_stag != !!(hw->vlan_filter[0] & VLAN_TAG_ERSVLM))
 			return 0;
 
+		vlan_update_hash(hw, hw->vlan_hash, -!is_stag, -is_stag);
+
 		vlan_write_single(dev, 0);
 
 		hw->vlan_filter[0] = 0;
@@ -176,7 +182,7 @@ static int vlan_del_hw_rx_fltr(struct net_device *dev,
 }
 
 static void vlan_update_hash(struct mac_device_info *hw, u32 hash,
-			     bool is_svlan)
+			     int add_ctags, int add_stags)
 {
 	void __iomem *ioaddr = hw->pcsr;
 	u32 value;
@@ -184,21 +190,23 @@ static void vlan_update_hash(struct mac_device_info *hw, u32 hash,
 	hw->vlan_hash = hash;
 	writel(hash, ioaddr + VLAN_HASH_TABLE);
 
-	value = readl(ioaddr + VLAN_TAG);
+	hw->vlan_ctags += add_ctags;
+	hw->vlan_stags += add_stags;
+	value = readl(ioaddr + VLAN_TAG) | VLAN_ETV;
 
-	if (hash) {
-		value |= VLAN_VTHM | VLAN_ETV;
-		if (is_svlan) {
-			hw->vlan_stag = true;
-			value |= VLAN_DOVLTC;
-		} else {
-			hw->vlan_stag = false;
-			value &= ~VLAN_DOVLTC;
-		}
+	if (hash)
+		value |= VLAN_VTHM;
+	else
+		value &= ~VLAN_VTHM;
+
+	if (hw->vlan_ctags && hw->vlan_stags) {
+		value |= VLAN_DOVLTC;
+	} else if (hw->vlan_stags) {
+		value &= ~VLAN_DOVLTC;
+		value |= VLAN_ERSVLM;
 	} else {
-		hw->vlan_stag = false;
-		value &= ~(VLAN_VTHM | VLAN_ETV);
 		value &= ~VLAN_DOVLTC;
+		value &= ~VLAN_ERSVLM;
 	}
 
 	if (!hw->vlan_filter[0])
@@ -213,7 +221,7 @@ static void vlan_restore_hw_rx_fltr(struct net_device *dev,
 	int i;
 
 	/* Hash-based Rx VLAN Filter */
-	vlan_update_hash(hw, hw->vlan_hash, hw->vlan_stag);
+	vlan_update_hash(hw, hw->vlan_hash, 0, 0);
 
 	/* Single Rx VLAN Filter */
 	if (hw->num_vlan == 1) {
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_vlan.h b/drivers/net/ethernet/stmicro/stmmac/stmmac_vlan.h
index 3ee0022282f1..e38f570e3297 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_vlan.h
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_vlan.h
@@ -19,6 +19,7 @@
 #define VLAN_EDVLP			BIT(26)
 #define VLAN_VTHM			BIT(25)
 #define VLAN_DOVLTC			BIT(20)
+#define VLAN_ERSVLM			BIT(19)
 #define VLAN_ESVL			BIT(18)
 #define VLAN_ETV			BIT(16)
 #define VLAN_VID			GENMASK(15, 0)