[PATCH net 5/5] net: stmmac: Fix double VLAN stripping on dwxgmac2

[Ovidiu Panait]

Currently, vlan_rx_hw() hardcodes ETH_P_8021Q when passing stripped
VLAN tags up the stack. If the MAC strips an 802.1ad outer tag, it is
incorrectly reported as an 802.1Q tag, breaking QinQ.

Fix this by adding a new get_rx_vlan_type() descriptor op so the driver
can read the actual type of the stripped tag. Implement this for dwxgmac2
only, which exposes the outer tag type in the RDES3 descriptor.

dwmac4 does not expose the tag type in the descriptor, so it does not
implement the op and keeps the ETH_P_8021Q fallback, as before.

Fixes: 534df0c1724b ("net: stmmac: dwxgmac2: Add support for HW-accelerated VLAN stripping")
Signed-off-by: Ovidiu Panait <ovidiu.panait.rb@xxxxxxxxxxx>
---
 .../net/ethernet/stmicro/stmmac/dwxgmac2_descs.c  | 15 +++++++++++++++
 drivers/net/ethernet/stmicro/stmmac/hwif.h        |  2 ++
 drivers/net/ethernet/stmicro/stmmac/stmmac_vlan.c |  6 +++++-
 3 files changed, 22 insertions(+), 1 deletion(-)

diff --git a/drivers/net/ethernet/stmicro/stmmac/dwxgmac2_descs.c b/drivers/net/ethernet/stmicro/stmmac/dwxgmac2_descs.c
index 6719ac6e395b..4fbd2442d918 100644
--- a/drivers/net/ethernet/stmicro/stmmac/dwxgmac2_descs.c
+++ b/drivers/net/ethernet/stmicro/stmmac/dwxgmac2_descs.c
@@ -61,6 +61,20 @@ static void dwxgmac2_set_rx_owner(struct dma_desc *p, int disable_rx_ic)
 	p->des3 |= cpu_to_le32(flags);
 }
 
+static u16 dwxgmac2_wrback_get_rx_vlan_type(struct dma_desc *p)
+{
+	u32 et_lt = FIELD_GET(XGMAC_RDES3_ET_LT, le32_to_cpu(p->des3));
+
+	switch (et_lt) {
+	case XGMAC_ET_LT_VLAN_STAG:
+	case XGMAC_ET_LT_DVLAN_STAG_STAG:
+	case XGMAC_ET_LT_DVLAN_STAG_CTAG:
+		return ETH_P_8021AD;
+	default:
+		return ETH_P_8021Q;
+	}
+}
+
 static u16 dwxgmac2_wrback_get_rx_vlan_tci(struct dma_desc *p)
 {
 	return le32_to_cpu(p->des0) & XGMAC_RDES0_VLAN_TAG_MASK;
@@ -350,6 +364,7 @@ const struct stmmac_desc_ops dwxgmac210_desc_ops = {
 	.rx_status = dwxgmac2_get_rx_status,
 	.set_tx_owner = dwxgmac2_set_tx_owner,
 	.set_rx_owner = dwxgmac2_set_rx_owner,
+	.get_rx_vlan_type = dwxgmac2_wrback_get_rx_vlan_type,
 	.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 66837caafa84..eb6aacc0dfbc 100644
--- a/drivers/net/ethernet/stmicro/stmmac/hwif.h
+++ b/drivers/net/ethernet/stmicro/stmmac/hwif.h
@@ -56,6 +56,8 @@ struct stmmac_desc_ops {
 	/* Clear interrupt on tx frame completion. When this bit is
 	 * set an interrupt happens as soon as the frame is transmitted */
 	void (*set_tx_ic)(struct dma_desc *p);
+	/* Get the VLAN tag type of the descriptor */
+	u16 (*get_rx_vlan_type)(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 9b5b3f11f699..4bb639c717f8 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_vlan.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_vlan.c
@@ -212,8 +212,12 @@ static void vlan_rx_hw(struct mac_device_info *hw,
 {
 	if (hw->desc->get_rx_vlan_valid(rx_desc)) {
 		u16 vid = hw->desc->get_rx_vlan_tci(rx_desc);
+		u16 type = ETH_P_8021Q;
 
-		__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), vid);
+		if (hw->desc->get_rx_vlan_type)
+			type = hw->desc->get_rx_vlan_type(rx_desc);
+
+		__vlan_hwaccel_put_tag(skb, htons(type), vid);
 	}
 }
 
-- 
2.34.1