net: stmmac: vlan: Extract VLAN TPID from DMA-descriptors

DW QoS Eth and DW XGMAC IP-cores are always equipped with the VLAN tag
stripping feature. That is the MAC are capable to extract VLAN tag from
the L2 header and place it into the dedicated DMA-descriptor field. This
feature is already supported by the STMMAC driver. But what is missing is
the VLAN tag type extraction in the meantime at least DW XGMAC can
determined and report the L2 packet type including VLAN-frame Tag protocol
ID.

So in order to support the feature denoted above let's introduce the
stmmac_desc_ops::get_rx_vlan_tpid() callback which would provide a VLAN
TPID of the received frame of course if the frame is detected to be VLAN.
Define it for both DW QoS Eth (GMAC4) and DW XGMAC IP-cores which are
currently supported by the driver.

In case of DW QoS Eth IP-core this callback shall return just 802.1Q tag
type since at the state of v5.20a IP-core the wrote-back receive
descriptor doesn't provide such information. That's why all the VLAN
frames received reported as C-Tagged.

Note for a VLAN tag and VLAN Tag protocol ID being valid they must be
extracted from the last and not erroneous descriptor. Thus the
dwxgmac2_wrback_get_rx_vlan_valid() method fixed accordingly.

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

4 files changed, 37 insertions, 5 deletions

diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac4_descs.c b/drivers/net/ethernet/stmicro/stmmac/dwmac4_descs.c
index 86eb6c01a7d0..4287c381e530 100644
--- a/drivers/net/ethernet/stmicro/stmmac/dwmac4_descs.c
+++ b/drivers/net/ethernet/stmicro/stmmac/dwmac4_descs.c
@@ -218,6 +218,12 @@ static int dwmac4_get_tx_ls(struct dma_desc *p)
 		>> TDES3_LAST_DESCRIPTOR_SHIFT;
 }
 
+static u16 dwmac4_wrback_get_rx_vlan_tpid(struct dma_desc *p)
+{
+	/* Alas at least up to v5.20 there is no TPID in wrback desc */
+	return ETH_P_8021Q;
+}
+
 static u16 dwmac4_wrback_get_rx_vlan_tci(struct dma_desc *p)
 {
 	return (le32_to_cpu(p->des0) & RDES0_VLAN_TAG_MASK);
@@ -604,6 +610,7 @@ const struct stmmac_desc_ops dwmac4_desc_ops = {
 	.set_tx_owner = dwmac4_set_tx_owner,
 	.set_rx_owner = dwmac4_set_rx_owner,
 	.get_tx_ls = dwmac4_get_tx_ls,
+	.get_rx_vlan_tpid = dwmac4_wrback_get_rx_vlan_tpid,
 	.get_rx_vlan_tci = dwmac4_wrback_get_rx_vlan_tci,
 	.get_rx_vlan_valid = dwmac4_wrback_get_rx_vlan_valid,
 	.get_rx_frame_len = dwmac4_wrback_get_rx_frame_len,
diff --git a/drivers/net/ethernet/stmicro/stmmac/dwxgmac2_descs.c b/drivers/net/ethernet/stmicro/stmmac/dwxgmac2_descs.c
index 192860b639f6..7301df6a6d34 100644
--- a/drivers/net/ethernet/stmicro/stmmac/dwxgmac2_descs.c
+++ b/drivers/net/ethernet/stmicro/stmmac/dwxgmac2_descs.c
@@ -154,6 +154,26 @@ static int dwxgmac2_get_tx_ls(struct dma_desc *p)
 	return (le32_to_cpu(p->des3) & XGMAC_RDES3_LD) > 0;
 }
 
+static u16 dwxgmac2_wrback_get_rx_vlan_tpid(struct dma_desc *p)
+{
+	u32 l2t;
+
+	l2t = FIELD_GET(XGMAC_RDES3_ET_LT, le32_to_cpu(p->des3));
+
+	switch (l2t) {
+	case XGMAC_L2T_VLAN_STAG:
+	case XGMAC_L2T_DVLAN_SSTAG:
+	case XGMAC_L2T_DVLAN_SCTAG:
+		return ETH_P_8021AD;
+	case XGMAC_L2T_VLAN_CTAG:
+	case XGMAC_L2T_DVLAN_CCTAG:
+	case XGMAC_L2T_DVLAN_CSTAG:
+		return ETH_P_8021Q;
+	default:
+		return 0;
+	}
+}
+
 static u16 dwxgmac2_wrback_get_rx_vlan_tci(struct dma_desc *p)
 {
 	return le32_to_cpu(p->des0) & XGMAC_RDES0_VLAN_TAG_MASK;
@@ -161,12 +181,13 @@ static u16 dwxgmac2_wrback_get_rx_vlan_tci(struct dma_desc *p)
 
 static bool dwxgmac2_wrback_get_rx_vlan_valid(struct dma_desc *p)
 {
-	u32 et_lt;
+	u32 des3 = le32_to_cpu(p->des3);
+	u32 l2t;
 
-	et_lt = FIELD_GET(XGMAC_RDES3_ET_LT, le32_to_cpu(p->des3));
+	l2t = FIELD_GET(XGMAC_RDES3_ET_LT, des3);
 
-	return et_lt >= XGMAC_ET_LT_VLAN_STAG &&
-	       et_lt <= XGMAC_ET_LT_DVLAN_STAG_CTAG;
+	return (des3 & XGMAC_RDES3_LD) && !(des3 & XGMAC_RDES3_ES) &&
+	       (l2t >= XGMAC_L2T_VLAN_STAG) && (l2t <= XGMAC_L2T_DVLAN_CSTAG);
 }
 
 static int dwxgmac2_get_rx_frame_len(struct dma_desc *p)
@@ -483,6 +504,7 @@ const struct stmmac_desc_ops dwxgmac210_desc_ops = {
 	.set_tx_owner = dwxgmac2_set_tx_owner,
 	.set_rx_owner = dwxgmac2_set_rx_owner,
 	.get_tx_ls = dwxgmac2_get_tx_ls,
+	.get_rx_vlan_tpid = dwxgmac2_wrback_get_rx_vlan_tpid,
 	.get_rx_vlan_tci = dwxgmac2_wrback_get_rx_vlan_tci,
 	.get_rx_vlan_valid = dwxgmac2_wrback_get_rx_vlan_valid,
 	.get_rx_frame_len = dwxgmac2_get_rx_frame_len,
diff --git a/drivers/net/ethernet/stmicro/stmmac/hwif.h b/drivers/net/ethernet/stmicro/stmmac/hwif.h
index b332d2a3bf27..63e926a5583a 100644
--- a/drivers/net/ethernet/stmicro/stmmac/hwif.h
+++ b/drivers/net/ethernet/stmicro/stmmac/hwif.h
@@ -66,6 +66,8 @@ struct stmmac_desc_ops {
 	void (*set_tx_ic)(struct dma_desc *p);
 	/* Last tx segment reports the transmit status */
 	int (*get_tx_ls)(struct dma_desc *p);
+	/* Get the protocol of the descriptor */
+	u16 (*get_rx_vlan_tpid)(struct dma_desc *p);
 	/* Get the tag of the descriptor */
 	u16 (*get_rx_vlan_tci)(struct dma_desc *p);
 	/* Get the valid status of descriptor */
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_vlan.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_vlan.c
index 4cf1096a119a..4ce60c5a93ae 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_vlan.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_vlan.c
@@ -231,9 +231,10 @@ static void vlan_rx_hw(struct mac_device_info *hw,
 		       struct dma_desc *rx_desc, struct sk_buff *skb)
 {
 	if (hw->desc->get_rx_vlan_valid(rx_desc)) {
+		u16 tpid = hw->desc->get_rx_vlan_tpid(rx_desc);
 		u16 vid = hw->desc->get_rx_vlan_tci(rx_desc);
 
-		__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), vid);
+		__vlan_hwaccel_put_tag(skb, htons(tpid), vid);
 	}
 }