HSD #15012771709: net: stmmac: Add option for VLAN filter fail queue enable

This option allows packet that fail VLAN filter to be routed
to specific Rx queue when receive all is also set.

When this option is enabled:
- Enable VFFQ only when entering promiscuous mode, because receive all
  will pass up all rx packets that failed address filtering (similar to
  promiscuous mode).
- VLAN-promiscuous mode is never entered to allow rx packet to fail VLAN
  filters and get routed to selected VFFQ Rx queue.

Signed-off-by: Boon Khai Ng <[email protected]>

Author: ngboonkhai

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

@@ -85,6 +85,11 @@
 #define XGMAC_RXQ_CTRL3			0x000000ac
 #define XGMAC_PSRQ(x)			GENMASK((x) * 8 + 7, (x) * 8)
 #define XGMAC_PSRQ_SHIFT(x)		((x) * 8)
+#define XGMAC_RXQ_CTRL4			0x00000094
+/* VFFQ mask might vary and depending on how many RX Q enabled */
+#define XGMAC_RXQCTRL_VFFQ_MASK		GENMASK(19, 17);
+#define XGMAC_RXQCTRL_VFFQ_SHIFT	17
+#define XGMAC_RXQCTRL_VFFQE		BIT(16)
 #define XGMAC_INT_STATUS		0x000000b0
 #define XGMAC_LPIIS			BIT(5)
 #define XGMAC_PMTIS			BIT(4)

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

@@ -659,6 +659,7 @@ static void dwxgmac2_set_filter(struct mac_device_info *hw,
 {
 	void __iomem *ioaddr = (void __iomem *)dev->base_addr;
 	u32 value = readl(ioaddr + XGMAC_PACKET_FILTER);
+	u32 value_ctrl = 0;
 	int mcbitslog2 = hw->mcast_bits_log2;
 	u32 mc_filter[8];
 	int i;
@@ -669,8 +670,17 @@ static void dwxgmac2_set_filter(struct mac_device_info *hw,
 	memset(mc_filter, 0, sizeof(mc_filter));
 
 	if (dev->flags & IFF_PROMISC) {
-		value |= XGMAC_FILTER_PR;
-		value |= XGMAC_FILTER_PCF;
+		if (hw->vlan_fail_q_en) {
+			value_ctrl = readl(ioaddr + XGMAC_RXQ_CTRL4);
+			value_ctrl &= ~XGMAC_RXQCTRL_VFFQ_MASK;
+			value_ctrl |= XGMAC_RXQCTRL_VFFQE |
+				(hw->vlan_fail_q << XGMAC_RXQCTRL_VFFQ_SHIFT);
+			writel(value_ctrl, ioaddr + XGMAC_RXQ_CTRL4);
+			value |= XGMAC_FILTER_PR | XGMAC_FILTER_RA;
+		} else {
+			value |= XGMAC_FILTER_PR | XGMAC_FILTER_PCF;
+		}
+
 	} else if ((dev->flags & IFF_ALLMULTI) ||
 		   (netdev_mc_count(dev) > hw->multicast_filter_bins)) {
 		value |= XGMAC_FILTER_PM;
@@ -715,7 +725,7 @@ static void dwxgmac2_set_filter(struct mac_device_info *hw,
 
 	writel(value, ioaddr + XGMAC_PACKET_FILTER);
 
-	if (dev->flags & IFF_PROMISC) {
+	if (dev->flags & IFF_PROMISC && !hw->vlan_fail_q_en) {
 		if (!hw->promisc) {
 			hw->promisc = 1;
 			dwxgmac2_vlan_promisc_enable(dev, hw);

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

@@ -543,6 +543,13 @@ stmmac_probe_config_dt(struct platform_device *pdev, u8 *mac)
 			dev_info(&pdev->dev, "RX VLAN HW Stripping\n");
 			plat->use_hw_vlan = true;
 		}
+
+		/*VLAN filter failed queue state */
+		if (of_property_read_bool(np, "snps,vlan-fail-q-en")) {
+			dev_info(&pdev->dev, "VLAN filter failed queue\n");
+			plat->vlan_fail_q_en = true;
+			plat->vlan_fail_q = plat->rx_queues_to_use - 1;
+		}
 	}
 
 	dma_cfg = devm_kzalloc(&pdev->dev, sizeof(*dma_cfg),