HSD #15012455743: net: stmmac: Add Double VLAN handling for VLAN Rx filtering

Add double VLAN handling for VLAN Rx Filtering.

Signed-off-by: Boon Khai Ng <boon.khai.ng@intel.com>

Author: ngboonkhai

drivers/net/ethernet/stmicro/stmmac/common.h

@@ -534,6 +534,7 @@ struct mac_device_info {
 	unsigned int promisc;
 	bool vlan_fail_q_en;
 	u8 vlan_fail_q;
+	unsigned int double_vlan;
 };
 
 struct stmmac_rx_routing {

drivers/net/ethernet/stmicro/stmmac/dwxgmac2.h

@@ -499,6 +499,9 @@
 #define XGMAC_VLAN_TAG_STRIP_ALL	(0x3 << XGMAC_VLAN_TAG_CTRL_EVLS_SHIFT)
 
 #define XGMAC_VLAN_TAG_DATA		0x00000054
+#define XGMAC_VLAN_TAG_DATA_ERIVLT	BIT(20)
+#define XGMAC_VLAN_TAG_DATA_ERSVLM	BIT(19)
+#define XGMAC_VLAN_TAG_DATA_DOVLTC	BIT(18)
 #define XGMAC_VLAN_TAG_DATA_ETV		BIT(17)
 #define XGMAC_VLAN_TAG_DATA_VEN		BIT(16)
 #define XGMAC_VLAN_TAG_DATA_VID		GENMASK(15, 0)

drivers/net/ethernet/stmicro/stmmac/dwxgmac2_core.c

@@ -438,6 +438,15 @@ static int dwxgmac2_write_vlan_filter(struct net_device *dev,
 	if (index >= hw->num_vlan)
 		return -EINVAL;
 
+	if (hw->double_vlan) {
+		data |= XGMAC_VLAN_TAG_DATA_DOVLTC;
+		data |= XGMAC_VLAN_TAG_DATA_ERIVLT;
+		data &= ~XGMAC_VLAN_TAG_DATA_ERSVLM;
+	} else {
+		data &= ~XGMAC_VLAN_TAG_DATA_DOVLTC;
+		data &= ~XGMAC_VLAN_TAG_DATA_ERIVLT;
+	}
+
 	writel(data, ioaddr + XGMAC_VLAN_TAG_DATA);
 
 	val = readl(ioaddr + XGMAC_VLAN_TAG);
@@ -1855,6 +1864,13 @@ static u32 dwxgmac2_get_num_vlan(void __iomem *ioaddr)
 	return num_vlan;
 }
 
+static u32 dwxgmac2_is_double_vlan(void __iomem *ioaddr)
+{
+	u32 val;
+	val = readl(ioaddr + XGMAC_HW_FEATURE3);
+	return (val & XGMAC_HWFEAT_DVLAN);
+}
+
 int dwxgmac2_setup(struct stmmac_priv *priv)
 {
 	struct mac_device_info *mac = priv->hw;
@@ -1889,6 +1905,7 @@ int dwxgmac2_setup(struct stmmac_priv *priv)
 	mac->mii.clk_csr_shift = 19;
 	mac->mii.clk_csr_mask = GENMASK(21, 19);
 	mac->num_vlan = dwxgmac2_get_num_vlan(priv->ioaddr);
+	mac->double_vlan = dwxgmac2_is_double_vlan(priv->ioaddr);
 	return 0;
 }