net: stmmac: dwmac1000: Add VLAN Rx filtering support

DW GMAC IP-core supports two types of the Rx VLAN tag filters. First one
is a basic perfect filter consisting a single CSR to place a being allowed
VID. Second one is a Hash-based filter. It implies to pass the only VLAN
tags which the most significant four bits of their CRC-32 point to a set
bit in the VLAN Hash Table register.

So basically the filters functionality is the same as could be found in
the DW QoS Ether and DW XGMAC IP-cores except there is no Extended VLAN
Perfect filter support and the S-VLAN filtering capability can't be
enabled independently from the C-VLAN tags filtering - it's either both
tag types allowed or just C-Tags. The CSRs layout and their mapping is
different.

Based on the description provided above let's introduce the VLAN Rx Tags
filtering support to the DW GMAC module of the STMMAC driver. It mainly
looks similar to what is found in the rest of the VLAN-capable modules
except the tags counter isn't utilized since the S-VLAN tags filtering is
switched on/off based on the HW-features flag. That leads to passing both
C- and S-Tags if the STAG HW-offloaded filtering is enabled.

Also note that the VLAN Tag Hash-based filtering availability isn't
indicated in the HW-features register. Instead an auto-detection procedure
has been implemented based on the fact that the VLAN Hash register is RO
if feature is unavailable.

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

5 files changed, 200 insertions, 9 deletions

diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac1000.h b/drivers/net/ethernet/stmicro/stmmac/dwmac1000.h
index a424f5884e43..3f44df41d446 100644
--- a/drivers/net/ethernet/stmicro/stmmac/dwmac1000.h
+++ b/drivers/net/ethernet/stmicro/stmmac/dwmac1000.h
@@ -19,6 +19,7 @@
 #define GMAC_MII_DATA		0x00000014	/* MII Data */
 #define GMAC_FLOW_CTRL		0x00000018	/* Flow Control */
 #define GMAC_VLAN_TAG		0x0000001c	/* VLAN Tag */
+#define GMAC_VERSION		0x00000020	/* GMAC version register */
 #define GMAC_DEBUG		0x00000024	/* GMAC debug register */
 
 #define GMAC_INT_STATUS		0x00000038	/* interrupt status register */
@@ -72,6 +73,9 @@ enum power_event {
 /* GMAC Watchdog timeout for received frames register */
 #define GMAC_WDT		0x000000dc
 
+/* VLAN Hash register */
+#define GMAC_VLAN_HASH_TABLE	0x00000588
+
 /* SGMII/RGMII status register */
 #define GMAC_RGSMIIIS_LNKMODE		BIT(0)
 #define GMAC_RGSMIIIS_SPEED		GENMASK(2, 1)
@@ -113,12 +117,18 @@ enum inter_frame_gap {
 #define GMAC_FRAME_FILTER_PM	0x00000010	/* Pass all multicast */
 #define GMAC_FRAME_FILTER_PCF	0x00000080	/* Pass Control frames */
 #define GMAC_FRAME_FILTER_HPF	0x00000400	/* Hash or perfect Filter */
+#define GMAC_FRAME_FILTER_VTFE	0x00010000	/* VLAN Tag Filter */
 #define GMAC_FRAME_FILTER_RA	0x80000000	/* Receive all mode */
 /* GMAC FLOW CTRL defines */
 #define GMAC_FLOW_CTRL_PT_MASK	GENMASK(31, 16)	/* Pause Time Mask */
 #define GMAC_FLOW_CTRL_UP	0x00000008	/* Unicast pause frame enable */
 #define GMAC_FLOW_CTRL_RFE	0x00000004	/* Rx Flow Control Enable */
 #define GMAC_FLOW_CTRL_TFE	0x00000002	/* Tx Flow Control Enable */
+/* VLAN Tag defines */
+#define GMAC_VLAN_VTHM		BIT(19)
+#define GMAC_VLAN_ESVL		BIT(18)
+#define GMAC_VLAN_ETV		BIT(16)
+#define GMAC_VLAN_VID		GENMASK(15, 0)
 /* Watchdog timeout for received frames defines */
 #define GMAC_WDT_PWE		BIT(16)
 #define GMAC_WDT_WTO		GENMASK(13, 0)
diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac1000_core.c b/drivers/net/ethernet/stmicro/stmmac/dwmac1000_core.c
index 312d22a52ea5..7ed350fbc8af 100644
--- a/drivers/net/ethernet/stmicro/stmmac/dwmac1000_core.c
+++ b/drivers/net/ethernet/stmicro/stmmac/dwmac1000_core.c
@@ -69,11 +69,6 @@ static void dwmac1000_core_init(struct mac_device_info *hw,
 
 	/* Mask GMAC interrupts */
 	writel(GMAC_INT_DEFAULT_MASK, ioaddr + GMAC_INT_MASK);
-
-#ifdef STMMAC_VLAN_TAG_USED
-	/* Tag detection without filtering */
-	writel(0x0, ioaddr + GMAC_VLAN_TAG);
-#endif
 }
 
 static void dwmac1000_irq_modify(struct mac_device_info *hw, u32 disable,
@@ -184,6 +179,150 @@ static void dwmac1000_set_mchash(void __iomem *ioaddr, u32 *mcfilterbits,
 		       ioaddr + GMAC_EXTHASH_BASE + regs * 4);
 }
 
+static void dwmac1000_vlan_write_single(struct net_device *dev, u32 data)
+{
+	void __iomem *ioaddr = (void __iomem *)dev->base_addr;
+	u32 val;
+
+	if (!(data & GMAC_VLAN_VID))
+		data |= GMAC_VLAN_VID;
+
+	val = readl(ioaddr + GMAC_VLAN_TAG) & ~GMAC_VLAN_VID;
+	val |= GMAC_VLAN_ETV | (data & GMAC_VLAN_VID);
+
+	writel(val, ioaddr + GMAC_VLAN_TAG);
+}
+
+static int dwmac1000_vlan_add_hw_rx_fltr(struct net_device *dev,
+					 struct mac_device_info *hw,
+					 __be16 proto, u16 vid)
+{
+	bool is_stag;
+	u32 val;
+
+	if (vid > 4095)
+		return -EINVAL;
+
+	is_stag = proto == htons(ETH_P_8021AD);
+
+	/* For single VLAN filter, VID 0 means VLAN promiscuous */
+	if (vid == 0)
+		return -EINVAL;
+
+	/* Note ESVL is utilized to virtually distinguish the C- and S-Tags
+	 * only. The bit is set/cleared in the set_hw_vlan_mode() depending
+	 * on the HW-offloaded features requested.
+	 */
+	val = vid;
+	if (is_stag)
+		val |= GMAC_VLAN_ESVL;
+
+	if (hw->vlan_filter[0] == val)
+		return 0;
+
+	if (hw->vlan_filter[0] & GMAC_VLAN_VID)
+		return -ENOSPC;
+
+	if (netif_running(dev))
+		dwmac1000_vlan_write_single(dev, val);
+
+	hw->vlan_filter[0] = val;
+
+	return 0;
+}
+
+static int dwmac1000_vlan_del_hw_rx_fltr(struct net_device *dev,
+					 struct mac_device_info *hw,
+					 __be16 proto, u16 vid)
+{
+	bool is_stag;
+
+	is_stag = proto == htons(ETH_P_8021AD);
+
+	/* Single Rx VLAN Filter */
+	if ((hw->vlan_filter[0] & GMAC_VLAN_VID) != vid)
+		return -ENOENT;
+
+	if (is_stag != !!(hw->vlan_filter[0] & GMAC_VLAN_ESVL))
+		return -ENOENT;
+
+	if (netif_running(dev))
+		dwmac1000_vlan_write_single(dev, 0);
+
+	hw->vlan_filter[0] = 0;
+
+	return 0;
+}
+
+static void dwmac1000_vlan_update_hash(struct mac_device_info *hw, u32 hash,
+				       int add_ctags, int add_stags)
+{
+	void __iomem *ioaddr = hw->pcsr;
+	u32 value;
+
+	hw->vlan_hash = hash;
+	writel(hash, ioaddr + GMAC_VLAN_HASH_TABLE);
+
+	value = readl(ioaddr + GMAC_VLAN_TAG) | GMAC_VLAN_ETV;
+
+	if (hash)
+		value |= GMAC_VLAN_VTHM;
+	else
+		value &= ~GMAC_VLAN_VTHM;
+
+	/* Note DW GMAC VLAN filter doesn't have a filter-specific flag to
+	 * toggle the S-Tags support. Instead if ESVL flag is set then both
+	 * tag types will be considered as valid. But since the bit also
+	 * affects the MAC transmitter it is handled in set_hw_vlan_mode().
+	 */
+
+	writel(value, ioaddr + GMAC_VLAN_TAG);
+}
+
+static void dwmac1000_vlan_restore_hw_rx_fltr(struct net_device *dev,
+					      struct mac_device_info *hw)
+{
+	/* Hash-based Rx VLAN Filter */
+	dwmac1000_vlan_update_hash(hw, hw->vlan_hash, 0, 0);
+
+	/* Single Rx VLAN Filter */
+	dwmac1000_vlan_write_single(dev, hw->vlan_filter[0]);
+}
+
+static void dwmac1000_vlan_set_hw_mode(struct mac_device_info *hw, bool rx_strip,
+				       bool rx_ctag, bool rx_stag, bool tx_stag)
+{
+	void __iomem *ioaddr = hw->pcsr;
+	u32 value;
+
+	/* Activate VLAN Tag Rx filters */
+	value = readl(ioaddr + GMAC_FRAME_FILTER);
+	if (rx_ctag || rx_stag)
+		value |= GMAC_FRAME_FILTER_VTFE;
+	else
+		value &= ~GMAC_FRAME_FILTER_VTFE;
+	writel(value, ioaddr + GMAC_FRAME_FILTER);
+
+	/* Activate S-VLAN feature on MAC Tx and Rx */
+	value = readl(ioaddr + GMAC_VLAN_TAG);
+	if (rx_stag)
+		value |= GMAC_VLAN_ESVL;
+	else
+		value &= ~GMAC_VLAN_ESVL;
+	writel(value, ioaddr + GMAC_VLAN_TAG);
+}
+
+static u32 dwmac1000_get_num_vlan(void __iomem *ioaddr)
+{
+	u32 val;
+
+	val = readl(ioaddr + GMAC_VERSION);
+	if ((val & GENMASK(7, 0)) >= DWMAC_CORE_3_70)
+		return 1;
+
+	return 0;
+}
+
 static void dwmac1000_set_filter(struct mac_device_info *hw,
 				 struct net_device *dev)
 {
@@ -195,20 +334,30 @@ static void dwmac1000_set_filter(struct mac_device_info *hw,
 	pr_debug("%s: # mcasts %d, # unicast %d\n", __func__,
 		 netdev_mc_count(dev), netdev_uc_count(dev));
 
+	value = readl(ioaddr + GMAC_FRAME_FILTER);
+	value &= ~GMAC_FRAME_FILTER_RA;
+	value &= ~GMAC_FRAME_FILTER_HPF;
+	value &= ~GMAC_FRAME_FILTER_PCF;
+	value &= ~GMAC_FRAME_FILTER_PM;
+	value &= ~GMAC_FRAME_FILTER_HMC;
+	value &= ~GMAC_FRAME_FILTER_PR;
+
 	memset(mc_filter, 0, sizeof(mc_filter));
 
 	if (dev->flags & IFF_PROMISC) {
-		value = GMAC_FRAME_FILTER_PR | GMAC_FRAME_FILTER_PCF;
+		value |= GMAC_FRAME_FILTER_RA;
+		value |= GMAC_FRAME_FILTER_PR;
+		value |= GMAC_FRAME_FILTER_PCF;
 	} else if (dev->flags & IFF_ALLMULTI) {
 		value = GMAC_FRAME_FILTER_PM;	/* pass all multi */
 	} else if (!netdev_mc_empty(dev) && (mcbitslog2 == 0)) {
 		/* Fall back to all multicast if we've no filter */
-		value = GMAC_FRAME_FILTER_PM;
+		value |= GMAC_FRAME_FILTER_PM;
 	} else if (!netdev_mc_empty(dev) && dev->flags & IFF_MULTICAST) {
 		struct netdev_hw_addr *ha;
 
 		/* Hash filter for multicast */
-		value = GMAC_FRAME_FILTER_HMC;
+		value |= GMAC_FRAME_FILTER_HMC;
 
 		netdev_for_each_mc_addr(ha, dev) {
 			/* The upper n bits of the calculated CRC are used to
@@ -257,10 +406,14 @@ static void dwmac1000_set_filter(struct mac_device_info *hw,
 	/* Enable Receive all mode (to debug filtering_fail errors) */
 	value |= GMAC_FRAME_FILTER_RA;
 #endif
+
+	/* VLAN filtering */
+	if (dev->features & NETIF_F_HW_VLAN_CTAG_FILTER)
+		dwmac1000_vlan_restore_hw_rx_fltr(dev, hw);
+
 	writel(value, ioaddr + GMAC_FRAME_FILTER);
 }
 
-
 static void dwmac1000_flow_ctrl(struct mac_device_info *hw, unsigned int duplex,
 				unsigned int fc, unsigned int pause_time,
 				u32 tx_cnt)
@@ -515,6 +668,14 @@ const struct stmmac_ops dwmac1000_ops = {
 	.set_mac_loopback = dwmac1000_set_mac_loopback,
 };
 
+const struct stmmac_vlan_ops dwmac1000_vlan_ops = {
+	.update_vlan_hash = dwmac1000_vlan_update_hash,
+	.add_hw_vlan_rx_fltr = dwmac1000_vlan_add_hw_rx_fltr,
+	.del_hw_vlan_rx_fltr = dwmac1000_vlan_del_hw_rx_fltr,
+	.restore_hw_vlan_rx_fltr = dwmac1000_vlan_restore_hw_rx_fltr,
+	.set_hw_vlan_mode = dwmac1000_vlan_set_hw_mode,
+};
+
 int dwmac1000_setup(struct stmmac_priv *priv)
 {
 	struct mac_device_info *mac = priv->hw;
@@ -545,6 +706,7 @@ int dwmac1000_setup(struct stmmac_priv *priv)
 	mac->mii.reg_mask = 0x000007C0;
 	mac->mii.clk_csr_shift = 2;
 	mac->mii.clk_csr_mask = GENMASK(5, 2);
+	mac->num_vlan = dwmac1000_get_num_vlan(priv->ioaddr);
 
 	return 0;
 }
diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac1000_dma.c b/drivers/net/ethernet/stmicro/stmmac/dwmac1000_dma.c
index 3a545442e903..17fa072b829e 100644
--- a/drivers/net/ethernet/stmicro/stmmac/dwmac1000_dma.c
+++ b/drivers/net/ethernet/stmicro/stmmac/dwmac1000_dma.c
@@ -209,6 +209,20 @@ static void dwmac1000_dump_dma_regs(struct stmmac_priv *priv,
 				readl(ioaddr + DMA_BUS_MODE + i * 4);
 }
 
+static bool dwmac1000_det_hw_vlhash(void __iomem *ioaddr)
+{
+	u32 tmp;
+
+	writel(0xffff, ioaddr + GMAC_VLAN_HASH_TABLE);
+	tmp = readl(ioaddr + GMAC_VLAN_HASH_TABLE);
+	if (tmp == 0xffff) {
+		writel(0x0, ioaddr + GMAC_VLAN_HASH_TABLE);
+		return true;
+	}
+
+	return false;
+}
+
 static int dwmac1000_get_hw_feature(void __iomem *ioaddr,
 				    struct dma_features *dma_cap)
 {
@@ -253,6 +267,9 @@ static int dwmac1000_get_hw_feature(void __iomem *ioaddr,
 
 	dma_cap->actphyif = FIELD_GET(DMA_HW_FEAT_ACTPHYIF, hw_cap);
 
+	/* 16-bit VLAN Hash Filter (flag is unavailable in the CSR) */
+	dma_cap->vlhash = dwmac1000_det_hw_vlhash(ioaddr);
+
 	return 0;
 }
 
diff --git a/drivers/net/ethernet/stmicro/stmmac/hwif.c b/drivers/net/ethernet/stmicro/stmmac/hwif.c
index c8025504e5af..46b8cf7345a8 100644
--- a/drivers/net/ethernet/stmicro/stmmac/hwif.c
+++ b/drivers/net/ethernet/stmicro/stmmac/hwif.c
@@ -141,6 +141,7 @@ static const struct stmmac_hwif_entry {
 		.desc = NULL,
 		.dma = &dwmac1000_dma_ops,
 		.mac = &dwmac1000_ops,
+		.vlan = &dwmac1000_vlan_ops,
 		.hwtimestamp = &dwmac1000_ptp,
 		.ptp = &dwmac1000_ptp_clock_ops,
 		.tc = NULL,
diff --git a/drivers/net/ethernet/stmicro/stmmac/hwif.h b/drivers/net/ethernet/stmicro/stmmac/hwif.h
index 63e926a5583a..7e43630c6e87 100644
--- a/drivers/net/ethernet/stmicro/stmmac/hwif.h
+++ b/drivers/net/ethernet/stmicro/stmmac/hwif.h
@@ -679,6 +679,7 @@ extern const struct stmmac_ops dwmac100_ops;
 extern const struct stmmac_dma_ops dwmac100_dma_ops;
 extern const struct stmmac_ops dwmac1000_ops;
 extern const struct stmmac_dma_ops dwmac1000_dma_ops;
+extern const struct stmmac_vlan_ops dwmac1000_vlan_ops;
 extern const struct stmmac_ops dwmac4_ops;
 extern const struct stmmac_dma_ops dwmac4_dma_ops;
 extern const struct stmmac_ops dwmac410_ops;