[PATCH v1 8/8] net: stmmac: Add option for VLAN filter fail queue enable

[Boon Khai Ng <boon.khai.ng@intel.com>]

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 <boon.khai.ng@intel.com>
---
 drivers/net/ethernet/stmicro/stmmac/dwxgmac2.h   |  5 +++++
 .../net/ethernet/stmicro/stmmac/dwxgmac2_core.c  | 16 +++++++++++++---
 .../ethernet/stmicro/stmmac/stmmac_platform.c    |  7 +++++++
 3 files changed, 25 insertions(+), 3 deletions(-)

diff --git a/drivers/net/ethernet/stmicro/stmmac/dwxgmac2.h b/drivers/net/ethernet/stmicro/stmmac/dwxgmac2.h
index 428f82905273..5043a520a00a 100644
--- a/drivers/net/ethernet/stmicro/stmmac/dwxgmac2.h
+++ b/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)
diff --git a/drivers/net/ethernet/stmicro/stmmac/dwxgmac2_core.c b/drivers/net/ethernet/stmicro/stmmac/dwxgmac2_core.c
index 4dff53dc771f..a3e95efd1e83 100644
--- a/drivers/net/ethernet/stmicro/stmmac/dwxgmac2_core.c
+++ b/drivers/net/ethernet/stmicro/stmmac/dwxgmac2_core.c
@@ -658,6 +658,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;
@@ -668,8 +669,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;
@@ -714,7 +724,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);
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_platform.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_platform.c
index 26da3a9da345..8bc69318ae0d 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_platform.c
+++ b/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),
-- 
2.25.1