stellar-stellar/src/packet/packet_parser.cpp

#include <string.h>
#include <stdlib.h>
#include <netinet/icmp6.h>
#include <netinet/ip_icmp.h>
#include <linux/ppp_defs.h>

#include "log.h"
#include "packet_helper.h"
#include "packet_private.h"
#include "packet_parser.h"

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

#define PACKET_PARSE_LOG_DEBUG(format, ...) void(0) // LOG_DEBUG("packet parse", format, ##__VA_ARGS__)
#define PACKET_PARSE_LOG_WARN(format, ...) LOG_WARN("packet parse", format, ##__VA_ARGS__)
#define PACKET_PARSE_LOG_ERROR(format, ...) LOG_ERROR("packet parse", format, ##__VA_ARGS__)

#define PACKET_LOG_DATA_INSUFFICIENCY(pkt, layer)                       \
    {                                                                   \
        PACKET_PARSE_LOG_WARN("pkt: %p, layer: %s, data insufficiency", \
                              (pkt), layer_proto_to_str(layer));        \
    }

#define PACKET_LOG_UNSUPPORT_PROTO(pkt, layer, next_proto)                     \
    {                                                                          \
        PACKET_PARSE_LOG_WARN("pkt: %p, layer: %s, unsupport next proto: %d",  \
                              (pkt), layer_proto_to_str(layer), (next_proto)); \
    }

#define PACKET_LOG_UNSUPPORT_ETHPROTO(pkt, next_proto)                            \
    {                                                                             \
        PACKET_PARSE_LOG_WARN("pkt: %p, layer: L3, unsupport next proto: %d %s",  \
                              (pkt), (next_proto), eth_proto_to_str(next_proto)); \
    }

#define PACKET_LOG_UNSUPPORT_IPPROTO(pkt, next_proto)                            \
    {                                                                            \
        PACKET_PARSE_LOG_WARN("pkt: %p, layer: L4, unsupport next proto: %d %s", \
                              (pkt), (next_proto), ip_proto_to_str(next_proto)); \
    }

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

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

// 数据链路层
static inline const char *parse_ether(struct packet *pkt, const char *data, uint16_t len);
static inline const char *parse_pweth(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_over_udp(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_auth(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_gtp_u(struct packet *pkt, const char *data, uint16_t len);
static inline const char *parse_gtp_c(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 inline struct raw_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, _proto, _hdr_len, _data, _len, _trim)                                                             \
    {                                                                                                                             \
        (_layer)->proto = (_proto);                                                                                               \
        (_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_PARSE_LOG_DEBUG("layer[%d/%d]: %s, header: {offset: %d, ptr: %p, len: %d}, payload: {ptr: %p, len: %d}",           \
                               (_pkt)->layers_used - 1, (_pkt)->layers_size, layer_proto_to_str((_proto)),                        \
                               (_layer)->hdr_offset, (_layer)->hdr_ptr, (_layer)->hdr_len, (_layer)->pld_ptr, (_layer)->pld_len); \
    }

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

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(pkt, LAYER_PROTO_ETHER);
        return data;
    }

    struct raw_layer *layer = get_free_layer(pkt);
    if (unlikely(layer == NULL))
    {
        return data;
    }
    uint16_t next_proto = eth_hdr_get_proto((const struct ethhdr *)data);
    SET_LAYER(pkt, layer, LAYER_PROTO_ETHER, sizeof(struct ethhdr), data, len, 0);

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

static inline const char *parse_pweth(struct packet *pkt, const char *data, uint16_t len)
{
    /*
     * 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
     */
    if (unlikely(len < 4))
    {
        PACKET_LOG_DATA_INSUFFICIENCY(pkt, LAYER_PROTO_PWETH);
        return data;
    }

    struct raw_layer *layer = get_free_layer(pkt);
    if (unlikely(layer == NULL))
    {
        return data;
    }
    SET_LAYER(pkt, layer, LAYER_PROTO_PWETH, 4, data, len, 0);

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

static inline int is_ppp_proto(uint16_t proto)
{
    // /usr/include/linux/ppp_defs.h.html
    switch (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(pkt, LAYER_PROTO_PPP);
        return data;
    }

    uint16_t hdr_len = 0;
    uint16_t next_proto = 0;

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

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

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

success:
    struct raw_layer *layer = get_free_layer(pkt);
    if (unlikely(layer == NULL))
    {
        return data;
    }
    SET_LAYER(pkt, layer, LAYER_PROTO_PPP, hdr_len, data, len, 0);
    switch (next_proto)
    {
    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(pkt, LAYER_PROTO_PPP, next_proto);
        return layer->pld_ptr;
    }
}

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

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

    // control message
    if (l2tp_hdr_get_type((const struct l2tp_hdr *)data))
    {
        return layer->pld_ptr;
    }
    // data message
    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)
{
    uint16_t hdr_len = calc_udp_l2tpv3_hdr_len(data, len);
    if (unlikely(hdr_len == 0 || hdr_len > len))
    {
        PACKET_LOG_DATA_INSUFFICIENCY(pkt, LAYER_PROTO_L2TP);
        return data;
    }

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

    // control message
    if (l2tp_hdr_get_type((const struct l2tp_hdr *)data))
    {
        return layer->pld_ptr;
    }
    // data message
    else
    {
        // TOOD
        return layer->pld_ptr;
    }
}

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

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

    // data message
    if (ntohl(*((uint32_t *)data)))
    {
        // TOOD
        return layer->pld_ptr;
    }
    // control message
    else
    {
        return layer->pld_ptr;
    }
}

static inline const char *parse_vlan(struct packet *pkt, const char *data, uint16_t len)
{
    if (unlikely(len < sizeof(struct vlan_hdr)))
    {
        PACKET_LOG_DATA_INSUFFICIENCY(pkt, LAYER_PROTO_VLAN);
        return data;
    }

    struct raw_layer *layer = get_free_layer(pkt);
    if (unlikely(layer == NULL))
    {
        return data;
    }
    uint16_t next_proto = vlan_hdr_get_ethertype((const struct vlan_hdr *)data);
    SET_LAYER(pkt, layer, LAYER_PROTO_VLAN, sizeof(struct vlan_hdr), data, len, 0);

    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(pkt, LAYER_PROTO_PPPOE);
        return data;
    }

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

    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)
{
    if (unlikely(len < 4))
    {
        PACKET_LOG_DATA_INSUFFICIENCY(pkt, LAYER_PROTO_MPLS);
        return data;
    }

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

    if (mpls_label_get_bos((const struct mpls_label *)data))
    {
        SET_LAYER(pkt, layer, LAYER_PROTO_MPLS, 4, data, len, 0);
        if (layer->pld_len == 0)
        {
            return layer->pld_ptr;
        }
        uint8_t next_proto = layer->pld_ptr[0] >> 4;
        switch (next_proto)
        {
        case 0:
            // the first four digits of the PW Ethernet control word must be "00000", but the first four digits of Ethernet may also be "0000"
            if (layer->pld_len >= sizeof(struct ethhdr) && is_eth_proto(eth_hdr_get_proto((const struct ethhdr *)layer->pld_ptr)))
            {
                return parse_ether(pkt, layer->pld_ptr, layer->pld_len);
            }
            else
            {
                return parse_pweth(pkt, layer->pld_ptr, layer->pld_len);
            }
        case 4:
            return parse_ipv4(pkt, layer->pld_ptr, layer->pld_len);
        case 6:
            return parse_ipv6(pkt, layer->pld_ptr, layer->pld_len);
        default:
            return parse_ether(pkt, layer->pld_ptr, layer->pld_len);
        }
    }
    else
    {
        SET_LAYER(pkt, layer, LAYER_PROTO_MPLS, 4, data, len, 0);
        return parse_mpls(pkt, layer->pld_ptr, layer->pld_len);
    }
}

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(pkt, LAYER_PROTO_IPV4);
        return data;
    }

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

    const struct ip *hdr = (const struct ip *)data;
    uint8_t version = ip4_hdr_get_version(hdr);
    if (unlikely(version != 4))
    {
        PACKET_PARSE_LOG_ERROR("packet %p ipv4 version %d != 4", pkt, version);
        return data;
    }

    uint16_t hdr_len = ip4_hdr_get_hdr_len(hdr);
    if (unlikely(hdr_len > len))
    {
        PACKET_LOG_DATA_INSUFFICIENCY(pkt, LAYER_PROTO_IPV4);
        return data;
    }

    uint16_t total_len = ip4_hdr_get_total_len(hdr);
    if (unlikely(total_len > len))
    {
        PACKET_LOG_DATA_INSUFFICIENCY(pkt, LAYER_PROTO_IPV4);
        return data;
    }
    if (unlikely(total_len < hdr_len))
    {
        PACKET_PARSE_LOG_ERROR("packet %p ip total_len %d < hdr_len %d", pkt, total_len, hdr_len);
        return data;
    }
    uint16_t trim_len = len - total_len;
    SET_LAYER(pkt, layer, LAYER_PROTO_IPV4, hdr_len, data, len, trim_len);

    // ip fragmented
    if (ip4_hdr_get_mf_flag(hdr) || ip4_hdr_get_frag_offset(hdr))
    {
        PACKET_PARSE_LOG_WARN("packet %p ip layer %p is fragmented", pkt, layer);
        pkt->frag_layer = layer;
        return layer->pld_ptr;
    }

    uint8_t next_proto = ip4_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(pkt, LAYER_PROTO_IPV6);
        return data;
    }

    struct raw_layer *layer = get_free_layer(pkt);
    if (unlikely(layer == NULL))
    {
        return data;
    }
    const struct ip6_hdr *hdr = (const struct ip6_hdr *)data;
    uint8_t version = ip6_hdr_get_version(hdr);
    if (unlikely(version != 6))
    {
        PACKET_PARSE_LOG_ERROR("packet %p ipv6 version %d != 6", pkt, version);
        return data;
    }

    uint16_t pld_len = ip6_hdr_get_payload_len(hdr);
    if (unlikely(pld_len + sizeof(struct ip6_hdr) > len))
    {
        PACKET_LOG_DATA_INSUFFICIENCY(pkt, LAYER_PROTO_IPV6);
        return data;
    }
    uint8_t next_proto = ip6_hdr_get_next_header(hdr);
    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(pkt, LAYER_PROTO_IPV6);
            return data;
        }
        struct ip6_ext *ext = (struct ip6_ext *)next_hdr_ptr;
        uint16_t skip_len = 0;
        if (next_proto == IPPROTO_AH)
        {
            /*
             * https://datatracker.ietf.org/doc/html/rfc4302#section-2
             * For IPv6, the total length of the header must be a multiple of 8-octet units.
             * (Note that although IPv6 [DH98] characterizes AH as an extension header,
             * its length is measured in 32-bit words, not the 64-bit words used by other IPv6 extension headers.)
             */
            skip_len = ext->ip6e_len * 4 + 8;
        }
        else
        {
            skip_len = ext->ip6e_len * 8 + 8;
        }
        if (unlikely(skip_len > pld_len))
        {
            PACKET_LOG_DATA_INSUFFICIENCY(pkt, LAYER_PROTO_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_PROTO_IPV6, hdr_len, data, len, trim_len);

    // ipv6 fragment
    if (next_proto == IPPROTO_FRAGMENT)
    {
        PACKET_PARSE_LOG_WARN("packet %p ipv6 layer %p is fragmented", pkt, layer);
        pkt->frag_layer = layer;
        return layer->pld_ptr;
    }

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

static inline const char *parse_auth(struct packet *pkt, const char *data, uint16_t len)
{
    /*
     * https://datatracker.ietf.org/doc/html/rfc4302#section-2
     * For IPv4: AH not IPv4 option, as an single layer
     * For IPv6: AH as IPv6 extension header
     *
     * AH 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
     * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
     * | Next Header   |  Payload Len  |          RESERVED             |
     * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
     * |                 Security Parameters Index (SPI)               |
     * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
     * |                    Sequence Number Field                      |
     * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
     * |                                                               |
     * +                Integrity Check Value-ICV (variable)           |
     * |                                                               |
     * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
     */

    if (unlikely(len < 12))
    {
        PACKET_LOG_DATA_INSUFFICIENCY(pkt, LAYER_PROTO_IPAH);
        return data;
    }
    /*
     * https://datatracker.ietf.org/doc/html/rfc4302#section-2
     * For IPv4, This 8-bit field specifies the length of AH in 32-bit words (4-byte units), minus "2".
     * Thus, for example, if an integrity algorithm yields a 96-bit authentication value,
     * this length field will be "4" (3 32-bit word fixed fields plus 3 32-bit words for the ICV, minus 2).
     */
    uint8_t next_proto = ((const uint8_t *)data)[0];
    uint16_t hdr_len = ((const uint8_t *)data)[1] * 4 + 8;
    if (unlikely(len < hdr_len))
    {
        PACKET_LOG_DATA_INSUFFICIENCY(pkt, LAYER_PROTO_IPAH);
        return data;
    }

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

    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)
{
    uint16_t hdr_len = calc_gre_hdr_len(data, len);
    if (unlikely(hdr_len == 0 || hdr_len > len))
    {
        PACKET_LOG_DATA_INSUFFICIENCY(pkt, LAYER_PROTO_GRE);
        return data;
    }

    struct raw_layer *layer = get_free_layer(pkt);
    if (unlikely(layer == NULL))
    {
        return data;
    }
    uint16_t next_proto = peek_gre_proto(data, len);
    SET_LAYER(pkt, layer, LAYER_PROTO_GRE, hdr_len, data, len, 0);

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

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(pkt, LAYER_PROTO_UDP);
        return data;
    }

    struct raw_layer *layer = get_free_layer(pkt);
    if (unlikely(layer == NULL))
    {
        return data;
    }
    const struct udphdr *udp_hdr = (struct udphdr *)data;
    SET_LAYER(pkt, layer, LAYER_PROTO_UDP, sizeof(struct udphdr), data, len, 0);

    uint16_t src_port = udp_hdr_get_src_port(udp_hdr);
    uint16_t dst_port = udp_hdr_get_dst_port(udp_hdr);

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

    if (dst_port == 2152 || src_port == 2152)
    {
        // only GTPv1-U, no GTPv2-U
        return parse_gtp_u(pkt, layer->pld_ptr, layer->pld_len);
    }

    if (dst_port == 2123 || src_port == 2123)
    {
        // GTPv1-C or GTPv2-C
        return parse_gtp_c(pkt, layer->pld_ptr, layer->pld_len);
    }

    if (dst_port == 1701 || src_port == 1701)
    {
        // L2TP_DPORT 1701
        if (unlikely(layer->pld_len < 8))
        {
            return layer->pld_ptr;
        }

        switch (l2tp_hdr_get_ver((const struct l2tp_hdr *)layer->pld_ptr))
        {
        case 2:
            return parse_l2tpv2_over_udp(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;
        }
    }

    if (dst_port == 3544 || src_port == 3544)
    {
        // Teredo IPv6 tunneling 3544
        if (unlikely(layer->pld_len < sizeof(struct ip6_hdr)))
        {
            return layer->pld_ptr;
        }
        const struct ip6_hdr *ipv6_hdr = (const struct ip6_hdr *)layer->pld_ptr;
        if (ip6_hdr_get_version(ipv6_hdr) != 6)
        {
            return layer->pld_ptr;
        }
        return parse_ipv6(pkt, layer->pld_ptr, layer->pld_len);
    }

    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(pkt, LAYER_PROTO_TCP);
        return data;
    }

    struct raw_layer *layer = get_free_layer(pkt);
    if (unlikely(layer == NULL))
    {
        return data;
    }
    uint16_t hdr_len = tcp_hdr_get_hdr_len((const struct tcphdr *)data);
    if (unlikely(hdr_len > len))
    {
        PACKET_LOG_DATA_INSUFFICIENCY(pkt, LAYER_PROTO_TCP);
        return data;
    }
    SET_LAYER(pkt, layer, LAYER_PROTO_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(pkt, LAYER_PROTO_ICMP);
        return data;
    }

    struct raw_layer *layer = get_free_layer(pkt);
    if (unlikely(layer == NULL))
    {
        return data;
    }
    SET_LAYER(pkt, layer, LAYER_PROTO_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(pkt, LAYER_PROTO_ICMP6);
        return data;
    }

    struct raw_layer *layer = get_free_layer(pkt);
    if (unlikely(layer == NULL))
    {
        return data;
    }
    SET_LAYER(pkt, layer, LAYER_PROTO_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)
{
    if (unlikely(len < sizeof(struct vxlan_hdr)))
    {
        PACKET_LOG_DATA_INSUFFICIENCY(pkt, LAYER_PROTO_VXLAN);
        return data;
    }

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

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

static inline const char *parse_gtp_u(struct packet *pkt, const char *data, uint16_t len)
{
    // only GTPv1-U, no GTPv2-U
    uint8_t version = peek_gtp_version(data, len);
    if (unlikely(version != 1))
    {
        return data;
    }

    uint16_t hdr_len = calc_gtp_hdr_len(data, len);
    if (unlikely(hdr_len == 0 || hdr_len > len))
    {
        PACKET_LOG_DATA_INSUFFICIENCY(pkt, LAYER_PROTO_GTP_U);
        return data;
    }

    uint8_t next_proto = (((const uint8_t *)(data + hdr_len))[0]) >> 4;
    if (next_proto != 4 && next_proto != 6)
    {
        // next_proto is not IPv4 or IPv6, this is not a normal GTP-U packet, fallback to UDP
        return data;
    }

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

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

static inline const char *parse_gtp_c(struct packet *pkt, const char *data, uint16_t len)
{
    // GTPv1-C or GTPv2-C
    uint16_t hdr_len = calc_gtp_hdr_len(data, len);
    if (unlikely(hdr_len == 0 || hdr_len > len))
    {
        PACKET_LOG_DATA_INSUFFICIENCY(pkt, LAYER_PROTO_GTP_C);
        return data;
    }

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

    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)
    {
    case ETH_P_8021Q:
    case ETH_P_8021AD:
        return parse_vlan(pkt, data, len);
    case ETH_P_IP:
        return parse_ipv4(pkt, data, len);
    case ETH_P_IPV6:
        return parse_ipv6(pkt, data, len);
    case ETH_P_PPP_SES:
        return parse_pppoe_ses(pkt, data, len);
    case ETH_P_MPLS_UC:
        return parse_mpls(pkt, data, len);
    case 0x880b:
        return parse_ppp(pkt, data, len);
    default:
        PACKET_LOG_UNSUPPORT_ETHPROTO(pkt, 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)
    {
    case IPPROTO_AH:
        return parse_auth(pkt, data, len);
    case IPPROTO_TCP:
        return parse_tcp(pkt, data, len);
    case IPPROTO_UDP:
        return parse_udp(pkt, data, len);
    case IPPROTO_IPIP:
        return parse_ipv4(pkt, data, len);
    case IPPROTO_IPV6:
        return parse_ipv6(pkt, data, len);
    case IPPROTO_GRE:
        return parse_gre(pkt, data, len);
    case IPPROTO_ICMP:
        return parse_icmp(pkt, data, len);
    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(pkt, 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;

    return parse_ether(pkt, data, len);
}

const char *layer_proto_to_str(enum layer_proto proto)
{
    switch (proto)
    {
    case LAYER_PROTO_ETHER:
        return "ETH";
    case LAYER_PROTO_PWETH:
        return "PWETH";
    case LAYER_PROTO_PPP:
        return "PPP";
    case LAYER_PROTO_L2TP:
        return "L2TP";
    case LAYER_PROTO_VLAN:
        return "VLAN";
    case LAYER_PROTO_PPPOE:
        return "PPPOE";
    case LAYER_PROTO_MPLS:
        return "MPLS";
    case LAYER_PROTO_IPV4:
        return "IPV4";
    case LAYER_PROTO_IPV6:
        return "IPV6";
    case LAYER_PROTO_IPAH:
        return "IPAH";
    case LAYER_PROTO_GRE:
        return "GRE";
    case LAYER_PROTO_UDP:
        return "UDP";
    case LAYER_PROTO_TCP:
        return "TCP";
    case LAYER_PROTO_ICMP:
        return "ICMP";
    case LAYER_PROTO_ICMP6:
        return "ICMP6";
    case LAYER_PROTO_VXLAN:
        return "VXLAN";
    case LAYER_PROTO_GTP_C:
        return "GTP-C";
    case LAYER_PROTO_GTP_U:
        return "GTP-U";
    default:
        return "UNKNOWN";
    }
}