[PATCH 3/3] net/af_packet: fix QinQ outer TPID on VLAN reinsertion

[William Bland <blandwwa@gmail.com>]

eth_af_packet_rx() called rte_vlan_insert(), which hardcodes EtherType
0x8100 (802.1Q). This was wrong when reinserting a stripped tag whose
original TPID was 0x88a8 (802.1ad QinQ).

Fix by reading the actual TPID from tp_vlan_tpid (struct tpacket2_hdr),
then passing it to rte_vlan_insert_tpid().

Fixes: 23deeebfcfa8 ("net/af_packet: support 802.1Q VLAN")

Signed-off-by: William Bland <blandwwa@gmail.com>
---
 app/test/meson.build                      |   2 +-
 app/test/test_net_ether.c                 | 122 ++++++++++++++++++++++
 drivers/net/af_packet/rte_eth_af_packet.c |  10 +-
 3 files changed, 131 insertions(+), 3 deletions(-)

diff --git a/app/test/meson.build b/app/test/meson.build
index 51abeeb732..e927d4306a 100644
--- a/app/test/meson.build
+++ b/app/test/meson.build
@@ -134,7 +134,7 @@ source_file_deps = {
     'test_meter.c': ['meter'],
     'test_metrics.c': ['metrics'],
     'test_mp_secondary.c': ['hash'],
-    'test_net_ether.c': ['net'],
+    'test_net_ether.c': ['net', 'mbuf'],
     'test_net_ip6.c': ['net'],
     'test_pcapng.c': ['net_null', 'net', 'ethdev', 'pcapng', 'bus_vdev'],
     'test_pdcp.c': ['eventdev', 'pdcp', 'net', 'timer', 'security'],
diff --git a/app/test/test_net_ether.c b/app/test/test_net_ether.c
index ec11224171..19f9c85e5b 100644
--- a/app/test/test_net_ether.c
+++ b/app/test/test_net_ether.c
@@ -3,6 +3,8 @@
  */
 
 #include <rte_ether.h>
+#include <rte_mbuf.h>
+#include <rte_mempool.h>
 
 #include <rte_test.h>
 #include "test.h"
@@ -162,4 +164,124 @@ test_net_ether(void)
 	return 0;
 }
 
+/*
+ * Build a minimal Ethernet frame in an mbuf: Ethernet header with the given
+ * ether_type followed by payload_len zero bytes.
+ */
+static struct rte_mbuf *
+alloc_frame(struct rte_mempool *mp, uint16_t ether_type, uint16_t payload_len)
+{
+	struct rte_ether_hdr *eh;
+	struct rte_mbuf *m;
+
+	m = rte_pktmbuf_alloc(mp);
+	if (m == NULL)
+		return NULL;
+
+	eh = (struct rte_ether_hdr *)rte_pktmbuf_append(m,
+			sizeof(*eh) + payload_len);
+	if (eh == NULL) {
+		rte_pktmbuf_free(m);
+		return NULL;
+	}
+
+	memset(eh->dst_addr.addr_bytes, 0xff, RTE_ETHER_ADDR_LEN);
+	memset(eh->src_addr.addr_bytes, 0x00, RTE_ETHER_ADDR_LEN);
+	eh->ether_type = rte_cpu_to_be_16(ether_type);
+
+	return m;
+}
+
+static int
+test_vlan_insert_8021q(struct rte_mempool *mp)
+{
+	struct rte_ether_hdr *eh;
+	struct rte_vlan_hdr *vh;
+	struct rte_mbuf *m;
+	int ret;
+
+	m = alloc_frame(mp, RTE_ETHER_TYPE_IPV4, 46);
+	TEST_ASSERT_NOT_NULL(m, "Failed to allocate mbuf");
+
+	m->vlan_tci = 100;
+	m->ol_flags |= RTE_MBUF_F_RX_VLAN | RTE_MBUF_F_RX_VLAN_STRIPPED;
+
+	ret = rte_vlan_insert(&m);
+	TEST_ASSERT_SUCCESS(ret, "rte_vlan_insert failed");
+
+	eh = rte_pktmbuf_mtod(m, struct rte_ether_hdr *);
+	TEST_ASSERT_EQUAL(rte_be_to_cpu_16(eh->ether_type), RTE_ETHER_TYPE_VLAN,
+		"Expected 802.1Q TPID 0x%04x, got 0x%04x",
+		RTE_ETHER_TYPE_VLAN, rte_be_to_cpu_16(eh->ether_type));
+
+	vh = (struct rte_vlan_hdr *)(eh + 1);
+	TEST_ASSERT_EQUAL(rte_be_to_cpu_16(vh->vlan_tci), 100,
+		"Expected VID 100, got %u", rte_be_to_cpu_16(vh->vlan_tci));
+
+	rte_pktmbuf_free(m);
+	return TEST_SUCCESS;
+}
+
+static int
+test_vlan_insert_tpid(struct rte_mempool *mp)
+{
+	struct rte_ether_hdr *eh;
+	struct rte_vlan_hdr *vh;
+	struct rte_mbuf *m;
+	int ret;
+
+	m = alloc_frame(mp, RTE_ETHER_TYPE_VLAN, sizeof(*vh) + 46);
+	TEST_ASSERT_NOT_NULL(m, "Failed to allocate mbuf");
+
+	vh = (struct rte_vlan_hdr *)(rte_pktmbuf_mtod(m, struct rte_ether_hdr *) + 1);
+	vh->vlan_tci = rte_cpu_to_be_16(200);
+	vh->eth_proto = rte_cpu_to_be_16(RTE_ETHER_TYPE_IPV4);
+
+	m->vlan_tci = 50;
+	m->ol_flags |= RTE_MBUF_F_RX_VLAN | RTE_MBUF_F_RX_VLAN_STRIPPED;
+
+	ret = rte_vlan_insert_tpid(&m, RTE_ETHER_TYPE_QINQ);
+	TEST_ASSERT_SUCCESS(ret, "rte_vlan_insert_tpid failed");
+
+	eh = rte_pktmbuf_mtod(m, struct rte_ether_hdr *);
+
+	TEST_ASSERT_EQUAL(rte_be_to_cpu_16(eh->ether_type), RTE_ETHER_TYPE_QINQ,
+		"Outer TPID: expected 0x%04x (802.1ad) got 0x%04x",
+		RTE_ETHER_TYPE_QINQ, rte_be_to_cpu_16(eh->ether_type));
+
+	vh = (struct rte_vlan_hdr *)(eh + 1);
+	TEST_ASSERT_EQUAL(rte_be_to_cpu_16(vh->vlan_tci), 50,
+		"Outer VID: expected 50, got %u",
+		rte_be_to_cpu_16(vh->vlan_tci));
+
+	rte_pktmbuf_free(m);
+	return TEST_SUCCESS;
+}
+
+static int
+test_vlan_insert(void)
+{
+	struct rte_mempool *mp;
+	int ret;
+
+	mp = rte_pktmbuf_pool_create("vlan_insert_test_pool", 64, 0, 0,
+				     RTE_MBUF_DEFAULT_BUF_SIZE,
+				     SOCKET_ID_ANY);
+	if (mp == NULL) {
+		fprintf(stderr, "Failed to create mempool\n");
+		return -1;
+	}
+
+	ret = test_vlan_insert_8021q(mp);
+	if (ret != TEST_SUCCESS)
+		goto out;
+
+	ret = test_vlan_insert_tpid(mp);
+
+out:
+	rte_mempool_free(mp);
+	return ret;
+}
+
 REGISTER_FAST_TEST(net_ether_autotest, NOHUGE_OK, ASAN_OK, test_net_ether);
+REGISTER_FAST_TEST(vlan_insert_autotest, NOHUGE_OK, ASAN_OK, test_vlan_insert);
diff --git a/drivers/net/af_packet/rte_eth_af_packet.c b/drivers/net/af_packet/rte_eth_af_packet.c
index b0ff22ea55..8225e0d7f9 100644
--- a/drivers/net/af_packet/rte_eth_af_packet.c
+++ b/drivers/net/af_packet/rte_eth_af_packet.c
@@ -222,11 +222,17 @@ eth_af_packet_rx(void *queue, struct rte_mbuf **bufs, uint16_t nb_pkts)
 
 		/* check for vlan info */
 		if (ppd->tp_status & TP_STATUS_VLAN_VALID) {
+			uint16_t tpid;
+
 			mbuf->vlan_tci = ppd->tp_vlan_tci;
 			mbuf->ol_flags |= (RTE_MBUF_F_RX_VLAN | RTE_MBUF_F_RX_VLAN_STRIPPED);
 
-			if (!pkt_q->vlan_strip && rte_vlan_insert(&mbuf))
-				PMD_LOG(ERR, "Failed to reinsert VLAN tag");
+			if (!pkt_q->vlan_strip) {
+				tpid = (ppd->tp_status & TP_STATUS_VLAN_TPID_VALID) ?
+					ppd->tp_vlan_tpid : RTE_ETHER_TYPE_VLAN;
+				if (rte_vlan_insert_tpid(&mbuf, tpid))
+					PMD_LOG(ERR, "Failed to reinsert VLAN tag");
+			}
 		}
 
 		/* add kernel provided timestamp when offloading is enabled */
-- 
2.43.0