TSG-16531 PacketAdapter适配容器环境，使用mrzcpd收包，通过RAW Socket注RST包v2.0.0-20230810

9 files changed, 1039 insertions, 279 deletions

diff --git a/common/src/decode_gtp.c b/common/src/decode_gtp.c
deleted file mode 100644
index 9398ab5..0000000
--- a/common/src/decode_gtp.c
+++ /dev/null
@@ -1,56 +0,0 @@
-#include "decode_gtp.h"
-
-#define GTP_TPDU 255
-#define GTP1U_PORT 2152
-#define GTP1_F_MASK 0x07
-
-#define GTP1_GET_TYPE(gtp1_hdr) ((gtp1_hdr)->type)
-#define GTP1_GET_FLAGS(gtp1_hdr) ((gtp1_hdr)->flags >> 5)
-#define GTP1_GET_HLEN(gtp1_hdr) (((gtp1_hdr)->flags & GTP1_F_MASK) > 0 ? 12 : 8)
-
-enum gtp_version_e
-{
-    GTP_V0 = 0,
-    GTP_V1,
-};
-
-int decode_gtp(gtp_info_t *packet, const uint8_t *data, uint32_t len)
-{
-    if (len < sizeof(gtp1_header_t))
-    {
-        LOG_ERROR("Parser GTP Header: packet length too small %d", len);
-        return -1;
-    }
-
-    packet->hdr = (gtp1_header_t *)data;
-    if (GTP1_GET_FLAGS(packet->hdr) != GTP_V1)
-    {
-        LOG_ERROR("Parser GTP Header: invalid gtp flags %d", GTP1_GET_FLAGS(packet->hdr));
-        return -1;
-    }
-
-    if (GTP1_GET_TYPE(packet->hdr) != GTP_TPDU)
-    {
-        LOG_ERROR("Parser GTP Header: invalid gtp type %d", GTP1_GET_TYPE(packet->hdr));
-        return -1;
-    }
-
-    /* From 29.060: "This field shall be present if and only if any one or
-     * more of the S, PN and E flags are set.".
-     *
-     * If any of the bit is set, then the remaining ones also have to be set.
-     */
-    packet->hdr_len = GTP1_GET_HLEN(packet->hdr);
-    packet->payload = (uint8_t *)data + packet->hdr_len;
-    packet->payload_len = len - packet->hdr_len;
-
-    return 0;
-}
-
-int dump_gtp_info(gtp_info_t *packet, char *buff, size_t size)
-{
-    return snprintf(buff, size,
-                    "{\"hdr_len\":%u,\"data_len\":%u}",
-                    packet->hdr_len,
-                    packet->payload_len);
-}
\ No newline at end of file
diff --git a/common/src/decode_ipv4.c b/common/src/decode_ipv4.c
deleted file mode 100644
index 512d5af..0000000
--- a/common/src/decode_ipv4.c
+++ /dev/null
@@ -1,68 +0,0 @@
-#include "decode_ipv4.h"
-
-#define IPV4_GET_HLEN(ip4_hdr) (((ip4_hdr)->ip_verhl & 0x0f) << 2)
-#define IPV4_GET_IPPROTO(ip4_hdr) ((ip4_hdr)->ip_proto)
-#define IPV4_GET_IPLEN(ip4_hdr) ((uint16_t)ntohs((ip4_hdr)->ip_len))
-#define IPV4_GET_SRC_ADDR(ip4_hdr) ((ip4_hdr)->ip4_hdrun1.ip4_un1.ip_src)
-#define IPV4_GET_DST_ADDR(ip4_hdr) ((ip4_hdr)->ip4_hdrun1.ip4_un1.ip_dst)
-
-int decode_ipv4(ipv4_info_t *packet, const uint8_t *data, uint32_t len)
-{
-    // 检查包长是否大于 IPv4 header
-    if (len < IPV4_HEADER_LEN)
-    {
-        LOG_ERROR("Parser IPv4 Header: packet length too small %d", len);
-        return -1;
-    }
-
-    // 检查 IPv4 header version
-    if (IP_GET_RAW_VER(data) != 4)
-    {
-        LOG_ERROR("Parser IPv4 Header: invalid IP version %d", IP_GET_RAW_VER(data));
-        return -1;
-    }
-
-    packet->hdr = (ipv4_header_t *)data;
-    // 检查 IPv4 header length
-    if (IPV4_GET_HLEN(packet->hdr) < IPV4_HEADER_LEN)
-    {
-        LOG_ERROR("Parser IPv4 Header: invalid IP header length %d", IPV4_GET_HLEN(packet->hdr));
-        return -1;
-    }
-
-    // 检查 IPv4 header total length
-    if (IPV4_GET_IPLEN(packet->hdr) < IPV4_GET_HLEN(packet->hdr))
-    {
-        LOG_ERROR("Parser IPv4 Header: invalid IP header total length %d", IPV4_GET_IPLEN(packet->hdr));
-        return -1;
-    }
-
-    // 检查是否 IP 分片
-    if (len < IPV4_GET_IPLEN(packet->hdr))
-    {
-        LOG_ERROR("Parser IPv4 Header: trunc packet");
-        return -1;
-    }
-
-    inet_ntop(AF_INET, &IPV4_GET_SRC_ADDR(packet->hdr), packet->src_addr, sizeof(packet->src_addr));
-    inet_ntop(AF_INET, &IPV4_GET_DST_ADDR(packet->hdr), packet->dst_addr, sizeof(packet->dst_addr));
-
-    packet->next_protocol = IPV4_GET_IPPROTO(packet->hdr);
-    packet->hdr_len = IPV4_GET_HLEN(packet->hdr);
-    packet->opts_len = packet->hdr_len - IPV4_HEADER_LEN;
-    packet->payload_len = len - packet->hdr_len;
-    packet->payload = (uint8_t *)data + packet->hdr_len;
-
-    return 0;
-}
-
-int dump_ipv4_info(ipv4_info_t *packet, char *buff, size_t size)
-{
-    return snprintf(buff, size,
-                    "{\"src_addr\":\"%s\",\"dst_addr\":\"%s\",\"hdr_len\":%u,\"opts_len\":%u,\"data_len\":%u}",
-                    packet->src_addr,
-                    packet->dst_addr,
-                    packet->hdr_len,
-                    packet->opts_len,
-                    packet->payload_len);
-}
\ No newline at end of file
diff --git a/common/src/decode_ipv6.c b/common/src/decode_ipv6.c
deleted file mode 100644
index b360550..0000000
--- a/common/src/decode_ipv6.c
+++ /dev/null
@@ -1,55 +0,0 @@
-#include "decode_ipv6.h"
-
-#define IPV6_GET_PLEN(ip6_hdr) ((uint16_t)ntohs((ip6_hdr)->ip6_hdrun.ip6_un1.ip6_un1_plen))
-#define IPV6_GET_NH(ip6_hdr) ((ip6_hdr)->ip6_hdrun.ip6_un1.ip6_un1_nxt)
-#define IPV6_GET_SRC_ADDR(ip6_hdr) ((ip6_hdr)->ip6_hdrun2.ip6_un2.ip6_src)
-#define IPV6_GET_DST_ADDR(ip6_hdr) ((ip6_hdr)->ip6_hdrun2.ip6_un2.ip6_dst)
-
-int decode_ipv6(ipv6_info_t *packet, const uint8_t *data, uint32_t len)
-{
-    if (len < IPV6_HEADER_LEN)
-    {
-        LOG_ERROR("Parser IPv6 Header: packet length too small %d", len);
-        return -1;
-    }
-
-    // 检查 IPv6 header version
-    if (IP_GET_RAW_VER(data) != 6)
-    {
-        LOG_ERROR("Parser IPv6 Header: invalid IP version %d", IP_GET_RAW_VER(data));
-        return -1;
-    }
-
-    packet->hdr = (ipv6_header_t *)data;
-    if (len < (IPV6_HEADER_LEN + IPV6_GET_PLEN(packet->hdr)))
-    {
-        LOG_ERROR("Parser IPv6 Header: trunc packet");
-        return -1;
-    }
-
-    if (len != (IPV6_HEADER_LEN + IPV6_GET_PLEN(packet->hdr)))
-    {
-        LOG_ERROR("Parser IPv6 Header: invalid payload length %d", IPV6_GET_PLEN(packet->hdr));
-        return -1;
-    }
-
-    inet_ntop(AF_INET6, &IPV6_GET_SRC_ADDR(packet->hdr), packet->src_addr, sizeof(packet->src_addr));
-    inet_ntop(AF_INET6, &IPV6_GET_DST_ADDR(packet->hdr), packet->dst_addr, sizeof(packet->dst_addr));
-
-    packet->next_protocol = IPV6_GET_NH(packet->hdr);
-    packet->hdr_len = IPV6_HEADER_LEN;
-    packet->payload = (uint8_t *)data + packet->hdr_len;
-    packet->payload_len = len - packet->hdr_len;
-
-    return 0;
-}
-
-int dump_ipv6_info(ipv6_info_t *packet, char *buff, size_t size)
-{
-    return snprintf(buff, size,
-                    "{\"src_addr\":\"%s\",\"dst_addr\":\"%s\",\"hdr_len\":%u,\"data_len\":%u}",
-                    packet->src_addr,
-                    packet->dst_addr,
-                    packet->hdr_len,
-                    packet->payload_len);
-}
\ No newline at end of file
diff --git a/common/src/decode_tcp.c b/common/src/decode_tcp.c
deleted file mode 100644
index f8a7cd6..0000000
--- a/common/src/decode_tcp.c
+++ /dev/null
@@ -1,52 +0,0 @@
-#include "decode_tcp.h"
-
-#define TCP_OPTLENMAX 40
-
-#define TCP_GET_HLEN(tcp_hdr) ((((tcp_hdr)->th_offx2 & 0xf0) >> 4) << 2)
-#define TCP_GET_SRC_PORT(tcp_hdr) ((uint16_t)ntohs((tcp_hdr)->th_sport))
-#define TCP_GET_DST_PORT(tcp_hdr) ((uint16_t)ntohs((tcp_hdr)->th_dport))
-
-int decode_tcp(tcp_info_t *packet, const uint8_t *data, uint32_t len)
-{
-    if (len < TCP_HEADER_LEN)
-    {
-        LOG_ERROR("Parser TCP Header: packet length too small %d", len);
-        return -1;
-    }
-
-    packet->hdr = (tcp_header_t *)data;
-    uint8_t hlen = TCP_GET_HLEN(packet->hdr);
-    if (len < hlen)
-    {
-        LOG_ERROR("Parser TCP Header: TCP packet too small %d", len);
-        return -1;
-    }
-
-    uint8_t tcp_opt_len = hlen - TCP_HEADER_LEN;
-    if (tcp_opt_len > TCP_OPTLENMAX)
-    {
-        LOG_ERROR("Parser TCP Header: invalid opt length %d", tcp_opt_len);
-        return -1;
-    }
-
-    packet->opt_len = tcp_opt_len;
-    packet->src_port = TCP_GET_SRC_PORT(packet->hdr);
-    packet->dst_port = TCP_GET_DST_PORT(packet->hdr);
-
-    packet->hdr_len = hlen;
-    packet->payload = (uint8_t *)data + packet->hdr_len;
-    packet->payload_len = len - packet->hdr_len;
-
-    return 0;
-}
-
-int dump_tcp_info(tcp_info_t *packet, char *buff, size_t size)
-{
-    return snprintf(buff, size,
-                    "{\"src_port\":%u,\"dst_port\":%u,\"hdr_len\":%u,\"opt_len\":%u,\"data_len\":%u}",
-                    packet->src_port,
-                    packet->dst_port,
-                    packet->hdr_len,
-                    packet->opt_len,
-                    packet->payload_len);
-}
\ No newline at end of file
diff --git a/common/src/decode_udp.c b/common/src/decode_udp.c
deleted file mode 100644
index fff1256..0000000
--- a/common/src/decode_udp.c
+++ /dev/null
@@ -1,48 +0,0 @@
-#include "decode_udp.h"
-
-#define UDP_GET_LEN(udp_hdr) ((uint16_t)ntohs((udp_hdr)->udp_len))
-#define UDP_GET_SRC_PORT(udp_hdr) ((uint16_t)ntohs((udp_hdr)->udp_src_port))
-#define UDP_GET_DST_PORT(udp_hdr) ((uint16_t)ntohs((udp_hdr)->udp_dst_port))
-
-int decode_udp(udp_info_t *packet, const uint8_t *data, uint32_t len)
-{
-    if (len < UDP_HEADER_LEN)
-    {
-        LOG_ERROR("Parser UDP Header: packet length too small %d", len);
-        return -1;
-    }
-
-    packet->hdr = (udp_header_t *)data;
-    // 检查 UDP header len
-    if (len < UDP_GET_LEN(packet->hdr))
-    {
-        LOG_ERROR("Parser UDP Header: UDP packet too small %d", len);
-        return -1;
-    }
-
-    // 检查 UDP header len
-    if (len != UDP_GET_LEN(packet->hdr))
-    {
-        LOG_ERROR("Parser UDP Header: invalid UDP header length %d", UDP_GET_LEN(packet->hdr));
-        return -1;
-    }
-
-    packet->src_port = UDP_GET_SRC_PORT(packet->hdr);
-    packet->dst_port = UDP_GET_DST_PORT(packet->hdr);
-
-    packet->hdr_len = UDP_HEADER_LEN;
-    packet->payload = (uint8_t *)data + UDP_HEADER_LEN;
-    packet->payload_len = len - UDP_HEADER_LEN;
-
-    return 0;
-}
-
-int dump_udp_info(udp_info_t *packet, char *buff, size_t size)
-{
-    return snprintf(buff, size,
-                    "{\"src_port\":%u,\"dst_port\":%u,\"hdr_len\":%u,\"data_len\":%u}",
-                    packet->src_port,
-                    packet->dst_port,
-                    packet->hdr_len,
-                    packet->payload_len);
-}
diff --git a/common/src/log.cpp b/common/src/log.cpp
new file mode 100644
index 0000000..5a6a7cf
--- /dev/null
+++ b/common/src/log.cpp
@@ -0,0 +1,33 @@
+#include "log.h"
+
+void *default_logger = NULL;
+
+// return  0 : success
+// return -1 : error
+int LOG_INIT(const char *profile)
+{
+    if (0 != MESA_handle_runtime_log_creation(profile))
+    {
+        fprintf(stderr, "FATAL: unable to create runtime logger\n");
+        return -1;
+    }
+
+    default_logger = MESA_create_runtime_log_handle("packet_adapter", RLOG_LV_DEBUG);
+    if (default_logger == NULL)
+    {
+        fprintf(stderr, "FATAL: unable to create log handle\n");
+        return -1;
+    }
+
+    return 0;
+}
+
+void LOG_CLOSE(void)
+{
+    MESA_handle_runtime_log_destruction();
+}
+
+void LOG_RELOAD(void)
+{
+    MESA_handle_runtime_log_reconstruction(NULL);
+}
diff --git a/common/src/packet_inject.cpp b/common/src/packet_inject.cpp
new file mode 100644
index 0000000..464fefa
--- /dev/null
+++ b/common/src/packet_inject.cpp
@@ -0,0 +1,58 @@
+#include <errno.h>
+#include <string.h>
+#include <unistd.h>
+#include <arpa/inet.h>
+
+#include "log.h"
+
+int packet_inject_ipv4(struct in_addr *ip_dst, const char *data, int len)
+{
+    struct sockaddr_in saddr4 = {0};
+    saddr4.sin_family = PF_INET;
+    memcpy(&saddr4.sin_addr, ip_dst, sizeof(struct in_addr));
+
+    int fd = socket(PF_INET, SOCK_RAW, IPPROTO_RAW);
+    if (fd == -1)
+    {
+        LOG_ERROR("Failed at socket(PF_INET, SOCK_RAW), %d: %s", errno, strerror(errno));
+        return -1;
+    }
+
+    if (sendto(fd, data, len, 0, (struct sockaddr *)&saddr4, sizeof(saddr4)) == -1)
+    {
+        char string[256] = {0};
+        inet_ntop(PF_INET, ip_dst, string, sizeof(string));
+        LOG_ERROR("Failed at send() %s, (%d: %s)", string, errno, strerror(errno));
+        close(fd);
+        return -1;
+    }
+
+    close(fd);
+    return 0;
+}
+
+int packet_inject_ipv6(struct in6_addr *ip6_dst, const char *data, int len)
+{
+    struct sockaddr_in6 saddr6 = {0};
+    saddr6.sin6_family = PF_INET6;
+    memcpy(&saddr6.sin6_addr, ip6_dst, sizeof(struct in6_addr));
+
+    int fd = socket(PF_INET6, SOCK_RAW, IPPROTO_RAW);
+    if (fd == -1)
+    {
+        LOG_ERROR("Failed at socket(PF_INET6, SOCK_RAW), %d: %s", errno, strerror(errno));
+        return -1;
+    }
+
+    if (sendto(fd, data, len, 0, (struct sockaddr *)&saddr6, sizeof(saddr6)) == -1)
+    {
+        char string[256] = {0};
+        inet_ntop(PF_INET6, ip6_dst, string, sizeof(string));
+        LOG_ERROR("Failed at send() %s, (%d: %s)", string, errno, strerror(errno));
+        close(fd);
+        return -1;
+    }
+
+    close(fd);
+    return 0;
+}
diff --git a/common/src/packet_io.cpp b/common/src/packet_io.cpp
new file mode 100644
index 0000000..0209514
--- /dev/null
+++ b/common/src/packet_io.cpp
@@ -0,0 +1,280 @@
+#include <sched.h>
+#include <stdlib.h>
+#include <string.h>
+#include <assert.h>
+#include <netinet/ether.h>
+#include <MESA/MESA_prof_load.h>
+
+#include "log.h"
+#include "packet_io.h"
+
+#define MAX_RX_BURST 128
+
+#define LOG_PKTIO "PACKET_IO"
+#define MIN(a, b) ((a) > (b) ? (b) : (a))
+
+struct config
+{
+    int thread_num;
+    int cpu_mask[MAX_THREAD_NUM];
+
+    int rx_burst_max;
+    int bypass_traffic;
+
+    char app_symbol[256];
+    char app_device[256];
+};
+
+struct packet_io
+{
+    struct config config;
+
+    struct mr_vdev *mr_dev;
+    struct mr_sendpath *mr_path;
+    struct mr_instance *mr_instance;
+
+    void *cb_args;
+    packet_handle_cb *callback;
+};
+
+static int packet_io_config(const char *profile, struct config *config)
+{
+    MESA_load_profile_int_def(profile, "PACKET_IO", "thread_num", (int *)&(config->thread_num), 1);
+    MESA_load_profile_uint_range(profile, "PACKET_IO", "cpu_mask", MAX_THREAD_NUM, (unsigned int *)config->cpu_mask);
+
+    MESA_load_profile_int_def(profile, "PACKET_IO", "rx_burst_max", (int *)&(config->rx_burst_max), 1);
+    MESA_load_profile_int_def(profile, "PACKET_IO", "bypass_traffic", (int *)&(config->bypass_traffic), 0);
+
+    MESA_load_profile_string_nodef(profile, "PACKET_IO", "app_symbol", config->app_symbol, sizeof(config->app_symbol));
+    MESA_load_profile_string_nodef(profile, "PACKET_IO", "app_device", config->app_device, sizeof(config->app_device));
+
+    config->thread_num = MIN(config->thread_num, MAX_THREAD_NUM);
+
+    if (config->rx_burst_max > MAX_RX_BURST)
+    {
+        LOG_ERROR("%s: invalid rx_burst_max, exceeds limit %d", LOG_PKTIO, MAX_RX_BURST);
+        return -1;
+    }
+
+    if (strlen(config->app_symbol) == 0)
+    {
+        LOG_ERROR("%s: invalid app_symbol in %s", LOG_PKTIO, profile);
+        return -1;
+    }
+
+    if (strlen(config->app_device) == 0)
+    {
+        LOG_ERROR("%s: invalid app_device in %s", LOG_PKTIO, profile);
+        return -1;
+    }
+
+    LOG_DEBUG("%s: PACKET_IO->thread_num        : %d", LOG_PKTIO, config->thread_num);
+    LOG_DEBUG("%s: PACKET_IO->rx_burst_max      : %d", LOG_PKTIO, config->rx_burst_max);
+    LOG_DEBUG("%s: PACKET_IO->bypass_traffic    : %d", LOG_PKTIO, config->bypass_traffic);
+    LOG_DEBUG("%s: PACKET_IO->app_symbol        : %s", LOG_PKTIO, config->app_symbol);
+    LOG_DEBUG("%s: PACKET_IO->app_device        : %s", LOG_PKTIO, config->app_device);
+
+    return 0;
+}
+
+static int marsio_buff_is_keepalive(char *raw_data, int raw_len)
+{
+    if (raw_data == NULL || raw_len < (int)(sizeof(struct ethhdr)))
+    {
+        return 0;
+    }
+
+    struct ethhdr *eth_hdr = (struct ethhdr *)raw_data;
+    if (eth_hdr->h_proto == 0xAAAA)
+    {
+        return 1;
+    }
+    else
+    {
+        return 0;
+    }
+}
+
+struct packet_io *packet_io_create(const char *profile)
+{
+    int opt = 1;
+    cpu_set_t coremask;
+    struct packet_io *handle = (struct packet_io *)calloc(1, sizeof(struct packet_io));
+    assert(handle != NULL);
+
+    if (packet_io_config(profile, &(handle->config)) != 0)
+    {
+        goto error_out;
+    }
+
+    CPU_ZERO(&coremask);
+    for (int i = 0; i < handle->config.thread_num; i++)
+    {
+        CPU_SET(handle->config.cpu_mask[i], &coremask);
+    }
+
+    handle->mr_instance = marsio_create();
+    if (handle->mr_instance == NULL)
+    {
+        LOG_ERROR("%s: unable to create marsio instance", LOG_PKTIO);
+        goto error_out;
+    }
+
+    if (marsio_option_set(handle->mr_instance, MARSIO_OPT_THREAD_MASK_IN_CPUSET, &coremask, sizeof(cpu_set_t)) != 0)
+    {
+        LOG_ERROR("%s: unable to set MARSIO_OPT_EXIT_WHEN_ERR option for marsio instance", LOG_PKTIO);
+        goto error_out;
+    }
+
+    if (marsio_option_set(handle->mr_instance, MARSIO_OPT_EXIT_WHEN_ERR, &opt, sizeof(opt)) != 0)
+    {
+        LOG_ERROR("%s: unable to set MARSIO_OPT_EXIT_WHEN_ERR option for marsio instance", LOG_PKTIO);
+        goto error_out;
+    }
+
+    if (marsio_init(handle->mr_instance, handle->config.app_symbol) != 0)
+    {
+        LOG_ERROR("%s: unable to initialize marsio instance", LOG_PKTIO);
+        goto error_out;
+    }
+
+    handle->mr_dev = marsio_open_device(handle->mr_instance, handle->config.app_device, handle->config.thread_num, handle->config.thread_num);
+    if (handle->mr_dev == NULL)
+    {
+        LOG_ERROR("%s: unable to open device %s", LOG_PKTIO, handle->config.app_device);
+        goto error_out;
+    }
+
+    handle->mr_path = marsio_sendpath_create_by_vdev(handle->mr_dev);
+    if (handle->mr_path == NULL)
+    {
+        LOG_ERROR("%s: unable to create sendpath for device %s", LOG_PKTIO, handle->config.app_device);
+        goto error_out;
+    }
+
+    return handle;
+
+error_out:
+    packet_io_destory(handle);
+    return NULL;
+}
+
+void packet_io_destory(struct packet_io *handle)
+{
+    if (handle)
+    {
+        if (handle->mr_path)
+        {
+            marsio_sendpath_destory(handle->mr_path);
+            handle->mr_path = NULL;
+        }
+
+        if (handle->mr_dev)
+        {
+            marsio_close_device(handle->mr_dev);
+            handle->mr_dev = NULL;
+        }
+
+        if (handle->mr_instance)
+        {
+            marsio_destory(handle->mr_instance);
+            handle->mr_instance = NULL;
+        }
+
+        free(handle);
+        handle = NULL;
+    }
+}
+
+void packet_io_set_callback(struct packet_io *handle, packet_handle_cb *cb, void *args)
+{
+    handle->callback = cb;
+    handle->cb_args = args;
+}
+
+int packet_io_thread_init(struct packet_io *handle, int thread_index)
+{
+    if (marsio_thread_init(handle->mr_instance) != 0)
+    {
+        LOG_ERROR("%s: unable to init marsio thread %d", LOG_PKTIO, thread_index);
+        return -1;
+    }
+
+    return 0;
+}
+
+void packet_io_thread_wait(struct packet_io *handle, int thread_index, int timeout_ms)
+{
+    struct mr_vdev *vdevs[] = {
+        handle->mr_dev,
+    };
+
+    marsio_poll_wait(handle->mr_instance, vdevs, 1, thread_index, timeout_ms);
+}
+
+int packet_io_thread_polling(struct packet_io *handle, int thread_index)
+{
+    int raw_len;
+    char *raw_data;
+    marsio_buff_t *rx_buff;
+    marsio_buff_t *rx_buffs[MAX_RX_BURST];
+    int nr_recv = marsio_recv_burst(handle->mr_dev, thread_index, rx_buffs, handle->config.rx_burst_max);
+    if (nr_recv <= 0)
+    {
+        return 0;
+    }
+
+    if (handle->config.bypass_traffic == 1)
+    {
+        marsio_send_burst(handle->mr_path, thread_index, rx_buffs, nr_recv);
+        return nr_recv;
+    }
+
+    for (int j = 0; j < nr_recv; j++)
+    {
+        rx_buff = rx_buffs[j];
+        raw_len = marsio_buff_datalen(rx_buff);
+        raw_data = marsio_buff_mtod(rx_buff);
+
+        if (marsio_buff_is_keepalive(raw_data, raw_len))
+        {
+            marsio_send_burst(handle->mr_path, thread_index, &rx_buff, 1);
+        }
+        else if (marsio_buff_is_ctrlbuf(rx_buff))
+        {
+            marsio_send_burst(handle->mr_path, thread_index, &rx_buff, 1);
+        }
+        else
+        {
+            if (handle->callback)
+            {
+                if (handle->callback(raw_data, raw_len, handle->cb_args) == ACTION_BYPASS)
+                {
+                    marsio_send_burst(handle->mr_path, thread_index, &rx_buff, 1);
+                }
+                else
+                {
+                    marsio_buff_free(handle->mr_instance, &rx_buff, 1, 0, thread_index);
+                }
+            }
+            else
+            {
+                marsio_send_burst(handle->mr_path, thread_index, &rx_buff, 1);
+            }
+        }
+    }
+
+    return nr_recv;
+}
+
+int packet_io_thread_number(struct packet_io *handle)
+{
+    if (handle)
+    {
+        return handle->config.thread_num;
+    }
+    else
+    {
+        return 0;
+    }
+}
\ No newline at end of file
diff --git a/common/src/packet_parser.cpp b/common/src/packet_parser.cpp
new file mode 100644
index 0000000..6e2e2c3
--- /dev/null
+++ b/common/src/packet_parser.cpp
@@ -0,0 +1,668 @@
+#include <string.h>
+#include <netinet/ip.h>
+#include <netinet/ip6.h>
+#define __FAVOR_BSD 1
+#include <netinet/tcp.h>
+#include <netinet/ether.h>
+
+#include "log.h"
+#include "packet_parser.h"
+
+#define LOG_PKT_PARSER "PACKET_PARSER"
+
+/******************************************************************************
+ * Struct
+ ******************************************************************************/
+
+struct udp_hdr
+{
+    uint16_t uh_sport; /* source port */
+    uint16_t uh_dport; /* destination port */
+    uint16_t uh_ulen;  /* udp length */
+    uint16_t uh_sum;   /* udp checksum */
+} __attribute__((__packed__));
+
+struct vlan_hdr
+{
+    uint16_t vlan_cfi;
+    uint16_t protocol;
+} __attribute__((__packed__));
+
+struct vxlan_hdr
+{
+    uint8_t flags[2];
+    uint16_t gdp; // group policy id
+    uint8_t vni[3];
+    uint8_t reserved;
+} __attribute__((__packed__));
+
+struct gtp_hdr
+{
+#if __BYTE_ORDER == __LITTLE_ENDIAN
+    unsigned char flags;
+    unsigned char msg_type;
+    unsigned short len;
+    unsigned int teid;
+#elif __BYTE_ORDER == __BIG_ENDIAN
+    unsigned int teid;
+    unsigned short len;
+    unsigned char msg_type;
+    unsigned char flags;
+#else
+#error "Please check <endian.h>"
+#endif
+} __attribute__((__packed__));
+
+#define GTP_HDR_VER_MASK (0xE0)
+#define GTP_HDR_FLAG_N_PDU (0x01)
+#define GTP_HDR_FLAG_SEQ_NUM (0x02)
+#define GTP_HDR_FLAG_NEXT_EXT_HDR (0x04)
+
+/******************************************************************************
+ * Static API
+ ******************************************************************************/
+
+static int packet_parser_push(struct packet_parser *handler, enum layer_type type, uint16_t hdr_offset, uint16_t hdr_len, uint16_t payload_len);
+
+// parser utils
+static const char *layer2str(enum layer_type type);
+static uint16_t parse_gtphdr_len(const struct gtp_hdr *gtph);
+
+// parser protocol
+static const void *parse_ether(struct packet_parser *handler, const void *data, uint16_t length);
+static const void *parse_ipv4(struct packet_parser *handler, const void *data, uint16_t length);
+static const void *parse_ipv6(struct packet_parser *handler, const void *data, uint16_t length);
+static const void *parse_tcp(struct packet_parser *handler, const void *data, uint16_t length);
+static const void *parse_udp(struct packet_parser *handler, const void *data, uint16_t length);
+static const void *parse_pppoe_ses(struct packet_parser *handler, const void *data, uint16_t length);
+static const void *parse_vxlan(struct packet_parser *handler, const void *data, uint16_t length);
+static const void *parse_vlan8021q(struct packet_parser *handler, const void *data, uint16_t length);
+static const void *parse_gtpv1_u(struct packet_parser *handler, const void *data, uint16_t length);
+static const void *parse_mpls(struct packet_parser *handler, const void *data, uint16_t length);
+
+/******************************************************************************
+ * Public API
+ ******************************************************************************/
+
+void packet_parser_init(struct packet_parser *handler)
+{
+    memset(handler, 0, sizeof(struct packet_parser));
+
+    handler->result.used = 0;
+    handler->result.size = sizeof(handler->result.layers) / sizeof(handler->result.layers[0]);
+    handler->packet_data = NULL;
+    handler->packet_len = 0;
+    handler->packet_id = 0;
+}
+
+// return most inner payload
+const void *packet_parser_parse(struct packet_parser *handler, const void *packet_data, uint16_t packet_len, uint64_t packet_id)
+{
+    handler->packet_data = packet_data;
+    handler->packet_len = packet_len;
+    handler->packet_id = packet_id;
+
+    // TESTED OK BY LWP
+    return parse_ether(handler, handler->packet_data, handler->packet_len);
+}
+
+const struct layer_record *packet_parser_get_most_inner(struct packet_parser *handler, enum layer_type type)
+{
+    struct parser_result *result = &handler->result;
+    for (int16_t i = result->used - 1; i >= 0; i--)
+    {
+        const struct layer_record *layer = &result->layers[i];
+        if (layer->type & type)
+        {
+            return layer;
+        }
+    }
+
+    return NULL;
+}
+
+const struct layer_record *packet_parser_get_most_outer(struct packet_parser *handler, enum layer_type type)
+{
+    struct parser_result *result = &handler->result;
+    for (int16_t i = 0; i <= result->used - 1; i++)
+    {
+        const struct layer_record *layer = &result->layers[i];
+        if (layer->type & type)
+        {
+            return layer;
+        }
+    }
+
+    return NULL;
+}
+
+// return 4: IPv4
+// return 6: IPv6
+uint8_t gtp_next_proto(const char *data)
+{
+    uint16_t hdr_len = parse_gtphdr_len((const struct gtp_hdr *)data);
+    if (hdr_len < 0)
+    {
+        return 0;
+    }
+
+    uint8_t next_proto = (((const uint8_t *)(data + hdr_len))[0]) >> 4;
+    return next_proto;
+}
+
+/******************************************************************************
+ * Private API
+ ******************************************************************************/
+
+static int packet_parser_push(struct packet_parser *handler, enum layer_type type, uint16_t hdr_offset, uint16_t hdr_len, uint16_t payload_len)
+{
+    struct parser_result *result = &handler->result;
+    if (result->used >= result->size)
+    {
+        return -1;
+    }
+
+    result->layers[result->used].type = type;
+    result->layers[result->used].hdr_offset = hdr_offset;
+    result->layers[result->used].hdr_len = hdr_len;
+    result->layers[result->used].pld_len = payload_len;
+    result->used++;
+
+    return 0;
+}
+
+static const char *layer2str(enum layer_type type)
+{
+    switch (type)
+    {
+    case LAYER_TYPE_ETHER:
+        return "ETHER";
+    case LAYER_TYPE_PPP:
+        return "PPP";
+    case LAYER_TYPE_HDLC:
+        return "HDLC";
+    case LAYER_TYPE_VLAN:
+        return "VLAN";
+    case LAYER_TYPE_PPPOE:
+        return "PPPOE";
+    case LAYER_TYPE_MPLS:
+        return "MPLS";
+    case LAYER_TYPE_IPV4:
+        return "IPV4";
+    case LAYER_TYPE_IPV6:
+        return "IPV6";
+    case LAYER_TYPE_UDP:
+        return "UDP";
+    case LAYER_TYPE_TCP:
+        return "TCP";
+    case LAYER_TYPE_G_VXLAN:
+        return "G_VXLAN";
+    case LAYER_TYPE_GTPV1_U:
+        return "GTPV1_U";
+    default:
+        return "UNKNOWN";
+    }
+}
+
+// FROM SAPP
+static uint16_t parse_gtphdr_len(const struct gtp_hdr *gtph)
+{
+    const unsigned char *p_ext_hdr = (unsigned char *)gtph + sizeof(struct gtp_hdr);
+    unsigned char next_hdr_type;
+    unsigned char this_ext_field_cont_len;
+
+    // v0 太古老已废弃，目前仅支持 GTPv1 版本
+    if (((gtph->flags & GTP_HDR_VER_MASK) >> 5) != 1)
+    {
+        return -1;
+    }
+
+    if (gtph->flags & (GTP_HDR_FLAG_SEQ_NUM | GTP_HDR_FLAG_N_PDU | GTP_HDR_FLAG_NEXT_EXT_HDR))
+    {
+        // skip seq field (2 bytes)
+        p_ext_hdr += 2;
+
+        // skip N-PDU field (1 byte)
+        p_ext_hdr++;
+
+        // 解析 GTP 扩展头部字段，参考 wireshark 源码 packet-gtp.c->dissect_gtp_common()
+        next_hdr_type = *p_ext_hdr;
+        if (gtph->flags & GTP_HDR_FLAG_NEXT_EXT_HDR)
+        {
+            while (next_hdr_type != 0)
+            {
+                // 指向长度字段, 以4个字节为单位
+                p_ext_hdr++;
+                this_ext_field_cont_len = *p_ext_hdr * 4 - 2;
+
+                // 指向数据部分第一个字节
+                p_ext_hdr++;
+                p_ext_hdr += this_ext_field_cont_len;
+
+                // 指向下一个头部字段
+                next_hdr_type = *p_ext_hdr;
+                p_ext_hdr++;
+            }
+        }
+        else
+        {
+            p_ext_hdr++;
+        }
+    }
+
+    return (char *)p_ext_hdr - (char *)gtph;
+}
+
+static const void *parse_ether(struct packet_parser *handler, const void *data, uint16_t length)
+{
+    if (length < sizeof(struct ethhdr))
+    {
+        LOG_ERROR("%s: trace id: %lu, layer: %s, err: data not enough", LOG_PKT_PARSER, handler->packet_id, layer2str(LAYER_TYPE_ETHER));
+        return data;
+    }
+
+    struct ethhdr *hdr = (struct ethhdr *)data;
+    uint16_t next_proto = ntohs(hdr->h_proto);
+    uint16_t hdr_len = sizeof(struct ethhdr);
+    uint16_t hdr_offset = (uintptr_t)data - (uintptr_t)(handler->packet_data);
+    uint16_t payload_len = length - hdr_len;
+    const void *payload_data = (const char *)data + hdr_len;
+
+    if (packet_parser_push(handler, LAYER_TYPE_ETHER, hdr_offset, hdr_len, payload_len) != 0)
+    {
+        return data;
+    }
+
+    LOG_DEBUG("%s: trace id: %lu, layer: %s, payload len: [%u/%u]", LOG_PKT_PARSER, handler->packet_id, layer2str(LAYER_TYPE_ETHER), payload_len, length);
+    switch (next_proto)
+    {
+    case ETH_P_8021Q:
+        // TESTED OK BY LWP
+        return parse_vlan8021q(handler, payload_data, payload_len);
+    case ETH_P_8021AD:
+        return parse_ether(handler, payload_data, payload_len);
+    case ETH_P_IP:
+        // TESTED OK BY LWP
+        return parse_ipv4(handler, payload_data, payload_len);
+    case ETH_P_IPV6:
+        // TESTED OK BY LWP
+        return parse_ipv6(handler, payload_data, payload_len);
+    case ETH_P_PPP_SES:
+        return parse_pppoe_ses(handler, payload_data, payload_len);
+    case ETH_P_MPLS_UC:
+        // TESTED OK BY LWP
+        return parse_mpls(handler, payload_data, payload_len);
+    default:
+        LOG_ERROR("%s: trace id: %lu, layer: %s, stop parse next protocol %d", LOG_PKT_PARSER, handler->packet_id, layer2str(LAYER_TYPE_ETHER), next_proto);
+        return payload_data;
+    }
+}
+
+static const void *parse_ipv4(struct packet_parser *handler, const void *data, uint16_t length)
+{
+    if (length < sizeof(struct ip))
+    {
+        LOG_ERROR("%s: trace id: %lu, layer: %s, err: data not enough", LOG_PKT_PARSER, handler->packet_id, layer2str(LAYER_TYPE_IPV4));
+        return data;
+    }
+
+    struct ip *hdr = (struct ip *)data;
+    uint16_t next_proto = hdr->ip_p;
+    uint16_t hdr_len = (hdr->ip_hl & 0xf) * 4u;
+    uint16_t hdr_offset = (uintptr_t)data - (uintptr_t)(handler->packet_data);
+    uint16_t payload_len = length - hdr_len;
+    const void *payload_data = (const char *)data + hdr_len;
+
+    if (packet_parser_push(handler, LAYER_TYPE_IPV4, hdr_offset, hdr_len, payload_len) != 0)
+    {
+        return data;
+    }
+
+    LOG_DEBUG("%s: trace id: %lu, layer: %s, payload len: [%u/%u]", LOG_PKT_PARSER, handler->packet_id, layer2str(LAYER_TYPE_IPV4), payload_len, length);
+    switch (next_proto)
+    {
+    case IPPROTO_TCP:
+        // TESTED OK BY LWP
+        return parse_tcp(handler, payload_data, payload_len);
+    case IPPROTO_UDP:
+        // TESTED OK BY LWP
+        return parse_udp(handler, payload_data, payload_len);
+    case IPPROTO_IPIP:
+        // TESTED OK BY LWP
+        return parse_ipv4(handler, payload_data, payload_len);
+    case IPPROTO_IPV6:
+        // TESTED OK BY LWP
+        return parse_ipv6(handler, payload_data, payload_len);
+    default:
+        // TODO GRE
+        LOG_ERROR("%s: trace id: %lu, layer: %s, stop parse next protocol %d", LOG_PKT_PARSER, handler->packet_id, layer2str(LAYER_TYPE_IPV4), next_proto);
+        return payload_data;
+    }
+}
+
+static const void *parse_ipv6(struct packet_parser *handler, const void *data, uint16_t length)
+{
+    if (length < sizeof(struct ip6_hdr))
+    {
+        LOG_ERROR("%s: trace id: %lu, layer: %s, err: data not enough", LOG_PKT_PARSER, handler->packet_id, layer2str(LAYER_TYPE_IPV6));
+        return data;
+    }
+
+    struct ip6_hdr *hdr = (struct ip6_hdr *)data;
+    uint16_t next_proto = hdr->ip6_nxt;
+    uint16_t hdr_len = sizeof(struct ip6_hdr);
+    uint16_t hdr_offset = (uintptr_t)data - (uintptr_t)(handler->packet_data);
+    uint16_t payload_len = length - hdr_len;
+    const void *payload_data = (const char *)data + hdr_len;
+
+    if (packet_parser_push(handler, LAYER_TYPE_IPV6, hdr_offset, hdr_len, payload_len) != 0)
+    {
+        return data;
+    }
+
+    LOG_DEBUG("%s: trace id: %lu, layer: %s, payload len: [%u/%u]", LOG_PKT_PARSER, handler->packet_id, layer2str(LAYER_TYPE_IPV6), payload_len, length);
+    switch (next_proto)
+    {
+    case IPPROTO_TCP:
+        // TESTED OK BY LWP
+        return parse_tcp(handler, payload_data, payload_len);
+    case IPPROTO_UDP:
+        // TESTED OK BY LWP
+        return parse_udp(handler, payload_data, payload_len);
+    case IPPROTO_IPIP:
+        // TESTED OK BY LWP
+        return parse_ipv4(handler, payload_data, payload_len);
+    case IPPROTO_IPV6:
+        // TESTED OK BY LWP
+        return parse_ipv6(handler, payload_data, payload_len);
+    default:
+        LOG_ERROR("%s: trace id: %lu, layer: %s, stop parse next protocol %d", LOG_PKT_PARSER, handler->packet_id, layer2str(LAYER_TYPE_IPV6), next_proto);
+        return payload_data;
+    }
+}
+
+static const void *parse_tcp(struct packet_parser *handler, const void *data, uint16_t length)
+{
+    if (length < sizeof(struct tcphdr))
+    {
+        LOG_ERROR("%s: trace id: %lu, layer: %s, err: data not enough", LOG_PKT_PARSER, handler->packet_id, layer2str(LAYER_TYPE_TCP));
+        return data;
+    }
+
+    struct tcphdr *hdr = (struct tcphdr *)data;
+    uint16_t hdr_len = hdr->th_off << 2;
+    uint16_t hdr_offset = (uintptr_t)data - (uintptr_t)(handler->packet_data);
+    uint16_t payload_len = length - hdr_len;
+    const void *payload_data = (const char *)data + hdr_len;
+
+    if (packet_parser_push(handler, LAYER_TYPE_TCP, hdr_offset, hdr_len, payload_len) != 0)
+    {
+        return data;
+    }
+
+    LOG_DEBUG("%s: trace id: %lu, layer: %s, payload len: [%u/%u]", LOG_PKT_PARSER, handler->packet_id, layer2str(LAYER_TYPE_TCP), payload_len, length);
+
+    // TESTED OK BY LWP
+    return payload_data;
+}
+
+static const void *parse_udp(struct packet_parser *handler, const void *data, uint16_t length)
+{
+    if (length < sizeof(struct udp_hdr))
+    {
+        LOG_ERROR("%s: trace id: %lu, layer: %s, err: data not enough", LOG_PKT_PARSER, handler->packet_id, layer2str(LAYER_TYPE_UDP));
+        return data;
+    }
+
+    struct udp_hdr *hdr = (struct udp_hdr *)data;
+    uint16_t hdr_len = sizeof(struct udp_hdr);
+    uint16_t hdr_offset = (uintptr_t)data - (uintptr_t)(handler->packet_data);
+    uint16_t payload_len = length - hdr_len;
+    const void *payload_data = (const char *)data + hdr_len;
+
+    if (packet_parser_push(handler, LAYER_TYPE_UDP, hdr_offset, hdr_len, payload_len) != 0)
+    {
+        return data;
+    }
+
+    LOG_DEBUG("%s: trace id: %lu, layer: %s, payload len: [%u/%u]", LOG_PKT_PARSER, handler->packet_id, layer2str(LAYER_TYPE_UDP), payload_len, length);
+    switch (ntohs(hdr->uh_dport))
+    {
+    case 4789: // VXLAN_DPORT
+        // TESTED OK BY LWP
+        return parse_vxlan(handler, payload_data, payload_len);
+    case 2152: // GTP1U_PORT
+        // TESTED OK BY LWP
+        return parse_gtpv1_u(handler, payload_data, payload_len);
+    default:
+        // TESTED OK BY LWP
+        return payload_data;
+    }
+}
+
+static const void *parse_pppoe_ses(struct packet_parser *handler, const void *data, uint16_t length)
+{
+    if (length < 8)
+    {
+        LOG_ERROR("%s: trace id: %lu, layer: %s, err: data not enough", LOG_PKT_PARSER, handler->packet_id, layer2str(LAYER_TYPE_PPPOE));
+        return data;
+    }
+
+    uint16_t next_proto = *((uint16_t *)data + 3);
+    uint16_t hdr_len = 8;
+    uint16_t hdr_offset = (uintptr_t)data - (uintptr_t)(handler->packet_data);
+    uint16_t payload_len = length - hdr_len;
+    const void *payload_data = (const char *)data + hdr_len;
+
+    if (packet_parser_push(handler, LAYER_TYPE_PPPOE, hdr_offset, hdr_len, payload_len) != 0)
+    {
+        return data;
+    }
+
+    LOG_DEBUG("%s: trace id: %lu, layer: %s, payload len: [%u/%u]", LOG_PKT_PARSER, handler->packet_id, layer2str(LAYER_TYPE_PPPOE), payload_len, length);
+    switch (next_proto)
+    {
+    case 0x2100: // PPPOE_TYPE_IPV4
+        // TESTED OK BY LWP
+        return parse_ipv4(handler, payload_data, payload_len);
+    case 0x5700: // PPPOE_TYPE_IPV6
+        return parse_ipv6(handler, payload_data, payload_len);
+    default:
+        LOG_ERROR("%s: trace id: %lu, layer: %s, stop parse next protocol %d", LOG_PKT_PARSER, handler->packet_id, layer2str(LAYER_TYPE_PPPOE), next_proto);
+        return payload_data;
+    }
+}
+
+static const void *parse_vxlan(struct packet_parser *handler, const void *data, uint16_t length)
+{
+    if (length < sizeof(struct vxlan_hdr))
+    {
+        LOG_ERROR("%s: trace id: %lu, layer: %s, err: data not enough", LOG_PKT_PARSER, handler->packet_id, layer2str(LAYER_TYPE_G_VXLAN));
+        return NULL;
+    }
+
+    // struct vxlan_hdr *vxlan_hdr = (struct vxlan_hdr *)data;
+    uint16_t hdr_len = sizeof(struct vxlan_hdr);
+    uint16_t hdr_offset = (uintptr_t)data - (uintptr_t)(handler->packet_data);
+    uint16_t payload_len = length - hdr_len;
+    const void *payload_data = (const char *)data + hdr_len;
+
+    if (packet_parser_push(handler, LAYER_TYPE_G_VXLAN, hdr_offset, hdr_len, payload_len) != 0)
+    {
+        return data;
+    }
+
+    LOG_DEBUG("%s: trace id: %lu, layer: %s, payload len: [%u/%u]", LOG_PKT_PARSER, handler->packet_id, layer2str(LAYER_TYPE_G_VXLAN), payload_len, length);
+    // TESTED OK BY LWP
+    return parse_ether(handler, payload_data, payload_len);
+}
+
+static const void *parse_vlan8021q(struct packet_parser *handler, const void *data, uint16_t length)
+{
+    if (length < sizeof(struct vlan_hdr))
+    {
+        LOG_ERROR("%s: trace id: %lu, layer: %s, err: data not enough", LOG_PKT_PARSER, handler->packet_id, layer2str(LAYER_TYPE_VLAN));
+        return NULL;
+    }
+
+    struct vlan_hdr *hdr = (struct vlan_hdr *)data;
+    uint16_t next_proto = ntohs(hdr->protocol);
+    uint16_t hdr_len = sizeof(struct vlan_hdr);
+    uint16_t hdr_offset = (uintptr_t)data - (uintptr_t)(handler->packet_data);
+    uint16_t payload_len = length - hdr_len;
+    const void *payload_data = (const char *)data + hdr_len;
+
+    if (packet_parser_push(handler, LAYER_TYPE_VLAN, hdr_offset, hdr_len, payload_len) != 0)
+    {
+        return data;
+    }
+
+    LOG_DEBUG("%s: trace id: %lu, layer: %s, payload len: [%u/%u]", LOG_PKT_PARSER, handler->packet_id, layer2str(LAYER_TYPE_VLAN), payload_len, length);
+    switch (next_proto)
+    {
+    case ETH_P_8021Q:
+        // TESTED OK BY LWP
+        return parse_vlan8021q(handler, payload_data, payload_len);
+    case ETH_P_IP:
+        // TESTED OK BY LWP
+        return parse_ipv4(handler, payload_data, payload_len);
+    case ETH_P_IPV6:
+        // TESTED OK BY LWP
+        return parse_ipv6(handler, payload_data, payload_len);
+    case ETH_P_PPP_SES:
+        // TESTED OK BY LWP
+        return parse_pppoe_ses(handler, payload_data, payload_len);
+    case ETH_P_MPLS_UC:
+        return parse_mpls(handler, payload_data, payload_len);
+    default:
+        LOG_ERROR("%s: trace id: %lu, layer: %s, stop parse next protocol %d", LOG_PKT_PARSER, handler->packet_id, layer2str(LAYER_TYPE_VLAN), next_proto);
+        return payload_data;
+    }
+}
+
+static const void *parse_gtpv1_u(struct packet_parser *handler, const void *data, uint16_t length)
+{
+    if (length < sizeof(struct gtp_hdr))
+    {
+        LOG_ERROR("%s: trace id: %lu, layer: %s, err: data not enough", LOG_PKT_PARSER, handler->packet_id, layer2str(LAYER_TYPE_GTPV1_U));
+        return NULL;
+    }
+
+    uint16_t hdr_len = parse_gtphdr_len((const struct gtp_hdr *)data);
+    if (hdr_len < 0)
+    {
+        return data;
+    }
+
+    uint8_t next_proto = (((const uint8_t *)((const char *)data + hdr_len))[0]) >> 4;
+    uint16_t hdr_offset = (uintptr_t)data - (uintptr_t)(handler->packet_data);
+    uint16_t payload_len = length - hdr_len;
+    const void *payload_data = (const char *)data + hdr_len;
+
+    if (packet_parser_push(handler, LAYER_TYPE_GTPV1_U, hdr_offset, hdr_len, payload_len) != 0)
+    {
+        return data;
+    }
+
+    LOG_DEBUG("%s: trace id: %lu, layer: %s, payload len: [%u/%u]", LOG_PKT_PARSER, handler->packet_id, layer2str(LAYER_TYPE_GTPV1_U), payload_len, length);
+    switch (next_proto)
+    {
+    case 4:
+        // TESTED OK BY LWP
+        return parse_ipv4(handler, payload_data, payload_len);
+    case 6:
+        // TESTED OK BY LWP
+        return parse_ipv6(handler, payload_data, payload_len);
+    default:
+        LOG_ERROR("%s: trace id: %lu, layer: %s, stop parse next protocol %d", LOG_PKT_PARSER, handler->packet_id, layer2str(LAYER_TYPE_GTPV1_U), next_proto);
+        return payload_data;
+    }
+}
+
+static const void *parse_mpls(struct packet_parser *handler, const void *data, uint16_t length)
+{
+    if (length < 4)
+    {
+        LOG_ERROR("%s: trace id: %lu, layer: %s, err: data not enough", LOG_PKT_PARSER, handler->packet_id, layer2str(LAYER_TYPE_MPLS));
+        return data;
+    }
+
+#define MPLS_LABEL_MASK (0xFFFFF000)
+#define MPLS_EXP_MASK (0x00000E00)
+#define MPLS_BLS_MASK (0x00000100)
+#define MPLS_TTL_MASK (0x000000FF)
+
+    /*
+     * MPLS Format
+     * 0                   1                   2                   3
+     * 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
+     * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+     * |                Label                  | Exp |S|       TTL     |
+     * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+     * Label : Label Value      20 bits
+     * Exp   : Experimental Use  3 bits
+     * S     : Bottom of Stack   1 bit
+     * TTL   : Time to Live      8 bits
+     */
+
+    uint32_t *hdr = (uint32_t *)data;
+    // unsigned int mpls_label = (ntohl(*hdr) & MPLS_LABEL_MASK) >> 12;
+    // unsigned int mpls_exp = (ntohl(*hdr) & MPLS_EXP_MASK) >> 9;
+    unsigned int mpls_bls = (ntohl(*hdr) & MPLS_BLS_MASK) >> 8;
+    // unsigned int mpls_ttl = (ntohl(*hdr) & MPLS_TTL_MASK);
+
+    uint16_t hdr_len = 4;
+    uint16_t hdr_offset = (uintptr_t)data - (uintptr_t)(handler->packet_data);
+    uint16_t payload_len = length - hdr_len;
+    const void *payload_data = (const char *)data + hdr_len;
+
+    if (packet_parser_push(handler, LAYER_TYPE_MPLS, hdr_offset, hdr_len, payload_len) != 0)
+    {
+        return data;
+    }
+
+    if (mpls_bls == 1)
+    {
+        uint8_t ip_version = (((uint8_t *)payload_data)[0]) >> 4;
+        if (ip_version == 0)
+        {
+            /*
+             * PW Ethernet Control Word
+             * 0                   1                   2                   3
+             * 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
+             * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+             * |0 0 0 0|   Reserved            |       Sequence Number         |
+             * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+             * Reference: https://tools.ietf.org/html/rfc4448
+             */
+            payload_data = (const char *)payload_data + 4;
+            payload_len = payload_len - 4;
+
+            LOG_DEBUG("%s: trace id: %lu, layer: %s, payload len: [%u/%u]", LOG_PKT_PARSER, handler->packet_id, layer2str(LAYER_TYPE_MPLS), payload_len, length);
+            return parse_ether(handler, payload_data, payload_len);
+        }
+        else if (ip_version == 4)
+        {
+            LOG_DEBUG("%s: trace id: %lu, layer: %s, payload len: [%u/%u]", LOG_PKT_PARSER, handler->packet_id, layer2str(LAYER_TYPE_MPLS), payload_len, length);
+            // TESTED OK BY LWP
+            return parse_ipv4(handler, payload_data, payload_len);
+        }
+        else if (ip_version == 6)
+        {
+            LOG_DEBUG("%s: trace id: %lu, layer: %s, payload len: [%u/%u]", LOG_PKT_PARSER, handler->packet_id, layer2str(LAYER_TYPE_MPLS), payload_len, length);
+            return parse_ipv6(handler, payload_data, payload_len);
+        }
+        else
+        {
+            LOG_DEBUG("%s: trace id: %lu, layer: %s, payload len: [%u/%u]", LOG_PKT_PARSER, handler->packet_id, layer2str(LAYER_TYPE_MPLS), payload_len, length);
+            // TESTED OK BY LWP
+            return parse_ether(handler, payload_data, payload_len);
+        }
+    }
+    else
+    {
+        LOG_DEBUG("%s: trace id: %lu, layer: %s, payload len: [%u/%u]", LOG_PKT_PARSER, handler->packet_id, layer2str(LAYER_TYPE_MPLS), payload_len, length);
+        // TESTED OK BY LWP
+        return parse_mpls(handler, payload_data, payload_len);
+    }
+}
\ No newline at end of file