stellar-stellar/src/packet/packet.cpp

#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 <netinet/icmp6.h>
#include <netinet/ip_icmp.h>
#include <linux/ppp_defs.h>

#include "uthash.h"
#include "packet_priv.h"
#include "udp_utils.h"
#include "tcp_utils.h"
#include "ipv4_utils.h"
#include "ipv6_utils.h"

#define likely(expr) __builtin_expect((expr), 1)
#define unlikely(expr) __builtin_expect((expr), 0)

#define PACKET_LOG_DATA_INSUFFICIENCY(type)              \
    {                                                    \
        PACKET_LOG_WARN("layer: %s, data insufficiency", \
                        layer_type_to_str((type)));      \
    }

#define PACKET_LOG_UNSUPPORT_PROTO(tag, next_proto)    \
    {                                                  \
        PACKET_LOG_WARN("%s: unsupport next proto %d", \
                        (tag), (next_proto));          \
    }

#define PACKET_LOG_UNSUPPORT_ETHPROTO(tag, next_proto)                     \
    {                                                                      \
        PACKET_LOG_WARN("%s: unsupport next eth proto %d: %s",             \
                        (tag), (next_proto), ethproto_to_str(next_proto)); \
    }

#define PACKET_LOG_UNSUPPORT_IPPROTO(tag, next_proto)                     \
    {                                                                     \
        PACKET_LOG_WARN("%s: unsupport next ip proto %d: %s",             \
                        (tag), (next_proto), ipproto_to_str(next_proto)); \
    }

/******************************************************************************
 * Static API
 ******************************************************************************/

static const char *ethproto_to_str(uint16_t proto);
static const char *ipproto_to_str(uint16_t proto);
static inline const char *ldbc_method_to_str(enum ldbc_method method);
static inline const char *layer_type_to_str(enum layer_type type);

static inline void set_tuple2(const char *data, enum layer_type type, struct tuple2 *tuple);
static inline void set_tuple4(const char *data, enum layer_type type, struct tuple4 *tuple);
static inline void set_tuple6(const char *data, enum layer_type type, struct tuple6 *tuple, uint64_t domain);

static inline struct packet_layer *get_free_layer(struct packet *pkt);

static inline uint16_t get_gtp_hdr_len(const char *data, uint16_t len);
static inline uint16_t get_gre_hdr_len(const char *data, uint16_t len);
static inline uint16_t get_l2tpv2_hdr_len(const char *data, uint16_t len);

// 数据链路层
static inline const char *parse_ether(struct packet *pkt, const char *data, uint16_t len);
static inline const char *parse_ppp(struct packet *pkt, const char *data, uint16_t len);
static inline const char *parse_l2tpv2(struct packet *pkt, const char *data, uint16_t len);
static inline const char *parse_l2tpv3_over_udp(struct packet *pkt, const char *data, uint16_t len);
static inline const char *parse_l2tpv3_over_ip(struct packet *pkt, const char *data, uint16_t len);

// 数据链路层 -- 隧道
static inline const char *parse_vlan(struct packet *pkt, const char *data, uint16_t len);
static inline const char *parse_pppoe_ses(struct packet *pkt, const char *data, uint16_t len);
static inline const char *parse_mpls(struct packet *pkt, const char *data, uint16_t len);
// 网络层
static inline const char *parse_ipv4(struct packet *pkt, const char *data, uint16_t len);
static inline const char *parse_ipv6(struct packet *pkt, const char *data, uint16_t len);
// 网络层 -- 隧道
static inline const char *parse_gre(struct packet *pkt, const char *data, uint16_t len);
// 传输层
static inline const char *parse_udp(struct packet *pkt, const char *data, uint16_t len);
static inline const char *parse_tcp(struct packet *pkt, const char *data, uint16_t len);
static inline const char *parse_icmp(struct packet *pkt, const char *data, uint16_t len);
static inline const char *parse_icmp6(struct packet *pkt, const char *data, uint16_t len);
// 传输层 -- 隧道
static inline const char *parse_vxlan(struct packet *pkt, const char *data, uint16_t len);
static inline const char *parse_gtpv1_u(struct packet *pkt, const char *data, uint16_t len);
// L3/L4
static inline const char *parse_l3(struct packet *pkt, uint16_t next_proto, const char *data, uint16_t len);
static inline const char *parse_l4(struct packet *pkt, uint8_t next_proto, const char *data, uint16_t len);

/******************************************************************************
 * Private API -- Utils
 ******************************************************************************/

static const char *ethproto_to_str(uint16_t proto)
{
    switch (proto)
    {
    case ETH_P_LOOP:
        return "LOOP";
    case ETH_P_PUP:
        return "PUP";
    case ETH_P_PUPAT:
        return "PUPAT";
    case ETH_P_IP:
        return "IP";
    case ETH_P_X25:
        return "X25";
    case ETH_P_ARP:
        return "ARP";
    case ETH_P_BPQ:
        return "BPQ";
    case ETH_P_IEEEPUP:
        return "IEEEPUP";
    case ETH_P_IEEEPUPAT:
        return "IEEEPUPAT";
    case ETH_P_DEC:
        return "DEC";
    case ETH_P_DNA_DL:
        return "DNA_DL";
    case ETH_P_DNA_RC:
        return "DNA_RC";
    case ETH_P_DNA_RT:
        return "DNA_RT";
    case ETH_P_LAT:
        return "LAT";
    case ETH_P_DIAG:
        return "DIAG";
    case ETH_P_CUST:
        return "CUST";
    case ETH_P_SCA:
        return "SCA";
    case ETH_P_TEB:
        return "TEB";
    case ETH_P_RARP:
        return "RARP";
    case ETH_P_ATALK:
        return "ATALK";
    case ETH_P_AARP:
        return "AARP";
    case ETH_P_8021Q:
        return "8021Q";
    case ETH_P_IPX:
        return "IPX";
    case ETH_P_IPV6:
        return "IPV6";
    case ETH_P_PAUSE:
        return "PAUSE";
    case ETH_P_SLOW:
        return "SLOW";
    case ETH_P_WCCP:
        return "WCCP";
    case ETH_P_PPP_DISC:
        return "PPP_DISC";
    case ETH_P_PPP_SES:
        return "PPP_SES";
    case ETH_P_MPLS_UC:
        return "MPLS_UC";
    case ETH_P_MPLS_MC:
        return "MPLS_MC";
    case ETH_P_ATMMPOA:
        return "ATMMPOA";
    case ETH_P_LINK_CTL:
        return "LINK_CTL";
    case ETH_P_ATMFATE:
        return "ATMFATE";
    case ETH_P_PAE:
        return "PAE";
    case ETH_P_AOE:
        return "AOE";
    case ETH_P_8021AD:
        return "8021AD";
    case ETH_P_802_EX1:
        return "802_EX1";
    case ETH_P_TIPC:
        return "TIPC";
    case ETH_P_8021AH:
        return "8021AH";
    case ETH_P_1588:
        return "1588";
    case ETH_P_FCOE:
        return "FCOE";
    case ETH_P_TDLS:
        return "TDLS";
    case ETH_P_FIP:
        return "FIP";
    case ETH_P_QINQ1:
        return "QINQ1";
    case ETH_P_QINQ2:
        return "QINQ2";
    case ETH_P_QINQ3:
        return "QINQ3";
    case ETH_P_EDSA:
        return "EDSA";
    case ETH_P_AF_IUCV:
        return "AF_IUCV";
    default:
        return "UNKNOWN";
    }
}

static const char *ipproto_to_str(uint16_t proto)
{
    switch (proto)
    {
    case IPPROTO_IP:
        return "IP";
    case IPPROTO_ICMP:
        return "ICMP";
    case IPPROTO_IGMP:
        return "IGMP";
    case IPPROTO_IPIP:
        return "IPIP";
    case IPPROTO_TCP:
        return "TCP";
    case IPPROTO_EGP:
        return "EGP";
    case IPPROTO_PUP:
        return "PUP";
    case IPPROTO_UDP:
        return "UDP";
    case IPPROTO_IDP:
        return "IDP";
    case IPPROTO_TP:
        return "TP";
    case IPPROTO_DCCP:
        return "DCCP";
    case IPPROTO_IPV6:
        return "IPV6";
    case IPPROTO_ROUTING:
        return "ROUTING";
    case IPPROTO_FRAGMENT:
        return "FRAGMENT";
    case IPPROTO_RSVP:
        return "RSVP";
    case IPPROTO_GRE:
        return "GRE";
    case IPPROTO_ESP:
        return "ESP";
    case IPPROTO_AH:
        return "AH";
    case IPPROTO_ICMPV6:
        return "ICMPV6";
    case IPPROTO_NONE:
        return "NONE";
    case IPPROTO_DSTOPTS:
        return "DSTOPTS";
    case IPPROTO_MTP:
        return "MTP";
    case IPPROTO_ENCAP:
        return "ENCAP";
    case IPPROTO_PIM:
        return "PIM";
    case IPPROTO_COMP:
        return "COMP";
    case IPPROTO_SCTP:
        return "SCTP";
    case IPPROTO_UDPLITE:
        return "UDPLITE";
    case IPPROTO_RAW:
        return "RAW";
    default:
        return "UNKNOWN";
    }
}

static inline const char *ldbc_method_to_str(enum ldbc_method method)
{
    switch (method)
    {
    case LDBC_METHOD_HASH_INT_IP:
        return "outter_internal_ip";
    case LDBC_METHOD_HASH_EXT_IP:
        return "outter_external_ip";
    case LDBC_METHOD_HASH_INT_IP_AND_EXT_IP:
        return "outter_internal_ip_and_external_ip";
    case LDBC_METHOD_HASH_INNERMOST_INT_IP:
        return "inner_internal_ip";
    case LDBC_METHOD_HASH_INNERMOST_EXT_IP:
        return "inner_external_ip";
    default:
        return "unknown";
    }
}

static inline const char *layer_type_to_str(enum layer_type type)
{
    switch (type)
    {
    case LAYER_TYPE_ETHER:
        return "ETH";
    case LAYER_TYPE_PPP:
        return "PPP";
    case LAYER_TYPE_HDLC:
        return "HDLC";
    case LAYER_TYPE_L2TP:
        return "L2TP";
    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_GRE:
        return "GRE";
    case LAYER_TYPE_UDP:
        return "UDP";
    case LAYER_TYPE_TCP:
        return "TCP";
    case LAYER_TYPE_ICMP:
        return "ICMP";
    case LAYER_TYPE_ICMP6:
        return "ICMP6";
    case LAYER_TYPE_VXLAN:
        return "VXLAN";
    case LAYER_TYPE_GTPV1_U:
        return "GTPV1";
    default:
        return "UNKNOWN";
    }
}

static inline void set_tuple2(const char *data, enum layer_type type, struct tuple2 *tuple)
{
    const struct ip *ipv4 = NULL;
    const struct ip6_hdr *ipv6 = NULL;

    switch (type)
    {
    case LAYER_TYPE_IPV4:
        ipv4 = (const struct ip *)data;
        tuple->ip_type = IP_TYPE_V4;
        tuple->src_addr.v4.s_addr = ipv4->ip_src.s_addr;
        tuple->dst_addr.v4.s_addr = ipv4->ip_dst.s_addr;
        break;
    case LAYER_TYPE_IPV6:
        ipv6 = (const struct ip6_hdr *)data;
        tuple->ip_type = IP_TYPE_V6;
        tuple->src_addr.v6 = ipv6->ip6_src;
        tuple->dst_addr.v6 = ipv6->ip6_dst;
        break;
    default:
        break;
    }
}

static inline void set_tuple4(const char *data, enum layer_type type, struct tuple4 *tuple)
{
    const struct ip *ipv4 = NULL;
    const struct ip6_hdr *ipv6 = NULL;
    const struct tcphdr *tcp = NULL;
    const struct udphdr *udp = NULL;

    switch (type)
    {
    case LAYER_TYPE_TCP:
        tcp = (const struct tcphdr *)data;
        tuple->src_port = tcp->th_sport;
        tuple->dst_port = tcp->th_dport;
        break;
    case LAYER_TYPE_UDP:
        udp = (const struct udphdr *)data;
        tuple->src_port = udp->uh_sport;
        tuple->dst_port = udp->uh_dport;
        break;
    case LAYER_TYPE_IPV4:
        ipv4 = (const struct ip *)data;
        tuple->ip_type = IP_TYPE_V4;
        tuple->src_addr.v4.s_addr = ipv4->ip_src.s_addr;
        tuple->dst_addr.v4.s_addr = ipv4->ip_dst.s_addr;
        break;
    case LAYER_TYPE_IPV6:
        ipv6 = (const struct ip6_hdr *)data;
        tuple->ip_type = IP_TYPE_V6;
        tuple->src_addr.v6 = ipv6->ip6_src;
        tuple->dst_addr.v6 = ipv6->ip6_dst;
        break;
    default:
        break;
    }
}

static inline void set_tuple6(const char *data, enum layer_type type, struct tuple6 *tuple, uint64_t domain)
{
    const struct ip *ipv4 = NULL;
    const struct ip6_hdr *ipv6 = NULL;
    const struct tcphdr *tcp = NULL;
    const struct udphdr *udp = NULL;

    tuple->domain = domain;

    switch (type)
    {
    case LAYER_TYPE_TCP:
        tcp = (const struct tcphdr *)data;
        tuple->ip_proto = IPPROTO_TCP;
        tuple->src_port = tcp->th_sport;
        tuple->dst_port = tcp->th_dport;
        break;
    case LAYER_TYPE_UDP:
        udp = (const struct udphdr *)data;
        tuple->ip_proto = IPPROTO_UDP;
        tuple->src_port = udp->uh_sport;
        tuple->dst_port = udp->uh_dport;
        break;
    case LAYER_TYPE_IPV4:
        ipv4 = (const struct ip *)data;
        tuple->ip_type = IP_TYPE_V4;
        tuple->src_addr.v4.s_addr = ipv4->ip_src.s_addr;
        tuple->dst_addr.v4.s_addr = ipv4->ip_dst.s_addr;
        break;
    case LAYER_TYPE_IPV6:
        ipv6 = (const struct ip6_hdr *)data;
        tuple->ip_type = IP_TYPE_V6;
        tuple->src_addr.v6 = ipv6->ip6_src;
        tuple->dst_addr.v6 = ipv6->ip6_dst;
        break;
    default:
        break;
    }
}

static inline struct packet_layer *get_free_layer(struct packet *pkt)
{
    if (pkt->layers_used >= pkt->layers_size)
    {
        return NULL;
    }

    return &pkt->layers[pkt->layers_used];
}

#define SET_LAYER(_pkt, _layer, _type, _hdr_len, _data, _len, _trim)                                                        \
    {                                                                                                                       \
        (_layer)->type = (_type);                                                                                           \
        (_layer)->hdr_offset = (_pkt)->data_len - (_pkt)->trim_len - (_len);                                                \
        (_layer)->hdr_ptr = (_data);                                                                                        \
        (_layer)->hdr_len = (_hdr_len);                                                                                     \
        (_layer)->pld_ptr = (_data) + (_hdr_len);                                                                           \
        (_layer)->pld_len = (_len) - (_hdr_len) - (_trim);                                                                  \
        (_pkt)->trim_len += (_trim);                                                                                        \
        (_pkt)->layers_used++;                                                                                              \
        PACKET_LOG_DEBUG("layer[%d/%d]: %s, hdr_offset: %d, hdr_ptr: %p, hdr_len: %d, pld_ptr: %p, pld_len: %d",            \
                         (_pkt)->layers_used - 1, (_pkt)->layers_size, layer_type_to_str((_type)),                          \
                         (_layer)->hdr_offset, (_layer)->hdr_ptr, (_layer)->hdr_len, (_layer)->pld_ptr, (_layer)->pld_len); \
    }

/******************************************************************************
 * Private API -- Parses
 ******************************************************************************/

static inline uint16_t get_gtp_hdr_len(const char *data, uint16_t len)
{
#define GTP_HDR_VER (0xE0)
#define GTP_HDR_FLAG_N_PDU (0x01)
#define GTP_HDR_FLAG_SEQ_NUM (0x02)
#define GTP_HDR_FLAG_EXT_HDR (0x04)

    struct gtp_hdr
    {
        uint8_t flags;
        uint8_t msg_type;
        uint16_t msg_len;
        uint32_t teid;
    } __attribute__((__packed__));

    struct gtp_opt
    {
        uint16_t seq_num;
        uint8_t npdu;
        uint8_t next_ext_hdr;
    } __attribute__((__packed__));

    uint16_t hdr_offset = 0;
    if (len < sizeof(struct gtp_hdr))
    {
        return 0;
    }
    const struct gtp_hdr *gtp = (const struct gtp_hdr *)data;
    hdr_offset += sizeof(struct gtp_hdr); // skip gre hdr

    // GTPv0 Not Supported
    if (((gtp->flags & GTP_HDR_VER) >> 5) != 1)
    {
        return 0;
    }

    if (gtp->flags & (GTP_HDR_FLAG_SEQ_NUM | GTP_HDR_FLAG_N_PDU | GTP_HDR_FLAG_EXT_HDR))
    {
        if (hdr_offset + sizeof(struct gtp_opt) > len)
        {
            return 0;
        }
        struct gtp_opt *opt_hdr = (struct gtp_opt *)((char *)data + hdr_offset);
        uint8_t next_ext_hdr = opt_hdr->next_ext_hdr;
        hdr_offset += sizeof(struct gtp_opt); // skip gre opt

        while (next_ext_hdr)
        {
            if (hdr_offset + 1 > len)
            {
                return 0;
            }
            uint8_t length = *((char *)data + hdr_offset) * 4 - 2;
            hdr_offset += 1; // skip length field

            if (hdr_offset + length + 1 > len)
            {
                return 0;
            }
            hdr_offset += length; // skip data field
            next_ext_hdr = *((char *)data + hdr_offset);
            hdr_offset += 1; // skip next ext hdr field
        }
    }

    return hdr_offset;
}

static inline uint16_t get_gre_hdr_len(const char *data, uint16_t len)
{
    /*
     * GRE Header Format (Version 0)
     *
     *   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
     *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
     *  |C|R|K|S|s|Recur|  Flags  | Ver |         Protocol Type         |
     *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
     *  |      Checksum (optional)      |       Offset (optional)       |
     *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
     *  |                         Key (optional)                        |
     *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
     *  |                    Sequence Number (optional)                 |
     *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
     *  |                         Routing (optional)
     *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
     *
     *   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
     *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
     *  |       Address Family          |  SRE Offset   |  SRE Length   |
     *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
     *  |                        Routing Information ...
     *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
     *
     * https://datatracker.ietf.org/doc/html/rfc1701
     * https://datatracker.ietf.org/doc/html/rfc2890
     */

    /*
     * Enhanced GRE header (Version 1)
     *
     *   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
     *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
     *  |C|R|K|S|s|Recur|A| Flags | Ver |         Protocol Type         |
     *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
     *  |    Key (HW) Payload Length    |       Key (LW) Call ID        |
     *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
     *  |                  Sequence Number (Optional)                   |
     *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
     *  |               Acknowledgment Number (Optional)                |
     *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
     *
     * https://datatracker.ietf.org/doc/html/rfc2637
     */

/* bit positions for flags in header */
#define GRE_CHECKSUM 0x8000
#define GRE_ROUTING 0x4000
#define GRE_KEY 0x2000
#define GRE_SEQUENCE 0x1000
#define GRE_STRICTSOURCE 0x0800
#define GRE_RECURSION 0x0700
#define GRE_ACK 0x0080          /* only in special PPTPized GRE header */
#define GRE_RESERVED_PPP 0x0078 /* only in special PPTPized GRE header */
#define GRE_RESERVED 0x00F8
#define GRE_VERSION 0x0007

    if (len < 4)
    {
        return 0;
    }

    struct SRE
    {
        uint16_t address_family;
        uint8_t sre_offset;
        uint8_t sre_length;
    } __attribute__((__packed__));

    uint16_t sre_size = sizeof(struct SRE);
    const uint16_t *gre = (const uint16_t *)data;
    uint16_t flags = ntohs(gre[0]);
    uint16_t version = flags & GRE_VERSION;
    uint16_t hdr_offset = 0;

    if (version == 0)
    {
        hdr_offset = 4;

        if ((flags & GRE_CHECKSUM) || (flags & GRE_ROUTING))
        {
            hdr_offset += 4;
        }
        if (flags & GRE_KEY)
        {
            hdr_offset += 4;
        }
        if (flags & GRE_SEQUENCE)
        {
            hdr_offset += 4;
        }
        if (flags & GRE_ROUTING)
        {
            while (hdr_offset + sre_size <= len)
            {
                struct SRE *sre = (struct SRE *)((char *)data + hdr_offset);
                if (sre->sre_length == 0)
                {
                    hdr_offset += sre_size;
                    break;
                }
                else
                {
                    hdr_offset += sre_size + sre->sre_length;
                }
            }
        }
    }

    if (version == 1)
    {
        hdr_offset = 8;
        if (flags & GRE_SEQUENCE)
        {
            hdr_offset += 4;
        }
        if (flags & GRE_ACK)
        {
            hdr_offset += 4;
        }
    }

    if (hdr_offset > len)
    {
        return 0;
    }

    return hdr_offset;
}

static inline uint16_t get_l2tpv2_hdr_len(const char *data, uint16_t len)
{
    /*
     * Layer Two Tunneling Protocol "L2TP"
     * https://datatracker.ietf.org/doc/html/rfc2661
     *
     * 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
     * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
     * |T|L|x|x|S|x|O|P|x|x|x|x|  Ver  |          Length (opt)         |
     * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
     * |           Tunnel ID           |           Session ID          |
     * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
     * |             Ns (opt)          |             Nr (opt)          |
     * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
     * |      Offset Size (opt)        |    Offset pad... (opt)
     * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
     *
     * Figure 3.1 L2TP Message Header
     */

#define CONTROL_BIT(msg_info) (msg_info & 0x8000)  // Type bit control = 1 data = 0
#define LENGTH_BIT(msg_info) (msg_info & 0x4000)   // Length bit = 1
#define SEQUENCE_BIT(msg_info) (msg_info & 0x0800) // SEQUENCE bit = 1 Ns and Nr fields
#define OFFSET_BIT(msg_info) (msg_info & 0x0200)   // Offset
#define PRIORITY_BIT(msg_info) (msg_info & 0x0100) // Priority
#define L2TP_VERSION(msg_info) (msg_info & 0x000f) // Version of l2tp

    if (unlikely(len < 6))
    {
        return 0;
    }

    uint16_t control = ntohs(*((uint16_t *)data));
    if (L2TP_VERSION(control) != 2)
    {
        return 0;
    }

    if (CONTROL_BIT(control))
    {
        if (LENGTH_BIT(control) != 1 || SEQUENCE_BIT(control) != 1 || OFFSET_BIT(control) != 0 || PRIORITY_BIT(control) != 0)
        {
            return 0;
        }
        else
        {
            return ntohs(*((uint16_t *)(data + 2)));
        }
    }
    else
    {
        uint16_t skip_len = 2;
        if (LENGTH_BIT(control))
        {
            skip_len += 2; // skip length field
        }
        skip_len += 2; // skip tunnel id field
        skip_len += 2; // skip session id field
        if (SEQUENCE_BIT(control))
        {
            skip_len += 2; // skip ns field
            skip_len += 2; // skip nr field
        }
        if (OFFSET_BIT(control))
        {
            if (skip_len + 2 > len)
            {
                return 0;
            }
            uint16_t offset = ntohs(*((uint16_t *)(data + skip_len)));
            if (offset == 0)
            {
                return skip_len + 2;
            }

            // invalid offset
            if (offset > len)
            {
                return 0;
            }
            else
            {
                return offset;
            }
        }
        else
        {
            return skip_len;
        }
    }
}

static inline const char *parse_ether(struct packet *pkt, const char *data, uint16_t len)
{
    if (unlikely(len < sizeof(struct ethhdr)))
    {
        PACKET_LOG_DATA_INSUFFICIENCY(LAYER_TYPE_ETHER);
        return data;
    }

    struct packet_layer *layer = get_free_layer(pkt);
    if (unlikely(layer == NULL))
    {
        return data;
    }
    uint16_t next_proto = ntohs(((struct ethhdr *)data)->h_proto);
    SET_LAYER(pkt, layer, LAYER_TYPE_ETHER, sizeof(struct ethhdr), data, len, 0);

    // TESTED
    return parse_l3(pkt, next_proto, layer->pld_ptr, layer->pld_len);
}

static inline int next_proto_is_ppp(uint16_t next_proto)
{
    // /usr/include/linux/ppp_defs.h.html
    switch (next_proto)
    {
    case PPP_IP:         /* Internet Protocol */
    case PPP_AT:         /* AppleTalk Protocol */
    case PPP_IPX:        /* IPX protocol */
    case PPP_VJC_COMP:   /* VJ compressed TCP */
    case PPP_VJC_UNCOMP: /* VJ uncompressed TCP */
    case PPP_MP:         /* Multilink protocol */
    case PPP_IPV6:       /* Internet Protocol Version 6 */
    case PPP_COMPFRAG:   /* fragment compressed below bundle */
    case PPP_COMP:       /* compressed packet */
    case PPP_MPLS_UC:    /* Multi Protocol Label Switching - Unicast */
    case PPP_MPLS_MC:    /* Multi Protocol Label Switching - Multicast */
    case PPP_IPCP:       /* IP Control Protocol */
    case PPP_ATCP:       /* AppleTalk Control Protocol */
    case PPP_IPXCP:      /* IPX Control Protocol */
    case PPP_IPV6CP:     /* IPv6 Control Protocol */
    case PPP_CCPFRAG:    /* CCP at link level (below MP bundle) */
    // case PPP_CCP:        /* Compression Control Protocol */ (same as PPP_CCPFRAG)
    case PPP_MPLSCP: /* MPLS Control Protocol */
    case PPP_LCP:    /* Link Control Protocol */
    case PPP_PAP:    /* Password Authentication Protocol */
    case PPP_LQR:    /* Link Quality Report protocol */
    case PPP_CHAP:   /* Cryptographic Handshake Auth. Protocol */
    case PPP_CBCP:   /* Callback Control Protocol */
        return 1;
    default:
        return 0;
    }
}

static inline const char *parse_ppp(struct packet *pkt, const char *data, uint16_t len)
{
    /*
     * https://datatracker.ietf.org/doc/html/rfc1661#section-2
     * +----------+-------------+---------+
     * | Protocol | Information | Padding |
     * | 8/16 bits|      *      |    *    |
     * +----------+-------------+---------+
     *
     * https://datatracker.ietf.org/doc/html/rfc1331#section-3.1
     * +----------+----------+----------+----------+------------
     * |   Flag   | Address  | Control  | Protocol | Information
     * | 01111110 | 11111111 | 00000011 | 16 bits  |      *
     * +----------+----------+----------+----------+------------
     *         ---+----------+----------+-----------------
     *            |   FCS    |   Flag   | Inter-frame Fill
     *            | 16 bits  | 01111110 | or next Address
     *         ---+----------+----------+-----------------
     */
    if (unlikely(len < 4))
    {
        PACKET_LOG_DATA_INSUFFICIENCY(LAYER_TYPE_PPP);
        return data;
    }

    uint16_t hdr_len = 0;
    uint16_t next_proto = 0;

    // ppp header 1 byte
    next_proto = *((uint8_t *)data);
    if (next_proto_is_ppp(next_proto))
    {
        hdr_len = 1;
        goto success;
    }

    // ppp header 2 bytes
    next_proto = ntohs(*((uint16_t *)data));
    if (next_proto_is_ppp(next_proto))
    {
        hdr_len = 2;
        goto success;
    }

    // ppp header 4 bytes
    next_proto = ntohs(*((uint16_t *)data + 1));
    hdr_len = 4;

success:
    struct packet_layer *layer = get_free_layer(pkt);
    if (unlikely(layer == NULL))
    {
        return data;
    }
    SET_LAYER(pkt, layer, LAYER_TYPE_PPP, hdr_len, data, len, 0);
    switch (next_proto)
    {
        // TESTED
    case PPP_IP:
        return parse_ipv4(pkt, layer->pld_ptr, layer->pld_len);
    case PPP_IPV6:
        return parse_ipv6(pkt, layer->pld_ptr, layer->pld_len);
    default:
        PACKET_LOG_UNSUPPORT_PROTO("ppp", next_proto);
        return layer->pld_ptr;
    }
}

static inline const char *parse_l2tpv2(struct packet *pkt, const char *data, uint16_t len)
{
#define CONTROL_BIT(msg_info) (msg_info & 0x8000) // Type bit control = 1 data = 0

    uint16_t hdr_len = get_l2tpv2_hdr_len(data, len);
    if (unlikely(hdr_len == 0 || hdr_len > len))
    {
        PACKET_LOG_DATA_INSUFFICIENCY(LAYER_TYPE_L2TP);
        return data;
    }

    struct packet_layer *layer = get_free_layer(pkt);
    if (unlikely(layer == NULL))
    {
        return data;
    }
    SET_LAYER(pkt, layer, LAYER_TYPE_L2TP, hdr_len, data, len, 0);

    uint16_t control = ntohs(*((uint16_t *)data));
    if (CONTROL_BIT(control))
    {
        return layer->pld_ptr;
    }
    else
    {
        return parse_ppp(pkt, layer->pld_ptr, layer->pld_len);
    }
}

static inline const char *parse_l2tpv3_over_udp(struct packet *pkt, const char *data, uint16_t len)
{
    // TODO
    return data;
}

static inline const char *parse_l2tpv3_over_ip(struct packet *pkt, const char *data, uint16_t len)
{
    // TODO
    return data;
}

static inline const char *parse_vlan(struct packet *pkt, const char *data, uint16_t len)
{
    struct vlan_hdr
    {
        uint16_t vlan_cfi;
        uint16_t protocol;
    } __attribute__((__packed__));

    if (unlikely(len < sizeof(struct vlan_hdr)))
    {
        PACKET_LOG_DATA_INSUFFICIENCY(LAYER_TYPE_VLAN);
        return data;
    }

    struct packet_layer *layer = get_free_layer(pkt);
    if (unlikely(layer == NULL))
    {
        return data;
    }
    uint16_t next_proto = ntohs(((struct vlan_hdr *)data)->protocol);
    SET_LAYER(pkt, layer, LAYER_TYPE_VLAN, sizeof(struct vlan_hdr), data, len, 0);

    // TESTED
    return parse_l3(pkt, next_proto, layer->pld_ptr, layer->pld_len);
}

static inline const char *parse_pppoe_ses(struct packet *pkt, const char *data, uint16_t len)
{

    if (unlikely(len < 6))
    {
        PACKET_LOG_DATA_INSUFFICIENCY(LAYER_TYPE_PPPOE);
        return data;
    }

    struct packet_layer *layer = get_free_layer(pkt);
    if (unlikely(layer == NULL))
    {
        return data;
    }
    SET_LAYER(pkt, layer, LAYER_TYPE_PPPOE, 6, data, len, 0);

    // TESTED
    return parse_ppp(pkt, layer->pld_ptr, layer->pld_len);
}

static inline const char *parse_mpls(struct packet *pkt, const char *data, uint16_t len)
{
    /*
     * 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
     */

#define MPLS_LABEL_MASK (0xFFFFF000)
#define MPLS_EXP_MASK (0x00000E00)
#define MPLS_BLS_MASK (0x00000100)
#define MPLS_TTL_MASK (0x000000FF)

    enum mpls_next_proto
    {
        MPLS_NEXT_PROTO_ETHER = 0x0,
        MPLS_NEXT_PROTO_MPLS = 0x1,
        MPLS_NEXT_PROTO_IPV4 = 0x4,
        MPLS_NEXT_PROTO_IPV6 = 0x6,
    };

    // 4 + 1
    if (unlikely(len < 5))
    {
        PACKET_LOG_DATA_INSUFFICIENCY(LAYER_TYPE_MPLS);
        return data;
    }

    struct packet_layer *layer = get_free_layer(pkt);
    if (unlikely(layer == NULL))
    {
        return data;
    }

    uint16_t hdr_len = 4;
    uint32_t *hdr = (uint32_t *)data;
    unsigned int mpls_bls = (ntohl(*hdr) & MPLS_BLS_MASK) >> 8;
    enum mpls_next_proto next_proto;
    if (mpls_bls == 1)
    {
        switch ((((uint8_t *)(data + 4))[0]) >> 4)
        {
        case 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
             */
            hdr_len += 4; // skip PW Ethernet Control Word
            next_proto = MPLS_NEXT_PROTO_ETHER;
            break;
        case 4:
            next_proto = MPLS_NEXT_PROTO_IPV4;
            break;
        case 6:
            next_proto = MPLS_NEXT_PROTO_IPV6;
            break;
        default:
            next_proto = MPLS_NEXT_PROTO_ETHER;
            break;
        }
    }
    else
    {
        next_proto = MPLS_NEXT_PROTO_MPLS;
    }
    if (unlikely(hdr_len > len))
    {
        PACKET_LOG_DATA_INSUFFICIENCY(LAYER_TYPE_MPLS);
        return data;
    }
    SET_LAYER(pkt, layer, LAYER_TYPE_MPLS, hdr_len, data, len, 0);

    switch (next_proto)
    {
        // TESTED
    case MPLS_NEXT_PROTO_IPV4:
        return parse_ipv4(pkt, layer->pld_ptr, layer->pld_len);
    case MPLS_NEXT_PROTO_IPV6:
        return parse_ipv6(pkt, layer->pld_ptr, layer->pld_len);
        // TESTED
    case MPLS_NEXT_PROTO_ETHER:
        return parse_ether(pkt, layer->pld_ptr, layer->pld_len);
        // TESTED
    case MPLS_NEXT_PROTO_MPLS:
        return parse_mpls(pkt, layer->pld_ptr, layer->pld_len);
    default:
        // unreachable
        return layer->pld_ptr;
    }
}

static inline const char *parse_ipv4(struct packet *pkt, const char *data, uint16_t len)
{
    if (unlikely(len < sizeof(struct ip)))
    {
        PACKET_LOG_DATA_INSUFFICIENCY(LAYER_TYPE_IPV4);
        return data;
    }

    struct packet_layer *layer = get_free_layer(pkt);
    if (unlikely(layer == NULL))
    {
        return data;
    }
    struct ip *hdr = (struct ip *)data;
    uint16_t hdr_len = ipv4_hdr_get_hdr_len(hdr);
    if (unlikely(hdr_len > len))
    {
        PACKET_LOG_DATA_INSUFFICIENCY(LAYER_TYPE_IPV4);
        return data;
    }

    uint16_t total_len = ipv4_hdr_get_total_len(hdr);
    if (unlikely(total_len > len))
    {
        PACKET_LOG_DATA_INSUFFICIENCY(LAYER_TYPE_IPV4);
        return data;
    }
    uint16_t trim_len = len - total_len;
    SET_LAYER(pkt, layer, LAYER_TYPE_IPV4, hdr_len, data, len, trim_len);

    // ip fragmented
    if (ipv4_hdr_get_mf_flag(hdr) || ipv4_hdr_get_frag_offset(hdr))
    {
        PACKET_LOG_WARN("packet %p ip layer %p is fragmented", pkt, layer);
        pkt->frag_layer = layer;
        // try continue parse
    }

    // TESTED
    uint8_t next_proto = ipv4_hdr_get_proto(hdr);
    return parse_l4(pkt, next_proto, layer->pld_ptr, layer->pld_len);
}

static inline const char *parse_ipv6(struct packet *pkt, const char *data, uint16_t len)
{
    /*
     * IP6 Extension Headers
     *
     * Internet Protocol, Version 6 (IPv6)     : https://datatracker.ietf.org/doc/html/rfc2460
     * IP Encapsulating Security Payload (ESP) : https://datatracker.ietf.org/doc/html/rfc2406
     * IP Authentication Header                : https://datatracker.ietf.org/doc/html/rfc4302
     *
     * skip next header
     *      #define IPPROTO_HOPOPTS   0 // IP6 hop-by-hop options
     *      #define IPPROTO_ROUTING  43 // IP6 routing header
     *      #define IPPROTO_AH       51 // IP6 Auth Header
     *      #define IPPROTO_DSTOPTS  60 // IP6 destination option
     *
     * not skip next header
     *      #define IPPROTO_FRAGMENT 44 // IP6 fragmentation header
     *      #define IPPROTO_ESP      50 // IP6 Encap Sec. Payload
     *      #define IPPROTO_NONE     59 // IP6 no next header
     */

    if (unlikely(len < sizeof(struct ip6_hdr)))
    {
        PACKET_LOG_DATA_INSUFFICIENCY(LAYER_TYPE_IPV6);
        return data;
    }

    struct packet_layer *layer = get_free_layer(pkt);
    if (unlikely(layer == NULL))
    {
        return data;
    }
    uint16_t pld_len = ipv6_hdr_get_payload_len((const struct ip6_hdr *)data);
    if (unlikely(pld_len + sizeof(struct ip6_hdr) > len))
    {
        PACKET_LOG_DATA_INSUFFICIENCY(LAYER_TYPE_IPV6);
        return data;
    }
    uint8_t next_proto = ipv6_hdr_get_next_header((const struct ip6_hdr *)data);
    uint16_t hdr_len = sizeof(struct ip6_hdr);
    uint16_t trim_len = len - pld_len - sizeof(struct ip6_hdr);
    const char *next_hdr_ptr = data + hdr_len;
    while (next_proto == IPPROTO_HOPOPTS || next_proto == IPPROTO_ROUTING || next_proto == IPPROTO_AH || next_proto == IPPROTO_DSTOPTS)
    {
        if (unlikely(pld_len < 2))
        {
            PACKET_LOG_DATA_INSUFFICIENCY(LAYER_TYPE_IPV6);
            return data;
        }
        struct ip6_ext *ext = (struct ip6_ext *)next_hdr_ptr;
        uint16_t skip_len = ext->ip6e_len * 8 + 8;
        if (unlikely(skip_len > pld_len))
        {
            PACKET_LOG_DATA_INSUFFICIENCY(LAYER_TYPE_IPV6);
            return data;
        }
        hdr_len += skip_len;
        pld_len -= skip_len;
        next_hdr_ptr += skip_len;
        next_proto = ext->ip6e_nxt;
    }
    SET_LAYER(pkt, layer, LAYER_TYPE_IPV6, hdr_len, data, len, trim_len);

    // ipv6 fragment
    if (next_proto == IPPROTO_FRAGMENT)
    {
        PACKET_LOG_WARN("packet %p ipv6 layer %p is fragmented", pkt, layer);
        pkt->frag_layer = layer;
        // try continue parse
    }

    // TESTED
    return parse_l4(pkt, next_proto, layer->pld_ptr, layer->pld_len);
}

static inline const char *parse_gre(struct packet *pkt, const char *data, uint16_t len)
{
#define GRE_PRO_IPV4 (0x0800)
#define GRE_PRO_IPV6 (0x86DD)
#define GRE_PRO_ARP (0x0806)
#define GRE_PRO_PPP (0x880B)

    uint16_t hdr_len = get_gre_hdr_len(data, len);
    if (unlikely(hdr_len == 0 || hdr_len > len))
    {
        PACKET_LOG_DATA_INSUFFICIENCY(LAYER_TYPE_GRE);
        return data;
    }

    struct packet_layer *layer = get_free_layer(pkt);
    if (unlikely(layer == NULL))
    {
        return data;
    }
    uint16_t next_proto = ntohs(*((uint16_t *)data + 1));
    SET_LAYER(pkt, layer, LAYER_TYPE_GRE, hdr_len, data, len, 0);

    switch (next_proto)
    {
    case GRE_PRO_IPV4:
        return parse_ipv4(pkt, layer->pld_ptr, layer->pld_len);
    case GRE_PRO_IPV6:
        return parse_ipv6(pkt, layer->pld_ptr, layer->pld_len);
        // TESTED
    case GRE_PRO_PPP:
        return parse_ppp(pkt, layer->pld_ptr, layer->pld_len);
    default:
        PACKET_LOG_UNSUPPORT_PROTO("gre", next_proto);
        return layer->pld_ptr;
    }
}

static inline const char *parse_udp(struct packet *pkt, const char *data, uint16_t len)
{
    if (unlikely(len < sizeof(struct udphdr)))
    {
        PACKET_LOG_DATA_INSUFFICIENCY(LAYER_TYPE_UDP);
        return data;
    }

    struct packet_layer *layer = get_free_layer(pkt);
    if (unlikely(layer == NULL))
    {
        return data;
    }
    struct udphdr *hdr = (struct udphdr *)data;
    SET_LAYER(pkt, layer, LAYER_TYPE_UDP, sizeof(struct udphdr), data, len, 0);

    if (udp_hdr_get_dst_port(hdr) == 4789)
    {
        // TESTED
        // VXLAN_DPORT 4789
        return parse_vxlan(pkt, layer->pld_ptr, layer->pld_len);
    }

    if (udp_hdr_get_dst_port(hdr) == 2152 || udp_hdr_get_src_port(hdr) == 2152)
    {
        // TESTED
        // GTP1U_PORT 2152
        return parse_gtpv1_u(pkt, layer->pld_ptr, layer->pld_len);
    }

    if (udp_hdr_get_dst_port(hdr) == 1701)
    {
        // L2TP_DPORT 1701
        if (unlikely(layer->pld_len < 8))
        {
            return layer->pld_ptr;
        }

        uint16_t control = ntohs(*((uint16_t *)layer->pld_ptr));
        switch (L2TP_VERSION(control))
        {
        case 2:
            // TESTED
            return parse_l2tpv2(pkt, layer->pld_ptr, layer->pld_len);
        case 3:
            return parse_l2tpv3_over_udp(pkt, layer->pld_ptr, layer->pld_len);
        default:
            return layer->pld_ptr;
        }
    }

    return layer->pld_ptr;
}

static inline const char *parse_tcp(struct packet *pkt, const char *data, uint16_t len)
{
    if (unlikely(len < sizeof(struct tcphdr)))
    {
        PACKET_LOG_DATA_INSUFFICIENCY(LAYER_TYPE_TCP);
        return data;
    }

    struct packet_layer *layer = get_free_layer(pkt);
    if (unlikely(layer == NULL))
    {
        return data;
    }
    uint16_t hdr_len = tcp_hdr_get_hdr_len((struct tcphdr *)data);
    if (unlikely(hdr_len > len))
    {
        PACKET_LOG_DATA_INSUFFICIENCY(LAYER_TYPE_TCP);
        return data;
    }
    SET_LAYER(pkt, layer, LAYER_TYPE_TCP, hdr_len, data, len, 0);

    return layer->pld_ptr;
}

static inline const char *parse_icmp(struct packet *pkt, const char *data, uint16_t len)
{
    if (unlikely(len < sizeof(struct icmphdr)))
    {
        PACKET_LOG_DATA_INSUFFICIENCY(LAYER_TYPE_ICMP);
        return data;
    }

    struct packet_layer *layer = get_free_layer(pkt);
    if (unlikely(layer == NULL))
    {
        return data;
    }
    SET_LAYER(pkt, layer, LAYER_TYPE_ICMP, sizeof(struct icmphdr), data, len, 0);

    return layer->pld_ptr;
}

static inline const char *parse_icmp6(struct packet *pkt, const char *data, uint16_t len)
{
    if (unlikely(len < sizeof(struct icmp6_hdr)))
    {
        PACKET_LOG_DATA_INSUFFICIENCY(LAYER_TYPE_ICMP6);
        return data;
    }

    struct packet_layer *layer = get_free_layer(pkt);
    if (unlikely(layer == NULL))
    {
        return data;
    }
    SET_LAYER(pkt, layer, LAYER_TYPE_ICMP6, sizeof(struct icmp6_hdr), data, len, 0);

    return layer->pld_ptr;
}

static inline const char *parse_vxlan(struct packet *pkt, const char *data, uint16_t len)
{
    struct vxlan_hdr
    {
        uint8_t flags[2];
        uint16_t gdp;
        uint8_t vni[3];
        uint8_t reserved;
    } __attribute__((__packed__));

    if (unlikely(len < sizeof(struct vxlan_hdr)))
    {
        PACKET_LOG_DATA_INSUFFICIENCY(LAYER_TYPE_VXLAN);
        return data;
    }

    struct packet_layer *layer = get_free_layer(pkt);
    if (unlikely(layer == NULL))
    {
        return data;
    }
    SET_LAYER(pkt, layer, LAYER_TYPE_VXLAN, sizeof(struct vxlan_hdr), data, len, 0);

    // TESTED
    return parse_ether(pkt, layer->pld_ptr, layer->pld_len);
}

static inline const char *parse_gtpv1_u(struct packet *pkt, const char *data, uint16_t len)
{
    uint16_t hdr_len = get_gtp_hdr_len(data, len);
    if (unlikely(hdr_len == 0 || hdr_len > len))
    {
        PACKET_LOG_DATA_INSUFFICIENCY(LAYER_TYPE_GTPV1_U);
        return data;
    }

    struct packet_layer *layer = get_free_layer(pkt);
    if (unlikely(layer == NULL))
    {
        return data;
    }
    uint8_t next_proto = (((const uint8_t *)(data + hdr_len))[0]) >> 4;
    SET_LAYER(pkt, layer, LAYER_TYPE_GTPV1_U, hdr_len, data, len, 0);

    switch (next_proto)
    {
        // TESTED
    case 4:
        return parse_ipv4(pkt, layer->pld_ptr, layer->pld_len);
        // TESTED
    case 6:
        return parse_ipv6(pkt, layer->pld_ptr, layer->pld_len);
    default:
        PACKET_LOG_UNSUPPORT_PROTO("gtp", next_proto);
        return layer->pld_ptr;
    }
}

static inline const char *parse_l3(struct packet *pkt, uint16_t next_proto, const char *data, uint16_t len)
{
    switch (next_proto)
    {
        // TESTED
    case ETH_P_8021Q:
    case ETH_P_8021AD:
        return parse_vlan(pkt, data, len);
        // TESTED
    case ETH_P_IP:
        return parse_ipv4(pkt, data, len);
        // TESTED
    case ETH_P_IPV6:
        return parse_ipv6(pkt, data, len);
        // TESTED
    case ETH_P_PPP_SES:
        return parse_pppoe_ses(pkt, data, len);
        // TESTED
    case ETH_P_MPLS_UC:
        return parse_mpls(pkt, data, len);
    default:
        // TESTED ARP
        PACKET_LOG_UNSUPPORT_ETHPROTO("l3", next_proto);
        return data;
    }
}

static inline const char *parse_l4(struct packet *pkt, uint8_t next_proto, const char *data, uint16_t len)
{
    switch (next_proto)
    {
        // TESTED
    case IPPROTO_TCP:
        return parse_tcp(pkt, data, len);
        // TESTED
    case IPPROTO_UDP:
        return parse_udp(pkt, data, len);
        // TESTED
    case IPPROTO_IPIP:
        return parse_ipv4(pkt, data, len);
        // TESTED
    case IPPROTO_IPV6:
        return parse_ipv6(pkt, data, len);
        // TESTED
    case IPPROTO_GRE:
        return parse_gre(pkt, data, len);
        // TESTED
    case IPPROTO_ICMP:
        return parse_icmp(pkt, data, len);
        // TESTED
    case IPPROTO_ICMPV6:
        return parse_icmp6(pkt, data, len);
    case 115:
        // L2TP
        return parse_l2tpv3_over_ip(pkt, data, len);
    default:
        PACKET_LOG_UNSUPPORT_IPPROTO("l4", next_proto);
        return data;
    }
}

/******************************************************************************
 * Public API
 ******************************************************************************/

// return innermost payload
const char *packet_parse(struct packet *pkt, const char *data, uint16_t len)
{
    pkt->frag_layer = NULL;
    pkt->layers_used = 0;
    pkt->layers_size = PACKET_MAX_LAYERS;
    pkt->data_ptr = data;
    pkt->data_len = len;
    pkt->trim_len = 0;

    // TESTED
    return parse_ether(pkt, data, len);
}

void packet_print_str(const struct packet *pkt)
{
    if (pkt == NULL)
    {
        return;
    }

    char buffer[2048] = {0};
    printf("packet: %p, data_ptr: %p, data_len: %u, trim_len: %u, layers_used: %u, layers_size: %u\n",
           pkt, pkt->data_ptr, pkt->data_len, pkt->trim_len,
           pkt->layers_used, pkt->layers_size);
    for (uint8_t i = 0; i < pkt->layers_used; i++)
    {
        int used = 0;
        const struct packet_layer *layer = &pkt->layers[i];
        printf("  layer[%u]: %p, type: %s, hdr_offset: %u, hdr_ptr: %p, hdr_len: %u, pld_ptr: %p, pld_len: %u\n",
               i, layer, layer_type_to_str(layer->type), layer->hdr_offset,
               layer->hdr_ptr, layer->hdr_len, layer->pld_ptr, layer->pld_len);
        switch (layer->type)
        {
        case LAYER_TYPE_ETHER:
            break;
        case LAYER_TYPE_PPP:
            break;
        case LAYER_TYPE_HDLC:
            break;
        case LAYER_TYPE_L2TP:
            break;
        case LAYER_TYPE_VLAN:
            break;
        case LAYER_TYPE_PPPOE:
            break;
        case LAYER_TYPE_MPLS:
            break;
        case LAYER_TYPE_IPV4:
            used = ipv4_hdr_to_str((const struct ip *)layer->hdr_ptr, buffer, sizeof(buffer));
            break;
        case LAYER_TYPE_IPV6:
            used = ipv6_hdr_to_str((const struct ip6_hdr *)layer->hdr_ptr, buffer, sizeof(buffer));
            break;
        case LAYER_TYPE_GRE:
            break;
        case LAYER_TYPE_UDP:
            used = udp_hdr_to_str((const struct udphdr *)layer->hdr_ptr, buffer, sizeof(buffer));
            break;
        case LAYER_TYPE_TCP:
            used = tcp_hdr_to_str((const struct tcphdr *)layer->hdr_ptr, buffer, sizeof(buffer));
            break;
        case LAYER_TYPE_ICMP:
            break;
        case LAYER_TYPE_ICMP6:
            break;
        case LAYER_TYPE_VXLAN:
            break;
        case LAYER_TYPE_GTPV1_U:
            break;
        default:
            break;
        }
        if (used)
        {
            printf("    %s\n", buffer);
        }
    }
}

void packet_print_table(const struct packet *pkt)
{
    if (pkt == NULL)
    {
        return;
    }

    printf("packet: %p, data_ptr: %p, data_len: %u, trim_len: %u, layers_used: %u, layers_size: %u\n",
           pkt, pkt->data_ptr, pkt->data_len, pkt->trim_len,
           pkt->layers_used, pkt->layers_size);
    printf("+------------+------------+------------+------------+------------+------------+------------+\n");
    printf("| %-10s | %-10s | %-10s | %-10s | %-10s | %-10s | %-10s |\n",
           "index", "type", "hdr ptr", "hdr offset", "hdr len", "pld ptr", "pld len");
    printf("+------------+------------+------------+------------+------------+------------+------------+\n");

    for (uint8_t i = 0; i < pkt->layers_used; i++)
    {
        const struct packet_layer *layer = &pkt->layers[i];
        printf("| %-10u | %-10s | %-10p | %-10u | %-10u | %-10p | %-10u |\n",
               i, layer_type_to_str(layer->type),
               layer->hdr_ptr, layer->hdr_offset, layer->hdr_len,
               layer->pld_ptr, layer->pld_len);
        printf("+------------+------------+------------+------------+------------+------------+------------+\n");
    }
    printf("data: ");
    for (uint16_t i = 0; i < pkt->data_len; i++)
    {
        printf("0x%02x, ", (uint8_t)pkt->data_ptr[i]);
    }
    printf("\n");
}

// return  0 : found
// return -1 : not found
int packet_get_innermost_tuple2(const struct packet *pkt, struct tuple2 *tuple)
{
    memset(tuple, 0, sizeof(struct tuple2));
    const struct packet_layer *layer = NULL;

    for (int8_t i = pkt->layers_used - 1; i >= 0; i--)
    {
        layer = &pkt->layers[i];

        if (layer->type & LAYER_TYPE_L3)
        {
            set_tuple2((const char *)pkt->data_ptr + layer->hdr_offset, layer->type, tuple);
            return 0;
        }
    }

    return -1;
}

// return  0 : found
// return -1 : not found
int packet_get_outermost_tuple2(const struct packet *pkt, struct tuple2 *tuple)
{
    memset(tuple, 0, sizeof(struct tuple2));
    const struct packet_layer *layer = NULL;

    for (int8_t i = 0; i < pkt->layers_used; i++)
    {
        layer = &pkt->layers[i];

        if (layer->type & LAYER_TYPE_L3)
        {
            set_tuple2((const char *)pkt->data_ptr + layer->hdr_offset, layer->type, tuple);
            return 0;
        }
    }

    return -1;
}

// return  0 : found
// return -1 : not found
int packet_get_innermost_tuple4(const struct packet *pkt, struct tuple4 *tuple)
{
    memset(tuple, 0, sizeof(struct tuple4));
    const struct packet_layer *layer_l3 = NULL;
    const struct packet_layer *layer_l4 = NULL;
    const struct packet_layer *layer = NULL;

    for (int8_t i = pkt->layers_used - 1; i >= 0; i--)
    {
        layer = &pkt->layers[i];

        // first get L4 layer
        if (layer->type & (LAYER_TYPE_UDP | LAYER_TYPE_TCP))
        {
            layer_l4 = layer;
            continue;
        }

        // second get L3 layer
        if (layer->type & LAYER_TYPE_L3)
        {
            layer_l3 = layer;
            break;
        }
    }

    if (layer_l3 && layer_l4)
    {
        set_tuple4((const char *)pkt->data_ptr + layer_l3->hdr_offset, layer_l3->type, tuple);
        set_tuple4((const char *)pkt->data_ptr + layer_l4->hdr_offset, layer_l4->type, tuple);
        return 0;
    }
    else
    {
        return -1;
    }
}

// return  0 : found
// return -1 : not found
int packet_get_outermost_tuple4(const struct packet *pkt, struct tuple4 *tuple)
{
    memset(tuple, 0, sizeof(struct tuple4));
    const struct packet_layer *layer_l3 = NULL;
    const struct packet_layer *layer_l4 = NULL;
    const struct packet_layer *layer = NULL;

    for (int8_t i = 0; i < pkt->layers_used; i++)
    {
        layer = &pkt->layers[i];

        // first get L3 layer
        if (layer->type & LAYER_TYPE_L3)
        {
            layer_l3 = layer;
            continue;
        }

        // second get L4 layer
        if (layer->type & (LAYER_TYPE_UDP | LAYER_TYPE_TCP))
        {
            layer_l4 = layer;
            break;
        }
    }

    if (layer_l3 && layer_l4)
    {
        set_tuple4((const char *)pkt->data_ptr + layer_l3->hdr_offset, layer_l3->type, tuple);
        set_tuple4((const char *)pkt->data_ptr + layer_l4->hdr_offset, layer_l4->type, tuple);
        return 0;
    }
    else
    {
        return -1;
    }
}

// return  0 : found
// return -1 : not found
int packet_get_innermost_tuple6(const struct packet *pkt, struct tuple6 *tuple)
{
    memset(tuple, 0, sizeof(struct tuple6));
    const struct packet_layer *layer_l3 = NULL;
    const struct packet_layer *layer_l4 = NULL;
    const struct packet_layer *layer = NULL;
    uint64_t domain = packet_get_domain(pkt);

    for (int8_t i = pkt->layers_used - 1; i >= 0; i--)
    {
        layer = &pkt->layers[i];

        // first get L4 layer
        if (layer->type & (LAYER_TYPE_UDP | LAYER_TYPE_TCP))
        {
            layer_l4 = layer;
            continue;
        }

        // second get L3 layer
        if (layer->type & LAYER_TYPE_L3)
        {
            layer_l3 = layer;
            break;
        }
    }

    if (layer_l3 && layer_l4 && layer_l4 - layer_l3 == 1)
    {
        set_tuple6((const char *)pkt->data_ptr + layer_l3->hdr_offset, layer_l3->type, tuple, domain);
        set_tuple6((const char *)pkt->data_ptr + layer_l4->hdr_offset, layer_l4->type, tuple, domain);
        return 0;
    }
    else
    {
        return -1;
    }
}

// return  0 : found
// return -1 : not found
int packet_get_outermost_tuple6(const struct packet *pkt, struct tuple6 *tuple)
{
    memset(tuple, 0, sizeof(struct tuple6));
    const struct packet_layer *layer_l3 = NULL;
    const struct packet_layer *layer_l4 = NULL;
    const struct packet_layer *layer = NULL;
    uint64_t domain = packet_get_domain(pkt);

    for (int8_t i = 0; i < pkt->layers_used; i++)
    {
        layer = &pkt->layers[i];

        // first get L3 layer
        if (layer->type & LAYER_TYPE_L3)
        {
            layer_l3 = layer;
            continue;
        }

        // second get L4 layer
        if (layer->type & (LAYER_TYPE_UDP | LAYER_TYPE_TCP))
        {
            layer_l4 = layer;
            break;
        }
    }

    if (layer_l3 && layer_l4 && layer_l4 - layer_l3 == 1)
    {
        set_tuple6((const char *)pkt->data_ptr + layer_l3->hdr_offset, layer_l3->type, tuple, domain);
        set_tuple6((const char *)pkt->data_ptr + layer_l4->hdr_offset, layer_l4->type, tuple, domain);
        return 0;
    }
    else
    {
        return -1;
    }
}

const struct packet_layer *packet_get_innermost_layer(const struct packet *pkt, enum layer_type type)
{
    const struct packet_layer *layer = NULL;

    for (int8_t i = pkt->layers_used - 1; i >= 0; i--)
    {
        layer = &pkt->layers[i];
        if (layer->type & type)
        {
            return layer;
        }
    }

    return NULL;
}

const struct packet_layer *packet_get_outermost_layer(const struct packet *pkt, enum layer_type type)
{
    const struct packet_layer *layer = NULL;

    for (int8_t i = 0; i < pkt->layers_used; i++)
    {
        layer = &pkt->layers[i];
        if (layer->type & type)
        {
            return layer;
        }
    }

    return NULL;
}

// direction 1: E2I
// direction 0: I2E
uint64_t packet_get_hash(const struct packet *pkt, enum ldbc_method method, int direction)
{
    uint64_t temp = 0;
    uint64_t hash_value = 1;

    int inner_addr_len = 0;
    int outer_addr_len = 0;
    const char *inner_src_addr = NULL;
    const char *inner_dst_addr = NULL;
    const char *outer_src_addr = NULL;
    const char *outer_dst_addr = NULL;

    struct tuple2 inner_addr;
    struct tuple2 outer_addr;

    if (pkt == NULL)
    {
        return hash_value;
    }

    if (packet_get_innermost_tuple2(pkt, &inner_addr) == -1)
    {
        return hash_value;
    }

    if (packet_get_outermost_tuple2(pkt, &outer_addr) == -1)
    {
        return hash_value;
    }

    if (inner_addr.ip_type == IP_TYPE_V4)
    {
        inner_src_addr = (const char *)&inner_addr.src_addr.v4;
        inner_dst_addr = (const char *)&inner_addr.dst_addr.v4;
        inner_addr_len = sizeof(struct in_addr);
    }
    else
    {
        inner_src_addr = (const char *)&inner_addr.src_addr.v6;
        inner_dst_addr = (const char *)&inner_addr.dst_addr.v6;
        inner_addr_len = sizeof(struct in6_addr);
    }

    if (outer_addr.ip_type == IP_TYPE_V4)
    {
        outer_src_addr = (const char *)&outer_addr.src_addr.v4;
        outer_dst_addr = (const char *)&outer_addr.dst_addr.v4;
        outer_addr_len = sizeof(struct in_addr);
    }
    else
    {
        outer_src_addr = (const char *)&outer_addr.src_addr.v6;
        outer_dst_addr = (const char *)&outer_addr.dst_addr.v6;
        outer_addr_len = sizeof(struct in6_addr);
    }

    switch (method)
    {
    case LDBC_METHOD_HASH_INT_IP:
        if (direction)
        {
            // direction 1: E2I
            HASH_VALUE(outer_dst_addr, outer_addr_len, hash_value);
        }
        else
        {
            // direction 0: I2E
            HASH_VALUE(outer_src_addr, outer_addr_len, hash_value);
        }
        break;
    case LDBC_METHOD_HASH_EXT_IP:
        if (direction)
        {
            // direction 1: E2I
            HASH_VALUE(outer_src_addr, outer_addr_len, hash_value);
        }
        else
        {
            // direction 0: I2E
            HASH_VALUE(outer_dst_addr, outer_addr_len, hash_value);
        }
        break;
    case LDBC_METHOD_HASH_INT_IP_AND_EXT_IP:
        HASH_VALUE(outer_src_addr, outer_addr_len, hash_value);
        HASH_VALUE(outer_dst_addr, outer_addr_len, temp);
        hash_value = hash_value ^ temp;
        break;
    case LDBC_METHOD_HASH_INNERMOST_INT_IP:
        if (direction)
        {
            // direction 1: E2I
            HASH_VALUE(inner_dst_addr, inner_addr_len, hash_value);
        }
        else
        {
            // direction 0: I2E
            HASH_VALUE(inner_src_addr, inner_addr_len, hash_value);
        }
        break;
    case LDBC_METHOD_HASH_INNERMOST_EXT_IP:
        if (direction)
        {
            // direction 1: E2I
            HASH_VALUE(inner_src_addr, inner_addr_len, hash_value);
        }
        else
        {
            // direction 0: I2E
            HASH_VALUE(inner_dst_addr, inner_addr_len, hash_value);
        }
        break;
    default:
        return hash_value;
    }

    return hash_value;
}