From 1febf2f274f5bf13004ddde6d20e8744e36fd650 Mon Sep 17 00:00:00 2001
From: compile_success <980965867@qq.com>
Date: Fri, 3 Nov 2023 14:33:20 +0000
Subject: [PATCH] add vlan support

---
 src/common/gazelle_opt.h         |  4 +++-
 src/lstack/core/lstack_cfg.c     | 13 +++++++++++++
 src/lstack/core/lstack_lwip.c    |  4 ++++
 src/lstack/include/lstack_cfg.h  |  1 +
 src/lstack/lstack.conf           |  3 +++
 src/lstack/netif/lstack_ethdev.c |  8 +++++++-
 src/lstack/netif/lstack_vdev.c   |  4 +++-
 7 files changed, 34 insertions(+), 3 deletions(-)

diff --git a/src/common/gazelle_opt.h b/src/common/gazelle_opt.h
index 7b855f9..0479051 100644
--- a/src/common/gazelle_opt.h
+++ b/src/common/gazelle_opt.h
@@ -52,7 +52,9 @@
 #define STACK_THREAD_DEFAULT        4
 #define STACK_NIC_READ_DEFAULT      128
 
-#define MBUF_MAX_DATA_LEN           1460
+#define MTU_DEFAULT_DATA_LEN        1460
+#define VLAN_HEAD_LEN               4
+#define MBUF_MAX_DATA_LEN           (MTU_DEFAULT_DATA_LEN - VLAN_HEAD_LEN)
 
 #define DPDK_PKT_BURST_SIZE         512
 
diff --git a/src/lstack/core/lstack_cfg.c b/src/lstack/core/lstack_cfg.c
index c4278b5..ac96b1b 100644
--- a/src/lstack/core/lstack_cfg.c
+++ b/src/lstack/core/lstack_cfg.c
@@ -78,6 +78,7 @@ static int32_t parse_udp_enable(void);
 static int32_t parse_nic_rxqueue_size(void);
 static int32_t parse_nic_txqueue_size(void);
 static int32_t parse_stack_thread_mode(void);
+static int32_t parse_nic_vlan_mode(void);
 
 #define PARSE_ARG(_arg, _arg_string, _default_val, _min_val, _max_val, _ret) \
     do { \
@@ -138,6 +139,7 @@ static struct config_vector_t g_config_tbl[] = {
     { "nic_rxqueue_size", parse_nic_rxqueue_size},
     { "nic_txqueue_size", parse_nic_txqueue_size},
     { "stack_thread_mode", parse_stack_thread_mode },
+    { "nic_vlan_mode", parse_nic_vlan_mode },
     { NULL,           NULL }
 };
 
@@ -1224,3 +1226,14 @@ static int32_t parse_stack_thread_mode(void)
 
     return 0;
 }
+
+static int32_t parse_nic_vlan_mode(void)
+{
+    int32_t ret;
+    PARSE_ARG(g_config_params.nic.vlan_mode, "nic_vlan_mode", 0, 0, 4094, ret);
+    if (ret != 0) {
+        LSTACK_PRE_LOG(LSTACK_ERR, "cfg: invalid vlan mode value %d ret=%d. only support 0~4094\n", \
+                            g_config_params.nic.vlan_mode, ret);
+    }
+    return ret;
+}
diff --git a/src/lstack/core/lstack_lwip.c b/src/lstack/core/lstack_lwip.c
index cdb0089..d5c4896 100644
--- a/src/lstack/core/lstack_lwip.c
+++ b/src/lstack/core/lstack_lwip.c
@@ -196,6 +196,10 @@ void do_lwip_init_sock(int32_t fd)
     (void)replenish_send_idlembuf(stack, sock);
 
     sock->stack = stack;
+    if (get_global_cfg_params()->nic.vlan_mode > 0 && get_global_cfg_params()->nic.vlan_mode < 4095) {
+	sock->conn->pcb.udp->netif_hints.tci = get_global_cfg_params()->nic.vlan_mode;
+    }
+
     init_list_node_null(&sock->recv_list);
     init_list_node_null(&sock->event_list);
 }
diff --git a/src/lstack/include/lstack_cfg.h b/src/lstack/include/lstack_cfg.h
index fc627e3..9dea4c1 100644
--- a/src/lstack/include/lstack_cfg.h
+++ b/src/lstack/include/lstack_cfg.h
@@ -62,6 +62,7 @@ struct secondary_attach_arg {
 struct cfg_nic_params {
     uint32_t rxqueue_size;
     uint32_t txqueue_size;
+    uint16_t vlan_mode;
 };
 
 struct cfg_params {
diff --git a/src/lstack/lstack.conf b/src/lstack/lstack.conf
index 48973fe..3eb4685 100644
--- a/src/lstack/lstack.conf
+++ b/src/lstack/lstack.conf
@@ -63,3 +63,6 @@ process_idx=0
 
 #tuple_filer=0, below cfg valid
 listen_shadow=0
+
+#vlan mode; only support 0~4094, 0 is disabled
+nic_vlan_mode=0
diff --git a/src/lstack/netif/lstack_ethdev.c b/src/lstack/netif/lstack_ethdev.c
index da16e85..383a56b 100644
--- a/src/lstack/netif/lstack_ethdev.c
+++ b/src/lstack/netif/lstack_ethdev.c
@@ -778,7 +778,13 @@ int32_t gazelle_eth_dev_poll(struct protocol_stack *stack, uint8_t use_ltran_fla
         /* copy arp into other stack */
         if (!use_ltran_flag) {
             struct rte_ether_hdr *ethh = rte_pktmbuf_mtod(stack->pkts[i], struct rte_ether_hdr *);
-            if (unlikely(RTE_BE16(RTE_ETHER_TYPE_ARP) == ethh->ether_type)) {
+	    u16_t type;
+	    type = ethh->ether_type;
+	    if (type == PP_HTONS(ETHTYPE_VLAN)) {
+		struct eth_vlan_hdr *vlan = (struct eth_vlan_hdr *)(((char *)ethh) + SIZEOF_ETH_HDR);
+		type = vlan->tpid;
+	    }
+            if (unlikely(RTE_BE16(RTE_ETHER_TYPE_ARP) == type)) {
                 stack_broadcast_arp(stack->pkts[i], stack);
                 /* copy arp into other process */
                 transfer_arp_to_other_process(stack->pkts[i]);
diff --git a/src/lstack/netif/lstack_vdev.c b/src/lstack/netif/lstack_vdev.c
index 81b48dc..fb295e0 100644
--- a/src/lstack/netif/lstack_vdev.c
+++ b/src/lstack/netif/lstack_vdev.c
@@ -20,6 +20,7 @@
 #include <rte_ethdev.h>
 #include <rte_gro.h>
 #include <rte_net.h>
+#include <netif/ethernet.h>
 
 #include "lstack_cfg.h"
 #include "lstack_dpdk.h"
@@ -83,12 +84,13 @@ static uint32_t vdev_rx_poll(struct protocol_stack *stack, struct rte_mbuf **pkt
     }
 
     /* skip gro when tcp/ip cksum offloads disable */
-    if (get_protocol_stack_group()->rx_offload == 0) {
+    if (get_protocol_stack_group()->rx_offload == 0 || get_global_cfg_params()->nic.vlan_mode > 0) {
         return pkt_num;
     }
 
     for (uint32_t i = 0; i < pkt_num; i++) {
         struct rte_ether_hdr *ethh = rte_pktmbuf_mtod(pkts[i], struct rte_ether_hdr *);
+
         if (unlikely(RTE_BE16(RTE_ETHER_TYPE_IPV4) != ethh->ether_type)) {
             continue;
         }
-- 
2.27.0