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

1 files changed, 668 insertions, 0 deletions

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