net: stmmac: dwmac1000: Add VLAN tag insertion support

DW GMAC supports the VLAN Tag insertion in the framework of the "SA, VLAN,
and CRC Insertion on TX" IP-core option. It' implementation looks very
similar to what can be found in the DW QoS Ether and DW XGMAC controllers
except it doesn't provide a way to re-initialize the VLAN tag via the
specially formed DMA-descriptors. The only way to set the outbound VLAN
Tag is to initialize the respective register. But in order to prevent the
outbound frames corruption that can be done only if there is no packets
pending to be fetched and transmitted via the device DMA engine.

Based on the notes above let's add the VLAN Tag insertion support to the
DW GMAC submodule of the driver. Basically it means to provide two
callbacks:
1. dwmac1000_set_vlan_tag() - setup VLAN tag for the ongoing outbound
traffic.
2. enh_desc_set_vlan() - enable VLAN tag insertion for the frame of the
passed descriptor.

It's also required to selectively enable/disable SVLAN Tags depending on
the current network device HW-features state.

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

7 files changed, 71 insertions, 1 deletions

diff --git a/drivers/net/ethernet/stmicro/stmmac/descs.h b/drivers/net/ethernet/stmicro/stmmac/descs.h
index 22e129e524b7..48fb1cee663f 100644
--- a/drivers/net/ethernet/stmicro/stmmac/descs.h
+++ b/drivers/net/ethernet/stmicro/stmmac/descs.h
@@ -102,6 +102,7 @@
 #define	ETDES0_ERROR_SUMMARY		BIT(15)
 #define	ETDES0_IP_HEADER_ERROR		BIT(16)
 #define	ETDES0_TIME_STAMP_STATUS	BIT(17)
+#define	ETDES0_VLIC_MASK		GENMASK(19, 18)
 #define	ETDES0_SECOND_ADDRESS_CHAINED	BIT(20)
 #define	ETDES0_END_RING			BIT(21)
 #define	ETDES0_CHECKSUM_INSERTION_MASK	GENMASK(23, 22)
diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac1000.h b/drivers/net/ethernet/stmicro/stmmac/dwmac1000.h
index 3f44df41d446..6d69ec596fb9 100644
--- a/drivers/net/ethernet/stmicro/stmmac/dwmac1000.h
+++ b/drivers/net/ethernet/stmicro/stmmac/dwmac1000.h
@@ -73,6 +73,9 @@ enum power_event {
 /* GMAC Watchdog timeout for received frames register */
 #define GMAC_WDT		0x000000dc
 
+/* VLAN Tag Inclusion or Replacement register */
+#define GMAC_VLAN_INCL		0x00000584
+
 /* VLAN Hash register */
 #define GMAC_VLAN_HASH_TABLE	0x00000588
 
@@ -132,6 +135,10 @@ enum inter_frame_gap {
 /* Watchdog timeout for received frames defines */
 #define GMAC_WDT_PWE		BIT(16)
 #define GMAC_WDT_WTO		GENMASK(13, 0)
+/* VLAN Tag inclusing defines */
+#define GMAC_VLAN_CSVL		BIT(19)
+#define GMAC_VLAN_VLC		GENMASK(17, 16)
+#define GMAC_VLAN_VLT		GENMASK(15, 0)
 
 /* DEBUG Register defines */
 /* MTL TxStatus FIFO */
diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac1000_core.c b/drivers/net/ethernet/stmicro/stmmac/dwmac1000_core.c
index 7ed350fbc8af..38032786b221 100644
--- a/drivers/net/ethernet/stmicro/stmmac/dwmac1000_core.c
+++ b/drivers/net/ethernet/stmicro/stmmac/dwmac1000_core.c
@@ -16,6 +16,7 @@
 #include <linux/slab.h>
 #include <linux/ethtool.h>
 #include <linux/io.h>
+#include <linux/iopoll.h>
 #include <linux/string_choices.h>
 #include "stmmac.h"
 #include "stmmac_pcs.h"
@@ -179,6 +180,29 @@ static void dwmac1000_set_mchash(void __iomem *ioaddr, u32 *mcfilterbits,
 		       ioaddr + GMAC_EXTHASH_BASE + regs * 4);
 }
 
+static int dwmac1000_vlan_set_tag(struct mac_device_info *hw, u32 queue, u16 vid)
+{
+	void __iomem *ioaddr = hw->pcsr;
+	bool ready;
+	u32 value;
+	int ret;
+
+	/* Wait for any transfer being finished so not to corrupt the pending
+	 * outbound frames.
+	 */
+	ret = read_poll_timeout_atomic(dwmac_dma_suspended, ready, ready,
+				       100, 50000, false,
+				       ioaddr, queue, DMA_DIR_TX);
+	if (ret)
+		return ret;
+
+	value = readl(ioaddr + GMAC_VLAN_INCL) & ~GMAC_VLAN_VLT;
+	value |= FIELD_PREP(GMAC_VLAN_VLT, vid);
+	writel(value, ioaddr + GMAC_VLAN_INCL);
+
+	return 0;
+}
+
 static void dwmac1000_vlan_write_single(struct net_device *dev, u32 data)
 {
 	void __iomem *ioaddr = (void __iomem *)dev->base_addr;
@@ -305,11 +329,19 @@ static void dwmac1000_vlan_set_hw_mode(struct mac_device_info *hw, bool rx_strip
 
 	/* Activate S-VLAN feature on MAC Tx and Rx */
 	value = readl(ioaddr + GMAC_VLAN_TAG);
-	if (rx_stag)
+	if (rx_stag || tx_stag)
 		value |= GMAC_VLAN_ESVL;
 	else
 		value &= ~GMAC_VLAN_ESVL;
 	writel(value, ioaddr + GMAC_VLAN_TAG);
+
+	/* Set Tx VLAN Insertion feature (might be unavailable) */
+	value = readl(ioaddr + GMAC_VLAN_INCL);
+	if (tx_stag)
+		value |= GMAC_VLAN_CSVL;
+	else
+		value &= ~GMAC_VLAN_CSVL;
+	writel(value, ioaddr + GMAC_VLAN_INCL);
 }
 
 static u32 dwmac1000_get_num_vlan(void __iomem *ioaddr)
@@ -669,6 +701,7 @@ const struct stmmac_ops dwmac1000_ops = {
 };
 
 const struct stmmac_vlan_ops dwmac1000_vlan_ops = {
+	.set_vlan_tag = dwmac1000_vlan_set_tag,
 	.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,
diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac1000_dma.c b/drivers/net/ethernet/stmicro/stmmac/dwmac1000_dma.c
index 17fa072b829e..8a6a6f79bde6 100644
--- a/drivers/net/ethernet/stmicro/stmmac/dwmac1000_dma.c
+++ b/drivers/net/ethernet/stmicro/stmmac/dwmac1000_dma.c
@@ -264,6 +264,8 @@ static int dwmac1000_get_hw_feature(void __iomem *ioaddr,
 	dma_cap->number_tx_channel = (hw_cap & DMA_HW_FEAT_TXCHCNT) >> 22;
 	/* Alternate (enhanced) DESC mode */
 	dma_cap->enh_desc = (hw_cap & DMA_HW_FEAT_ENHDESSEL) >> 24;
+	/* Source address/VLAN Insertion/Replacement/Deletion Engine */
+	dma_cap->vlins = (hw_cap & DMA_HW_FEAT_SAVLANINS) >> 27;
 
 	dma_cap->actphyif = FIELD_GET(DMA_HW_FEAT_ACTPHYIF, hw_cap);
 
diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac_dma.h b/drivers/net/ethernet/stmicro/stmmac/dwmac_dma.h
index a4e034c9fecf..c34e4ccb3b68 100644
--- a/drivers/net/ethernet/stmicro/stmmac/dwmac_dma.h
+++ b/drivers/net/ethernet/stmicro/stmmac/dwmac_dma.h
@@ -167,6 +167,7 @@ void dwmac_dma_start_rx(struct stmmac_priv *priv, void __iomem *ioaddr,
 			u32 chan);
 void dwmac_dma_stop_rx(struct stmmac_priv *priv, void __iomem *ioaddr,
 		       u32 chan);
+bool dwmac_dma_suspended(void __iomem *ioaddr, u32 chan, u32 dir);
 int dwmac_dma_interrupt(struct stmmac_priv *priv, void __iomem *ioaddr,
 			struct stmmac_extra_stats *x, u32 chan, u32 dir);
 void dwmac_dma_diagnostic_fr(struct stmmac_priv *priv, void __iomem *ioaddr,
diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac_lib.c b/drivers/net/ethernet/stmicro/stmmac/dwmac_lib.c
index 75dd1d370f64..90d2d93c322f 100644
--- a/drivers/net/ethernet/stmicro/stmmac/dwmac_lib.c
+++ b/drivers/net/ethernet/stmicro/stmmac/dwmac_lib.c
@@ -158,6 +158,24 @@ static void show_rx_process_state(unsigned int status)
 }
 #endif
 
+bool dwmac_dma_suspended(void __iomem *ioaddr, u32 chan, u32 dir)
+{
+	u32 status = readl(ioaddr + DMA_CHAN_STATUS(chan));
+	u32 ts, rs;
+
+	if (dir == DMA_DIR_TX) {
+		ts = FIELD_GET(DMA_STATUS_TS_MASK, status);
+		return ts == 6;
+	} else if (dir == DMA_DIR_RX) {
+		rs = FIELD_GET(DMA_STATUS_RS_MASK, status);
+		return rs == 4;
+	}
+
+	ts = FIELD_GET(DMA_STATUS_TS_MASK, status);
+	rs = FIELD_GET(DMA_STATUS_RS_MASK, status);
+	return ts == 6 && rs == 4;
+}
+
 int dwmac_dma_interrupt(struct stmmac_priv *priv, void __iomem *ioaddr,
 			struct stmmac_extra_stats *x, u32 chan, u32 dir)
 {
diff --git a/drivers/net/ethernet/stmicro/stmmac/enh_desc.c b/drivers/net/ethernet/stmicro/stmmac/enh_desc.c
index 24ac1d2ff164..01ac6cc72fa6 100644
--- a/drivers/net/ethernet/stmicro/stmmac/enh_desc.c
+++ b/drivers/net/ethernet/stmicro/stmmac/enh_desc.c
@@ -505,6 +505,11 @@ static void enh_desc_clear(struct dma_desc *p)
 	p->des2 = 0;
 }
 
+static void enh_desc_set_vlan(struct dma_desc *p, u32 type)
+{
+	p->des0 |= cpu_to_le32(FIELD_PREP(ETDES0_VLIC_MASK, type));
+}
+
 const struct stmmac_desc_ops enh_desc_ops = {
 	.tx_status = enh_desc_get_tx_status,
 	.rx_status = enh_desc_get_rx_status_nocoe,
@@ -529,6 +534,7 @@ const struct stmmac_desc_ops enh_desc_ops = {
 	.display_ring = enh_desc_display_ring,
 	.set_addr = enh_desc_set_addr,
 	.clear = enh_desc_clear,
+	.set_vlan = enh_desc_set_vlan,
 };
 
 const struct stmmac_desc_ops enh_desc_noext_ops = {
@@ -555,6 +561,7 @@ const struct stmmac_desc_ops enh_desc_noext_ops = {
 	.display_ring = enh_desc_display_ring,
 	.set_addr = enh_desc_set_addr,
 	.clear = enh_desc_clear,
+	.set_vlan = enh_desc_set_vlan,
 };
 
 const struct stmmac_desc_ops enh_desc_ext_ops = {
@@ -581,4 +588,5 @@ const struct stmmac_desc_ops enh_desc_ext_ops = {
 	.display_ring = enh_desc_display_ring,
 	.set_addr = enh_desc_set_addr,
 	.clear = enh_desc_clear,
+	.set_vlan = enh_desc_set_vlan,
 };