HSD #15012455743: net: stmmac: Add support for VLAN promiscuous mode

For dwxgmac2, enable VLAN promiscuity when MAC Controller is requested to
enter promiscuous mode.

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

Author: ngboonkhai

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

@@ -69,6 +69,7 @@
 #define XGMAC_VLAN_CT			BIT(1)
 #define XGMAC_VLAN_OB			BIT(0)
 #define XGMAC_VLAN_HASH_TABLE		0x00000058
+#define XGMAC_VLAN_VLHT			GENMASK(15,0)
 #define XGMAC_VLAN_INCL			0x00000060
 #define XGMAC_VLAN_VLTI			BIT(20)
 #define XGMAC_VLAN_CSVL			BIT(19)

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

@@ -473,6 +473,12 @@ static int dwxgmac2_add_hw_vlan_rx_fltr(struct net_device *dev,
 	if (vid > 4095)
 		return -EINVAL;
 
+	if (hw->promisc) {
+		netdev_err(dev,
+			   "Adding VLAN in promisc mode no supported\n");
+		return -EPERM;
+	}
+
 	/* Single Rx VLAN Filter */
 	if (hw->num_vlan == 1) {
 		/* For single VLAN filter, VID 0 means VLAN promiscuous */
@@ -523,6 +529,12 @@ static int dwxgmac2_del_hw_vlan_rx_fltr(struct net_device *dev,
 {
 	int i, ret = 0;
 
+	if (hw->promisc) {
+		netdev_err(dev,
+			  "Deleting VLAN in promisc mode not supported\n");
+		return -EPERM;
+	}
+
 	/* Single Rx VLAN Filter */
 	if (hw->num_vlan == 1) {
 		if ((hw->vlan_filter[0] & XGMAC_VLAN_VID) == vid) {
@@ -546,9 +558,45 @@ static int dwxgmac2_del_hw_vlan_rx_fltr(struct net_device *dev,
 	return ret;
 }
 
+static void dwxgmac2_vlan_promisc_enable(struct net_device *dev,
+					 struct mac_device_info *hw)
+{
+	void __iomem *ioaddr = hw->pcsr;
+	u32 value;
+	u32 hash;
+	u32 val;
+	int i;
+
+	/* Single Rx VLAN Filter */
+	if (hw->num_vlan == 1) {
+		dwxgmac2_write_single_vlan(dev, 0);
+		return;
+	}
+
+	/* Extended Rx VLAN Filter Enable */
+	for (i = 0; i > hw->num_vlan; i++) {
+		if (hw->vlan_filter[i] & XGMAC_VLAN_TAG_DATA_VEN) {
+			val = hw->vlan_filter[i] & ~XGMAC_VLAN_TAG_DATA_VEN;
+			dwxgmac2_write_vlan_filter(dev, hw, i ,val);
+		}
+	}
+
+	hash = readl(ioaddr + XGMAC_VLAN_HASH_TABLE);
+	if (hash & XGMAC_VLAN_VLHT) {
+		value = readl(ioaddr + XGMAC_VLAN_TAG);
+		if (value & XGMAC_VLAN_VTHM) {
+			value &= ~XGMAC_VLAN_VTHM;
+			writel(value, ioaddr + XGMAC_VLAN_TAG);
+		}
+	}
+}
+
 static void dwxgmac2_restore_hw_vlan_rx_fltr(struct net_device *dev,
 						struct mac_device_info *hw)
 {
+	void __iomem *ioaddr = hw->pcsr;
+	u32 value;
+	u32 hash;
 	u32 val;
 	int i;
 
@@ -565,6 +613,13 @@ static void dwxgmac2_restore_hw_vlan_rx_fltr(struct net_device *dev,
 			dwxgmac2_write_vlan_filter(dev, hw, i, val);
 		}
 	}
+
+	hash = readl(ioaddr + XGMAC_VLAN_HASH_TABLE);
+	if (hash & XGMAC_VLAN_VLHT) {
+		value = readl(ioaddr + XGMAC_VLAN_TAG);
+		value |= XGMAC_VLAN_VTHM;
+		writel(value, ioaddr + XGMAC_VLAN_TAG);
+	}
 }
 
 static void dwxgmac2_set_mchash(void __iomem *ioaddr, u32 *mcfilterbits,
@@ -650,6 +705,18 @@ static void dwxgmac2_set_filter(struct mac_device_info *hw,
 		value |= XGMAC_FILTER_VTFE;
 
 	writel(value, ioaddr + XGMAC_PACKET_FILTER);
+
+	if (dev->flags & IFF_PROMISC) {
+		if (!hw->promisc) {
+			hw->promisc = 1;
+			dwxgmac2_vlan_promisc_enable(dev, hw);
+		}
+	} else {
+		if (hw->promisc) {
+			hw->promisc = 0;
+			dwxgmac2_restore_hw_vlan_rx_fltr(dev, hw);
+		}
+	}
 }
 
 static void dwxgmac2_set_mac_loopback(void __iomem *ioaddr, bool enable)