TSG-13383 解析RAW_PACKET

* 增加 ETH_VLAN_VLAN_IP4_IP4_UDP 的测试用例
    * 增加 ETH_IP6_IP4_TCP_SSH 的测试用例
    * 增加 ETH_IP4_IP6_TCP 的测试用例
    * 增加 ETH_IP6_IP6_UDP 的测试用例
    * 增加 ETH_MPLS_IP4_TCP 的测试用例
    * 增加 ETH_MPLS_MPLS_IP4_TCP 的测试用例
    * 增加 ETH_VLAN_PPPOE_IP4_TCP 的测试用例
    * 增加 ETH_IP6_UDP_GTP_IP6_TCP_TLS 的测试用例
    * 增加 ETH_IP6_UDP_GTP_IP4_TCP_TLS 的测试用例
    * 增加 ETH_IP4_UDP_VXLAN_ETH_IP4_UDP_DNS 的测试用例
    * 增加 ETH_MPLS_MPLS_PWETHCW_ETH_ARP 的测试用例
    * 增加 ETH_MPLS_MPLS_PWETHCW_ETH_ARP 的测试用例

common/CMakeLists.txt

@@ -1,4 +1,4 @@
-add_library(common src/addr_tuple4.cpp src/session_table.cpp src/bfd.cpp)
+add_library(common src/addr_tuple4.cpp src/session_table.cpp src/raw_packet.cpp src/bfd.cpp)
 
 target_include_directories(common PUBLIC ${CMAKE_CURRENT_LIST_DIR}/include)
 

common/include/addr_tuple4.h

@@ -8,44 +8,42 @@ extern "C"
 
 #include <netinet/in.h>
 
-enum addr_tuple4_type
-{
+    enum addr_tuple4_type
+    {
         ADDR_TUPLE4_TYPE_V4,
         ADDR_TUPLE4_TYPE_V6,
-};
+    };
 
-struct addr_tuple4_v4
-{
+    struct addr_v4
+    {
         struct in_addr src_addr; /* network order */
         struct in_addr dst_addr; /* network order */
-    in_port_t src_port;      /* network order */
-    in_port_t dst_port;      /* network order */
-};
+    };
 
-struct addr_tuple4_v6
-{
+    struct addr_v6
+    {
         struct in6_addr src_addr; /* network order */
         struct in6_addr dst_addr; /* network order */
+    };
+
+    struct addr_tuple4
+    {
+        enum addr_tuple4_type addr_type;
         in_port_t src_port; /* network order */
         in_port_t dst_port; /* network order */
-};
-
-struct addr_tuple4
-{
-    enum addr_tuple4_type addr_type;
         union
         {
-        struct addr_tuple4_v4 addr_v4;
-        struct addr_tuple4_v6 addr_v6;
+            struct addr_v4 addr_v4;
+            struct addr_v6 addr_v6;
+        };
     };
-};
 
 #define INIT_ADDR_V4(name, src_addr_str, src_port_num, dst_addr_str, dst_port_num) \
     struct addr_tuple4 name;                                                       \
     memset(&name, 0, sizeof(name));                                                \
     (name).addr_type = ADDR_TUPLE4_TYPE_V4;                                        \
-    (name).addr_v4.src_port = htons((src_port_num));                               \
-    (name).addr_v4.dst_port = htons((dst_port_num));                               \
+    (name).src_port = htons((src_port_num));                                       \
+    (name).dst_port = htons((dst_port_num));                                       \
     inet_pton(AF_INET, (src_addr_str), &(name).addr_v4.src_addr);                  \
     inet_pton(AF_INET, (dst_addr_str), &(name).addr_v4.dst_addr);
 
@@ -53,13 +51,13 @@ struct addr_tuple4
     struct addr_tuple4 name;                                                       \
     memset(&name, 0, sizeof(name));                                                \
     (name).addr_type = ADDR_TUPLE4_TYPE_V6;                                        \
-    (name).addr_v6.src_port = htons((src_port_num));                               \
-    (name).addr_v6.dst_port = htons((dst_port_num));                               \
+    (name).src_port = htons((src_port_num));                                       \
+    (name).dst_port = htons((dst_port_num));                                       \
     inet_pton(AF_INET6, (src_addr_str), &(name).addr_v6.src_addr);                 \
     inet_pton(AF_INET6, (dst_addr_str), &(name).addr_v6.dst_addr);
 
-char *addr_tuple4_to_str(const struct addr_tuple4 *addr);
-void addr_tuple4_reverse(const struct addr_tuple4 *orin, struct addr_tuple4 *out);
+    char *addr_tuple4_to_str(const struct addr_tuple4 *addr);
+    void addr_tuple4_reverse(const struct addr_tuple4 *orin, struct addr_tuple4 *out);
 
 #ifdef __cpluscplus
 }

common/include/log.h

@@ -10,13 +10,13 @@ extern "C"
 
 #define LOG_DEBUG(format, ...)                       \
 	{                                                \
-		fprintf(stdout, format "\n", ##__VA_ARGS__); \
+		fprintf(stdout, "DEBUG " format "\n", ##__VA_ARGS__); \
 		fflush(stdout);                              \
 	}
 
 #define LOG_ERROR(format, ...)                       \
 	{                                                \
-		fprintf(stderr, format "\n", ##__VA_ARGS__); \
+		fprintf(stderr, "ERROR " format "\n", ##__VA_ARGS__); \
 		fflush(stderr);                              \
 	}
 

common/include/raw_packet.h

@@ -0,0 +1,80 @@
+#ifndef _RAW_PACKET_H
+#define _RAW_PACKET_H
+
+#ifdef __cpluscplus
+extern "C"
+{
+#endif
+
+enum layer_type
+{
+    // 数据链路层
+    LAYER_TYPE_ETHER   = 1 << 0,
+    LAYER_TYPE_PPP     = 1 << 1,
+    LAYER_TYPE_HDLC    = 1 << 2,
+    LAYER_TYPE_L2      = (LAYER_TYPE_ETHER | LAYER_TYPE_PPP | LAYER_TYPE_HDLC),
+
+    // 数据链路层 -- 隧道
+    LAYER_TYPE_VLAN    = 1 << 3,
+    LAYER_TYPE_PPPOE   = 1 << 4,
+    LAYER_TYPE_MPLS    = 1 << 5,
+    LAYER_TYPE_L2_TUN  = (LAYER_TYPE_VLAN | LAYER_TYPE_PPPOE | LAYER_TYPE_MPLS),
+
+    // 网络层
+    LAYER_TYPE_IPV4    = 1 << 6,
+    LAYER_TYPE_IPV6    = 1 << 7,
+    LAYER_TYPE_L3      = (LAYER_TYPE_IPV4 | LAYER_TYPE_IPV6),
+
+    // 网络层 -- 隧道
+
+    // 传输层
+    LAYER_TYPE_UDP     = 1 << 8,
+    LAYER_TYPE_TCP     = 1 << 9,
+    LAYER_TYPE_L4      = (LAYER_TYPE_UDP | LAYER_TYPE_TCP),
+
+    // 传输层 -- 隧道
+    LAYER_TYPE_G_VXLAN = 1 << 10,
+    LAYER_TYPE_GTPV1_U = 1 << 11,
+
+    // ALL
+    LAYER_TYPE_ALL     = (LAYER_TYPE_L2 | LAYER_TYPE_L2_TUN | LAYER_TYPE_L3 | LAYER_TYPE_L4 | LAYER_TYPE_G_VXLAN | LAYER_TYPE_GTPV1_U),
+
+    // UNKNOWN
+    LAYER_TYPE_UNKNOWN,
+};
+
+enum parse_status
+{
+    PARSE_STATUS_CONTINUE,
+    PARSE_STATUS_STOP
+};
+
+struct raw_pkt_parser;
+
+struct raw_pkt_parser *raw_packet_parser_create(enum layer_type expect_type, uint16_t expect_results_num);
+void raw_packet_parser_destory(struct raw_pkt_parser *handler);
+
+// return       0 : success
+// return -ENOMEM : error
+int raw_packet_parser_push(struct raw_pkt_parser *handler, enum layer_type type, uint16_t offset);
+// return PARSE_STATUS_CONTINUE
+// return PARSE_STATUS_STOP
+enum parse_status raw_packet_parser_status(struct raw_pkt_parser *handler, const void *data, enum layer_type this_type);
+// return most inner payload
+const void *raw_packet_parser_parse(struct raw_pkt_parser *handler, const void *data, size_t length);
+
+// return  0 : success
+// return -1 : error
+int raw_packet_parser_get_most_inner_tuple4(struct raw_pkt_parser *handler, struct addr_tuple4 *addr);
+int raw_packet_parser_get_most_outer_tuple4(struct raw_pkt_parser *handler, struct addr_tuple4 *addr);
+
+// return  0 : success
+// return -1 : error
+int raw_packet_parser_get_most_inner_address(struct raw_pkt_parser *handler, struct addr_tuple4 *addr);
+int raw_packet_parser_get_most_outer_address(struct raw_pkt_parser *handler, struct addr_tuple4 *addr);
+
+#ifdef __cpluscplus
+}
+#endif
+
+#endif

common/src/addr_tuple4.cpp

@@ -12,8 +12,8 @@ char *addr_tuple4_to_str(const struct addr_tuple4 *addr)
     {
         char src_addr[INET_ADDRSTRLEN] = {0};
         char dst_addr[INET_ADDRSTRLEN] = {0};
-        uint16_t src_port = ntohs((uint16_t)addr->addr_v4.src_port);
-        uint16_t dst_port = ntohs((uint16_t)addr->addr_v4.dst_port);
+        uint16_t src_port = ntohs((uint16_t)addr->src_port);
+        uint16_t dst_port = ntohs((uint16_t)addr->dst_port);
         inet_ntop(AF_INET, &addr->addr_v4.src_addr, src_addr, sizeof(src_addr));
         inet_ntop(AF_INET, &addr->addr_v4.dst_addr, dst_addr, sizeof(dst_addr));
         asprintf(&str_ret, "%s %u %s %u", src_addr, src_port, dst_addr, dst_port);
@@ -23,8 +23,8 @@ char *addr_tuple4_to_str(const struct addr_tuple4 *addr)
     {
         char src_addr[INET6_ADDRSTRLEN] = {0};
         char dst_addr[INET6_ADDRSTRLEN] = {0};
-        uint16_t src_port = ntohs((uint16_t)addr->addr_v6.src_port);
-        uint16_t dst_port = ntohs((uint16_t)addr->addr_v6.dst_port);
+        uint16_t src_port = ntohs((uint16_t)addr->src_port);
+        uint16_t dst_port = ntohs((uint16_t)addr->dst_port);
         inet_ntop(AF_INET6, &addr->addr_v6.src_addr, src_addr, sizeof(src_addr));
         inet_ntop(AF_INET6, &addr->addr_v6.dst_addr, dst_addr, sizeof(dst_addr));
         asprintf(&str_ret, "%s %u %s %u", src_addr, src_port, dst_addr, dst_port);
@@ -42,8 +42,8 @@ void addr_tuple4_reverse(const struct addr_tuple4 *orin, struct addr_tuple4 *out
         out->addr_type = ADDR_TUPLE4_TYPE_V4;
         out->addr_v4.src_addr = orin->addr_v4.dst_addr;
         out->addr_v4.dst_addr = orin->addr_v4.src_addr;
-        out->addr_v4.src_port = orin->addr_v4.dst_port;
-        out->addr_v4.dst_port = orin->addr_v4.src_port;
+        out->src_port = orin->dst_port;
+        out->dst_port = orin->src_port;
     }
 
     if (orin->addr_type == ADDR_TUPLE4_TYPE_V6)
@@ -51,7 +51,7 @@ void addr_tuple4_reverse(const struct addr_tuple4 *orin, struct addr_tuple4 *out
         out->addr_type = ADDR_TUPLE4_TYPE_V6;
         out->addr_v6.src_addr = orin->addr_v6.dst_addr;
         out->addr_v6.dst_addr = orin->addr_v6.src_addr;
-        out->addr_v6.src_port = orin->addr_v6.dst_port;
-        out->addr_v6.dst_port = orin->addr_v6.src_port;
+        out->src_port = orin->dst_port;
+        out->dst_port = orin->src_port;
     }
 }

common/src/raw_packet.cpp

@@ -0,0 +1,959 @@
+#include <assert.h>
+#include <errno.h>
+#include <string.h>
+#include <stdlib.h>
+
+#include <netinet/ip.h>
+#include <netinet/ip6.h>
+#define __FAVOR_BSD 1
+#include <netinet/tcp.h>
+#include <netinet/udp.h>
+#include <netinet/ether.h>
+#include <linux/ppp_defs.h>
+
+#include "log.h"
+#include "addr_tuple4.h"
+#include "raw_packet.h"
+
+#define LOG_TAG "RAW_PACKET_PARSER"
+
+/******************************************************************************
+ * Protocol Struct
+ ******************************************************************************/
+
+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)
+
+/******************************************************************************
+ * Parser Struct
+ ******************************************************************************/
+
+static uint64_t packet_trace_id = 0;
+
+struct layer_result
+{
+    uint16_t offset;
+    enum layer_type type;
+};
+
+struct layer_result_array
+{
+    struct layer_result *layers;
+    uint16_t layers_used;
+    uint16_t layers_size;
+};
+
+struct raw_pkt_parser
+{
+    enum layer_type expect_type;
+    struct layer_result_array results;
+
+    const void *ptr_pkt_start;
+    uint64_t pkt_trace_id;
+};
+
+/******************************************************************************
+ * Static API
+ ******************************************************************************/
+
+// parser utils
+static void set_addr_tuple4(const void *data, enum layer_type layer_type, struct addr_tuple4 *addr);
+static const char *layer_type2str(enum layer_type this_type);
+static uint16_t parse_gtphdr_len(const struct gtp_hdr *gtph);
+
+// parser protocol
+static const void *parse_ether(struct raw_pkt_parser *handler, const void *data, size_t length, enum layer_type this_type);
+static const void *parse_ipv4(struct raw_pkt_parser *handler, const void *data, size_t length, enum layer_type this_type);
+static const void *parse_ipv6(struct raw_pkt_parser *handler, const void *data, size_t length, enum layer_type this_type);
+static const void *parse_tcp(struct raw_pkt_parser *handler, const void *data, size_t length, enum layer_type this_type);
+static const void *parse_udp(struct raw_pkt_parser *handler, const void *data, size_t length, enum layer_type this_type);
+static const void *parse_ppp(struct raw_pkt_parser *handler, const void *data, size_t length, enum layer_type this_type);
+static const void *parse_pppoe_ses(struct raw_pkt_parser *handler, const void *data, size_t length, enum layer_type this_type);
+// static const void *parse_hdlc(struct raw_pkt_parser *handler, const void *data, size_t length, enum layer_type this_type);
+static const void *parse_vxlan(struct raw_pkt_parser *handler, const void *data, size_t length, enum layer_type this_type);
+static const void *parse_vlan8021q(struct raw_pkt_parser *handler, const void *data, size_t length, enum layer_type this_type);
+static const void *parse_gtpv1_u(struct raw_pkt_parser *handler, const void *data, size_t length, enum layer_type this_type);
+static const void *parse_mpls(struct raw_pkt_parser *handler, const void *data, size_t length, enum layer_type this_type);
+
+/******************************************************************************
+ * Public API
+ ******************************************************************************/
+
+struct raw_pkt_parser *raw_packet_parser_create(enum layer_type expect_type, uint16_t expect_results_num)
+{
+    struct raw_pkt_parser *handler = (struct raw_pkt_parser *)calloc(1, sizeof(struct raw_pkt_parser));
+    assert(handler);
+
+    handler->expect_type = expect_type;
+    handler->results.layers = (struct layer_result *)calloc(expect_results_num, sizeof(struct layer_result));
+    assert(handler->results.layers);
+    handler->results.layers_used = 0;
+    handler->results.layers_size = expect_results_num;
+
+    return handler;
+}
+
+void raw_packet_parser_destory(struct raw_pkt_parser *handler)
+{
+    if (handler)
+    {
+        if (handler->results.layers)
+        {
+            free(handler->results.layers);
+            handler->results.layers = NULL;
+        }
+
+        free(handler);
+        handler = NULL;
+    }
+}
+
+// return       0 : success
+// return -ENOMEM : error
+int raw_packet_parser_push(struct raw_pkt_parser *handler, enum layer_type type, uint16_t offset)
+{
+    struct layer_result_array *result = &handler->results;
+
+    if (result->layers_used >= result->layers_size)
+    {
+        return -ENOMEM;
+    }
+
+    result->layers[result->layers_used].offset = offset;
+    result->layers[result->layers_used].type = type;
+    result->layers_used++;
+
+    return 0;
+}
+
+// return PARSE_STATUS_CONTINUE
+// return PARSE_STATUS_STOP
+enum parse_status raw_packet_parser_status(struct raw_pkt_parser *handler, const void *data, enum layer_type this_type)
+{
+    /*
+     * only when this_type & handler->expect_type is true,
+     * the information of the current layer will be recorded in results.
+     */
+    if (!(this_type & handler->expect_type))
+    {
+        return PARSE_STATUS_CONTINUE;
+    }
+
+    uint16_t offset = (uintptr_t)data - (uintptr_t)(handler->ptr_pkt_start);
+    if (raw_packet_parser_push(handler, this_type, offset) < 0)
+    {
+        return PARSE_STATUS_STOP;
+    }
+    else
+    {
+        return PARSE_STATUS_CONTINUE;
+    }
+}
+
+// return most inner payload
+const void *raw_packet_parser_parse(struct raw_pkt_parser *handler, const void *data, size_t length)
+{
+    handler->ptr_pkt_start = data;
+    handler->pkt_trace_id = __atomic_fetch_add(&packet_trace_id, 1, __ATOMIC_RELAXED);
+
+    // TESTED
+    return parse_ether(handler, data, length, LAYER_TYPE_ETHER);
+}
+
+// return  0 : success
+// return -1 : error
+int raw_packet_parser_get_most_inner_tuple4(struct raw_pkt_parser *handler, struct addr_tuple4 *addr)
+{
+    const char *l3_layer_data = NULL;
+    const char *l4_layer_data = NULL;
+    const struct layer_result *l3_layer_result = NULL;
+    const struct layer_result *l4_layer_result = NULL;
+    struct layer_result_array *results = &handler->results;
+
+    // search L4 layer and L3 layer in reverse order
+    for (int8_t i = results->layers_used - 1; i >= 0; i--)
+    {
+        const struct layer_result *layer = &results->layers[i];
+        enum layer_type type = layer->type;
+
+        LOG_DEBUG("%s: find most inner tuple4, pkt_trace_id: %lu, layer[%d/%d]: %s", LOG_TAG, handler->pkt_trace_id, i, results->layers_size, layer_type2str(type));
+
+        // first get L4 layer
+        if (type & LAYER_TYPE_L4)
+        {
+            l4_layer_result = layer;
+            continue;
+        }
+
+        // second get L3 layer
+        if (type & LAYER_TYPE_L3)
+        {
+            l3_layer_result = layer;
+            break;
+        }
+    }
+
+    if (l3_layer_result)
+    {
+        l3_layer_data = (const char *)handler->ptr_pkt_start + l3_layer_result->offset;
+        set_addr_tuple4(l3_layer_data, l3_layer_result->type, addr);
+    }
+
+    if (l4_layer_result)
+    {
+        l4_layer_data = (const char *)handler->ptr_pkt_start + l4_layer_result->offset;
+        set_addr_tuple4(l4_layer_data, l4_layer_result->type, addr);
+    }
+
+    if (l3_layer_result && l4_layer_result)
+    {
+        return 0;
+    }
+    else
+    {
+        return -1;
+    }
+}
+
+// return  0 : success
+// return -1 : error
+int raw_packet_parser_get_most_outer_tuple4(struct raw_pkt_parser *handler, struct addr_tuple4 *addr)
+{
+    const char *l3_layer_data = NULL;
+    const char *l4_layer_data = NULL;
+    const struct layer_result *l3_layer_result = NULL;
+    const struct layer_result *l4_layer_result = NULL;
+    struct layer_result_array *results = &handler->results;
+
+    // search L3 layer and L4 layer in order
+    for (int8_t i = 0; i <= results->layers_used - 1; i++)
+    {
+        const struct layer_result *layer = &results->layers[i];
+        enum layer_type type = layer->type;
+
+        LOG_DEBUG("%s: find most outer tuple4, pkt_trace_id: %lu, layer[%d/%d]: %s", LOG_TAG, handler->pkt_trace_id, i, results->layers_size, layer_type2str(type));
+
+        // first get L3 layer
+        if (type & LAYER_TYPE_L3)
+        {
+            l3_layer_result = layer;
+            continue;
+        }
+
+        // second get L4 layer
+        if (type & LAYER_TYPE_L4)
+        {
+            l4_layer_result = layer;
+            break;
+        }
+    }
+
+    if (l3_layer_result)
+    {
+        l3_layer_data = (const char *)handler->ptr_pkt_start + l3_layer_result->offset;
+        set_addr_tuple4(l3_layer_data, l3_layer_result->type, addr);
+    }
+
+    if (l4_layer_result)
+    {
+        l4_layer_data = (const char *)handler->ptr_pkt_start + l4_layer_result->offset;
+        set_addr_tuple4(l4_layer_data, l4_layer_result->type, addr);
+    }
+
+    if (l3_layer_result && l4_layer_result)
+    {
+        return 0;
+    }
+    else
+    {
+        return -1;
+    }
+}
+
+// return  0 : success
+// return -1 : error
+int raw_packet_parser_get_most_inner_address(struct raw_pkt_parser *handler, struct addr_tuple4 *addr)
+{
+    const char *l3_layer_data = NULL;
+    struct layer_result_array *results = &handler->results;
+
+    // search L3 layer in reverse order
+    for (int8_t i = results->layers_used - 1; i >= 0; i--)
+    {
+        const struct layer_result *layer = &results->layers[i];
+        enum layer_type type = layer->type;
+
+        LOG_DEBUG("%s: find most inner address, pkt_trace_id: %lu, layer[%d/%d]: %s", LOG_TAG, handler->pkt_trace_id, i, results->layers_size, layer_type2str(type));
+        if (type & LAYER_TYPE_L3)
+        {
+            l3_layer_data = (const char *)handler->ptr_pkt_start + layer->offset;
+            set_addr_tuple4(l3_layer_data, type, addr);
+            return 0;
+        }
+    }
+
+    return -1;
+}
+
+// return  0 : success
+// return -1 : error
+int raw_packet_parser_get_most_outer_address(struct raw_pkt_parser *handler, struct addr_tuple4 *addr)
+{
+    const char *l3_layer_data = NULL;
+    struct layer_result_array *results = &handler->results;
+
+    // search L3 layer in order
+    for (int8_t i = 0; i <= results->layers_used - 1; i++)
+    {
+        const struct layer_result *layer = &results->layers[i];
+        enum layer_type type = layer->type;
+
+        LOG_DEBUG("%s: find most outer address, pkt_trace_id: %lu, layer[%d/%d]: %s", LOG_TAG, handler->pkt_trace_id, i, results->layers_size, layer_type2str(type));
+        if (type & LAYER_TYPE_L3)
+        {
+            l3_layer_data = (const char *)handler->ptr_pkt_start + layer->offset;
+            set_addr_tuple4(l3_layer_data, type, addr);
+            return 0;
+        }
+    }
+
+    return -1;
+}
+
+/******************************************************************************
+ * Private API
+ ******************************************************************************/
+
+static void set_addr_tuple4(const void *data, enum layer_type layer_type, struct addr_tuple4 *addr)
+{
+    const struct tcphdr *tcp_hdr = NULL;
+    const struct udphdr *udp_hdr = NULL;
+    const struct ip *ipv4_hdr = NULL;
+    const struct ip6_hdr *ipv6_hdr = NULL;
+
+    switch (layer_type)
+    {
+    case LAYER_TYPE_TCP:
+        tcp_hdr = (const struct tcphdr *)data;
+        addr->src_port = tcp_hdr->th_sport;
+        addr->dst_port = tcp_hdr->th_dport;
+        break;
+    case LAYER_TYPE_UDP:
+        udp_hdr = (const struct udphdr *)data;
+        addr->src_port = udp_hdr->uh_sport;
+        addr->dst_port = udp_hdr->uh_dport;
+        break;
+    case LAYER_TYPE_IPV4:
+        ipv4_hdr = (const struct ip *)data;
+        addr->addr_type = ADDR_TUPLE4_TYPE_V4;
+        addr->addr_v4.src_addr = ipv4_hdr->ip_src;
+        addr->addr_v4.dst_addr = ipv4_hdr->ip_dst;
+        break;
+    case LAYER_TYPE_IPV6:
+        ipv6_hdr = (const struct ip6_hdr *)data;
+        addr->addr_type = ADDR_TUPLE4_TYPE_V6;
+        memcpy(&addr->addr_v6.src_addr, &ipv6_hdr->ip6_src, sizeof(addr->addr_v6.src_addr));
+        memcpy(&addr->addr_v6.dst_addr, &ipv6_hdr->ip6_dst, sizeof(addr->addr_v6.dst_addr));
+        break;
+    default:
+        break;
+    }
+}
+
+static const char *layer_type2str(enum layer_type this_type)
+{
+    switch (this_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 raw_pkt_parser *handler, const void *data, size_t length, enum layer_type this_type)
+{
+    if (length < sizeof(struct ethhdr))
+    {
+        LOG_ERROR("%s: pkt_trace_id: %lu, this_layer: %s, err: data not enough", LOG_TAG, handler->pkt_trace_id, layer_type2str(this_type));
+        return data;
+    }
+
+    if (raw_packet_parser_status(handler, data, this_type) == PARSE_STATUS_STOP)
+    {
+        return data;
+    }
+
+    struct ethhdr *hdr = (struct ethhdr *)data;
+    uint16_t next_proto = ntohs(hdr->h_proto);
+    uint16_t hdr_len = sizeof(struct ethhdr);
+    const void *data_next_layer = (const char *)data + hdr_len;
+    size_t data_next_length = length - hdr_len;
+
+    LOG_DEBUG("%s: pkt_trace_id: %lu, this_layer: %s, payload_len: [%lu/%lu]", LOG_TAG, handler->pkt_trace_id, layer_type2str(this_type), data_next_length, length);
+    switch (next_proto)
+    {
+    case ETH_P_8021Q:
+        // TESTED
+        return parse_vlan8021q(handler, data_next_layer, data_next_length, LAYER_TYPE_VLAN);
+    case ETH_P_8021AD:
+        // TODO
+        return parse_ether(handler, data_next_layer, data_next_length, LAYER_TYPE_ETHER);
+    case ETH_P_IP:
+        // TESTED
+        return parse_ipv4(handler, data_next_layer, data_next_length, LAYER_TYPE_IPV4);
+    case ETH_P_IPV6:
+        // TESTED
+        return parse_ipv6(handler, data_next_layer, data_next_length, LAYER_TYPE_IPV6);
+    case ETH_P_PPP_SES:
+        // TODO
+        return parse_pppoe_ses(handler, data_next_layer, data_next_length, LAYER_TYPE_PPPOE);
+    case ETH_P_MPLS_UC:
+        // TESTED
+        return parse_mpls(handler, data_next_layer, data_next_length, LAYER_TYPE_MPLS);
+    default:
+        LOG_ERROR("%s: pkt_trace_id: %lu, this_layer: %s, stop parse next protocol %d", LOG_TAG, handler->pkt_trace_id, layer_type2str(this_type), next_proto);
+        return data_next_layer;
+    }
+}
+
+static const void *parse_ipv4(struct raw_pkt_parser *handler, const void *data, size_t length, enum layer_type this_type)
+{
+    if (length < sizeof(struct ip))
+    {
+        LOG_ERROR("%s: pkt_trace_id: %lu, this_layer: %s, err: data not enough", LOG_TAG, handler->pkt_trace_id, layer_type2str(this_type));
+        return data;
+    }
+
+    if (raw_packet_parser_status(handler, data, this_type) == PARSE_STATUS_STOP)
+    {
+        return data;
+    }
+
+    struct ip *hdr = (struct ip *)data;
+    uint16_t next_proto = hdr->ip_p;
+    uint16_t hdr_len = (hdr->ip_hl & 0xf) * 4u;
+    const void *data_next_layer = (const char *)data + hdr_len;
+    size_t data_next_length = length - hdr_len;
+
+    LOG_DEBUG("%s: pkt_trace_id: %lu, this_layer: %s, payload_len: [%lu/%lu]", LOG_TAG, handler->pkt_trace_id, layer_type2str(this_type), data_next_length, length);
+    switch (next_proto)
+    {
+    case IPPROTO_TCP:
+        // TESTED
+        return parse_tcp(handler, data_next_layer, data_next_length, LAYER_TYPE_TCP);
+    case IPPROTO_UDP:
+        // TESTED
+        return parse_udp(handler, data_next_layer, data_next_length, LAYER_TYPE_UDP);
+    case IPPROTO_IPIP:
+        // TESTED
+        return parse_ipv4(handler, data_next_layer, data_next_length, LAYER_TYPE_IPV4);
+    case IPPROTO_IPV6:
+        // TESTED
+        return parse_ipv6(handler, data_next_layer, data_next_length, LAYER_TYPE_IPV6);
+    default:
+        LOG_ERROR("%s: pkt_trace_id: %lu, this_layer: %s, stop parse next protocol %d", LOG_TAG, handler->pkt_trace_id, layer_type2str(this_type), next_proto);
+        return data_next_layer;
+    }
+}
+
+static const void *parse_ipv6(struct raw_pkt_parser *handler, const void *data, size_t length, enum layer_type this_type)
+{
+    if (length < sizeof(struct ip6_hdr))
+    {
+        LOG_ERROR("%s: pkt_trace_id: %lu, this_layer: %s, err: data not enough", LOG_TAG, handler->pkt_trace_id, layer_type2str(this_type));
+        return data;
+    }
+
+    if (raw_packet_parser_status(handler, data, this_type) == PARSE_STATUS_STOP)
+    {
+        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);
+    const void *data_next_layer = (const char *)data + hdr_len;
+    size_t data_next_length = length - hdr_len;
+
+    LOG_DEBUG("%s: pkt_trace_id: %lu, this_layer: %s, payload_len: [%lu/%lu]", LOG_TAG, handler->pkt_trace_id, layer_type2str(this_type), data_next_length, length);
+    switch (next_proto)
+    {
+    case IPPROTO_TCP:
+        // TESTED
+        return parse_tcp(handler, data_next_layer, data_next_length, LAYER_TYPE_TCP);
+    case IPPROTO_UDP:
+        // TESTED
+        return parse_udp(handler, data_next_layer, data_next_length, LAYER_TYPE_UDP);
+    case IPPROTO_IPIP:
+        // TESTED
+        return parse_ipv4(handler, data_next_layer, data_next_length, LAYER_TYPE_IPV4);
+    case IPPROTO_IPV6:
+        // TESTED
+        return parse_ipv6(handler, data_next_layer, data_next_length, LAYER_TYPE_IPV6);
+    default:
+        LOG_ERROR("%s: pkt_trace_id: %lu, this_layer: %s, stop parse next protocol %d", LOG_TAG, handler->pkt_trace_id, layer_type2str(this_type), next_proto);
+        return data_next_layer;
+    }
+}
+
+static const void *parse_tcp(struct raw_pkt_parser *handler, const void *data, size_t length, enum layer_type this_type)
+{
+    if (length < sizeof(struct tcphdr))
+    {
+        LOG_ERROR("%s: pkt_trace_id: %lu, this_layer: %s, err: data not enough", LOG_TAG, handler->pkt_trace_id, layer_type2str(this_type));
+        return data;
+    }
+
+    if (raw_packet_parser_status(handler, data, this_type) == PARSE_STATUS_STOP)
+    {
+        return data;
+    }
+
+    struct tcphdr *hdr = (struct tcphdr *)data;
+    uint16_t hdr_len = hdr->th_off << 2;
+    const void *data_next_layer = (const char *)data + hdr_len;
+    size_t data_next_length = length - hdr_len;
+
+    LOG_DEBUG("%s: pkt_trace_id: %lu, this_layer: %s, payload_len: [%lu/%lu]", LOG_TAG, handler->pkt_trace_id, layer_type2str(this_type), data_next_length, length);
+
+    return data_next_layer;
+}
+
+static const void *parse_udp(struct raw_pkt_parser *handler, const void *data, size_t length, enum layer_type this_type)
+{
+    if (length < sizeof(struct udphdr))
+    {
+        LOG_ERROR("%s: pkt_trace_id: %lu, this_layer: %s, err: data not enough", LOG_TAG, handler->pkt_trace_id, layer_type2str(this_type));
+        return data;
+    }
+
+    if (raw_packet_parser_status(handler, data, this_type) == PARSE_STATUS_STOP)
+    {
+        return data;
+    }
+
+    struct udphdr *hdr = (struct udphdr *)data;
+    uint16_t hdr_len = sizeof(struct udphdr);
+    const void *data_next_layer = (const char *)data + hdr_len;
+    size_t data_next_length = length - hdr_len;
+
+    LOG_DEBUG("%s: pkt_trace_id: %lu, this_layer: %s, payload_len: [%lu/%lu]", LOG_TAG, handler->pkt_trace_id, layer_type2str(this_type), data_next_length, length);
+    switch (ntohs(hdr->uh_dport))
+    {
+    // VXLAN_DPORT
+    case 4789:
+        // TESTED
+        return parse_vxlan(handler, data_next_layer, data_next_length, LAYER_TYPE_G_VXLAN);
+    // GTP1U_PORT
+    case 2152:
+        // TESTED
+        return parse_gtpv1_u(handler, data_next_layer, data_next_length, LAYER_TYPE_GTPV1_U);
+    default:
+        return data_next_layer;
+    }
+}
+
+static const void *parse_ppp(struct raw_pkt_parser *handler, const void *data, size_t length, enum layer_type this_type)
+{
+    if (length < PPP_HDRLEN)
+    {
+        LOG_ERROR("%s: pkt_trace_id: %lu, this_layer: %s, err: data not enough", LOG_TAG, handler->pkt_trace_id, layer_type2str(this_type));
+        return data;
+    }
+
+    if (raw_packet_parser_status(handler, data, this_type) == PARSE_STATUS_STOP)
+    {
+        return data;
+    }
+
+    uint16_t next_proto = PPP_PROTOCOL(data);
+    uint16_t hdr_len = PPP_HDRLEN;
+    const void *data_next_layer = (const char *)data + hdr_len;
+    size_t data_next_length = length - hdr_len;
+
+    LOG_DEBUG("%s: pkt_trace_id: %lu, this_layer: %s, payload_len: [%lu/%lu]", LOG_TAG, handler->pkt_trace_id, layer_type2str(this_type), data_next_length, length);
+    switch (next_proto)
+    {
+    case PPP_IP:
+        // TODO
+        return parse_ipv4(handler, data_next_layer, data_next_length, LAYER_TYPE_IPV4);
+    case PPP_IPV6:
+        // TODO
+        return parse_ipv6(handler, data_next_layer, data_next_length, LAYER_TYPE_IPV6);
+    default:
+        LOG_ERROR("%s: pkt_trace_id: %lu, this_layer: %s, stop parse next protocol %d", LOG_TAG, handler->pkt_trace_id, layer_type2str(this_type), next_proto);
+        return data_next_layer;
+    }
+}
+
+static const void *parse_pppoe_ses(struct raw_pkt_parser *handler, const void *data, size_t length, enum layer_type this_type)
+{
+    if (length < 8)
+    {
+        LOG_ERROR("%s: pkt_trace_id: %lu, this_layer: %s, err: data not enough", LOG_TAG, handler->pkt_trace_id, layer_type2str(this_type));
+        return data;
+    }
+
+    if (raw_packet_parser_status(handler, data, this_type) == PARSE_STATUS_STOP)
+    {
+        return data;
+    }
+
+    uint16_t next_proto = *((uint16_t *)data + 3);
+    uint16_t hdr_len = 8;
+    const void *data_next_layer = (const char *)data + hdr_len;
+    size_t data_next_length = length - hdr_len;
+
+    LOG_DEBUG("%s: pkt_trace_id: %lu, this_layer: %s, payload_len: [%lu/%lu]", LOG_TAG, handler->pkt_trace_id, layer_type2str(this_type), data_next_length, length);
+    switch (next_proto)
+    {
+        // PPPOE_TYPE_IPV4
+    case 0x2100:
+        // TESTED
+        return parse_ipv4(handler, data_next_layer, data_next_length, LAYER_TYPE_IPV4);
+        // PPPOE_TYPE_IPV6
+    case 0x5700:
+        // TODO
+        return parse_ipv6(handler, data_next_layer, data_next_length, LAYER_TYPE_IPV6);
+    default:
+        LOG_ERROR("%s: pkt_trace_id: %lu, this_layer: %s, stop parse next protocol %d", LOG_TAG, handler->pkt_trace_id, layer_type2str(this_type), next_proto);
+        return data_next_layer;
+    }
+}
+
+static const void *parse_hdlc(struct raw_pkt_parser *handler, const void *data, size_t length, enum layer_type this_type)
+{
+    if (length < 4)
+    {
+        LOG_ERROR("%s: pkt_trace_id: %lu, this_layer: %s, err: data not enough", LOG_TAG, handler->pkt_trace_id, layer_type2str(this_type));
+        return data;
+    }
+
+    if (raw_packet_parser_status(handler, data, this_type) == PARSE_STATUS_STOP)
+    {
+        return data;
+    }
+
+    uint16_t next_proto = ntohs(*(const uint16_t *)((const char *)data + 2));
+    uint16_t hdr_len = 4;
+    const void *data_next_layer = (const char *)data + hdr_len;
+    size_t data_next_length = length - hdr_len;
+
+    LOG_DEBUG("%s: pkt_trace_id: %lu, this_layer: %s, payload_len: [%lu/%lu]", LOG_TAG, handler->pkt_trace_id, layer_type2str(this_type), data_next_length, length);
+    // HDLC的协议字段与以太网的类似，对于IPv4，为0x0800
+    switch (next_proto)
+    {
+    // ETHER_TYPE_IPV4 0x0800
+    case 0x0800:
+        // TODO
+        return parse_ipv4(handler, data_next_layer, data_next_length, LAYER_TYPE_IPV4);
+    // ETHER_TYPE_IPV6 0x86DD
+    case 0x86DD:
+        // TODO
+        return parse_ipv6(handler, data_next_layer, data_next_length, LAYER_TYPE_IPV6);
+    default:
+        LOG_ERROR("%s: pkt_trace_id: %lu, this_layer: %s, stop parse next protocol %d", LOG_TAG, handler->pkt_trace_id, layer_type2str(this_type), next_proto);
+        return data_next_layer;
+    }
+}
+
+static const void *parse_vxlan(struct raw_pkt_parser *handler, const void *data, size_t length, enum layer_type this_type)
+{
+    if (length < sizeof(struct vxlan_hdr))
+    {
+        LOG_ERROR("%s: pkt_trace_id: %lu, this_layer: %s, err: data not enough", LOG_TAG, handler->pkt_trace_id, layer_type2str(this_type));
+        return NULL;
+    }
+
+    if (raw_packet_parser_status(handler, data, this_type) == PARSE_STATUS_STOP)
+    {
+        return data;
+    }
+
+    struct vxlan_hdr *vxlan_hdr = (struct vxlan_hdr *)data;
+    uint16_t hdr_len = sizeof(struct vxlan_hdr);
+    const void *data_next_layer = (const char *)data + hdr_len;
+    size_t data_next_length = length - hdr_len;
+
+    LOG_DEBUG("%s: pkt_trace_id: %lu, this_layer: %s, payload_len: [%lu/%lu]", LOG_TAG, handler->pkt_trace_id, layer_type2str(this_type), data_next_length, length);
+    // TESTED
+    return parse_ether(handler, data_next_layer, data_next_length, LAYER_TYPE_ETHER);
+
+    // TODO HDLC
+    // TODO PPP
+}
+
+static const void *parse_vlan8021q(struct raw_pkt_parser *handler, const void *data, size_t length, enum layer_type this_type)
+{
+    if (length < sizeof(struct vlan_hdr))
+    {
+        LOG_ERROR("%s: pkt_trace_id: %lu, this_layer: %s, err: data not enough", LOG_TAG, handler->pkt_trace_id, layer_type2str(this_type));
+        return NULL;
+    }
+
+    if (raw_packet_parser_status(handler, data, this_type) == PARSE_STATUS_STOP)
+    {
+        return data;
+    }
+
+    struct vlan_hdr *hdr = (struct vlan_hdr *)data;
+    uint16_t next_proto = ntohs(hdr->protocol);
+    uint16_t hdr_len = sizeof(struct vlan_hdr);
+    const void *data_next_layer = (const char *)data + hdr_len;
+    size_t data_next_length = length - hdr_len;
+
+    LOG_DEBUG("%s: pkt_trace_id: %lu, this_layer: %s, payload_len: [%lu/%lu]", LOG_TAG, handler->pkt_trace_id, layer_type2str(this_type), data_next_length, length);
+    switch (next_proto)
+    {
+    case ETH_P_8021Q:
+        // TESTED
+        return parse_vlan8021q(handler, data_next_layer, data_next_length, LAYER_TYPE_VLAN);
+    case ETH_P_IP:
+        // TESTED
+        return parse_ipv4(handler, data_next_layer, data_next_length, LAYER_TYPE_IPV4);
+    case ETH_P_IPV6:
+        // TODO
+        return parse_ipv6(handler, data_next_layer, data_next_length, LAYER_TYPE_IPV6);
+    case ETH_P_PPP_SES:
+        // TESTED
+        return parse_pppoe_ses(handler, data_next_layer, data_next_length, LAYER_TYPE_PPPOE);
+    case ETH_P_MPLS_UC:
+        // TODO
+        return parse_mpls(handler, data_next_layer, data_next_length, LAYER_TYPE_MPLS);
+    default:
+        LOG_ERROR("%s: pkt_trace_id: %lu, this_layer: %s, stop parse next protocol %d", LOG_TAG, handler->pkt_trace_id, layer_type2str(this_type), next_proto);
+        return data_next_layer;
+    }
+}
+
+static const void *parse_gtpv1_u(struct raw_pkt_parser *handler, const void *data, size_t length, enum layer_type this_type)
+{
+    if (length < sizeof(struct gtp_hdr))
+    {
+        LOG_ERROR("%s: pkt_trace_id: %lu, this_layer: %s, err: data not enough", LOG_TAG, handler->pkt_trace_id, layer_type2str(this_type));
+        return NULL;
+    }
+
+    uint16_t hdr_len = parse_gtphdr_len((const struct gtp_hdr *)data);
+    if (hdr_len < 0)
+    {
+        return data;
+    }
+
+    if (raw_packet_parser_status(handler, data, this_type) == PARSE_STATUS_STOP)
+    {
+        return data;
+    }
+
+    uint8_t next_proto = (((const uint8_t *)((const char *)data + hdr_len))[0]) >> 4;
+    const void *data_next_layer = (const char *)data + hdr_len;
+    size_t data_next_length = length - hdr_len;
+
+    LOG_DEBUG("%s: pkt_trace_id: %lu, this_layer: %s, payload_len: [%lu/%lu]", LOG_TAG, handler->pkt_trace_id, layer_type2str(this_type), data_next_length, length);
+    switch (next_proto)
+    {
+    case 4:
+        // TESTED
+        return parse_ipv4(handler, data_next_layer, data_next_length, LAYER_TYPE_IPV4);
+    case 6:
+        // TESTED
+        return parse_ipv6(handler, data_next_layer, data_next_length, LAYER_TYPE_IPV6);
+    default:
+        LOG_ERROR("%s: pkt_trace_id: %lu, this_layer: %s, stop parse next protocol %d", LOG_TAG, handler->pkt_trace_id, layer_type2str(this_type), next_proto);
+        return data_next_layer;
+    }
+}
+
+static const void *parse_mpls(struct raw_pkt_parser *handler, const void *data, size_t length, enum layer_type this_type)
+{
+    if (length < 4)
+    {
+        LOG_ERROR("%s: pkt_trace_id: %lu, this_layer: %s, err: data not enough", LOG_TAG, handler->pkt_trace_id, layer_type2str(this_type));
+        return data;
+    }
+
+    if (raw_packet_parser_status(handler, data, this_type) == PARSE_STATUS_STOP)
+    {
+        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;
+    const void *data_next_layer = (const char *)data + hdr_len;
+    size_t data_next_length = length - hdr_len;
+
+    if (mpls_bls == 1)
+    {
+        uint8_t ip_version = (((uint8_t *)data_next_layer)[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
+             */
+            data_next_layer = (const char *)data_next_layer + 4;
+            data_next_length = data_next_length - 4;
+
+            LOG_DEBUG("%s: pkt_trace_id: %lu, this_layer: %s, payload_len: [%lu/%lu]", LOG_TAG, handler->pkt_trace_id, layer_type2str(this_type), data_next_length, length);
+            // TESTED
+            return parse_ether(handler, data_next_layer, data_next_length, LAYER_TYPE_ETHER);
+        }
+        else if (ip_version == 4)
+        {
+            LOG_DEBUG("%s: pkt_trace_id: %lu, this_layer: %s, payload_len: [%lu/%lu]", LOG_TAG, handler->pkt_trace_id, layer_type2str(this_type), data_next_length, length);
+            // TESTED
+            return parse_ipv4(handler, data_next_layer, data_next_length, LAYER_TYPE_IPV4);
+        }
+        else if (ip_version == 6)
+        {
+            LOG_DEBUG("%s: pkt_trace_id: %lu, this_layer: %s, payload_len: [%lu/%lu]", LOG_TAG, handler->pkt_trace_id, layer_type2str(this_type), data_next_length, length);
+            // TODO
+            return parse_ipv6(handler, data_next_layer, data_next_length, LAYER_TYPE_IPV6);
+        }
+        else
+        {
+            LOG_DEBUG("%s: pkt_trace_id: %lu, this_layer: %s, payload_len: [%lu/%lu]", LOG_TAG, handler->pkt_trace_id, layer_type2str(this_type), data_next_length, length);
+            // TODO
+            return parse_ether(handler, data_next_layer, data_next_length, LAYER_TYPE_ETHER);
+        }
+    }
+    else
+    {
+        LOG_DEBUG("%s: pkt_trace_id: %lu, this_layer: %s, payload_len: [%lu/%lu]", LOG_TAG, handler->pkt_trace_id, layer_type2str(this_type), data_next_length, length);
+        // TESTED
+        return parse_mpls(handler, data_next_layer, data_next_length, LAYER_TYPE_MPLS);
+    }
+}

common/test/CMakeLists.txt

@@ -14,6 +14,14 @@ add_executable(gtest_session_table gtest_session_table.cpp)
 target_include_directories(gtest_session_table PUBLIC ${CMAKE_SOURCE_DIR}/common/include)
 target_link_libraries(gtest_session_table common gtest)
 
+###############################################################################
+# gtest_raw_packet
+###############################################################################
+
+add_executable(gtest_raw_packet gtest_raw_packet.cpp)
+target_include_directories(gtest_raw_packet PUBLIC ${CMAKE_SOURCE_DIR}/common/include)
+target_link_libraries(gtest_raw_packet common gtest)
+
 ###############################################################################
 # gtest_discover_tests
 ###############################################################################
@@ -21,3 +29,4 @@ target_link_libraries(gtest_session_table common gtest)
 include(GoogleTest)
 gtest_discover_tests(gtest_addr_tuple4)
 gtest_discover_tests(gtest_session_table)
+gtest_discover_tests(gtest_raw_packet)