stellar-stellar/infra/packet_manager/packet_helper.h

#pragma once

#ifdef __cplusplus
extern "C"
{
#endif

#include "utils_internal.h"
#include <stdio.h>
#include <string.h>
#include <stdbool.h>
#include <arpa/inet.h>
#include <linux/if_ether.h>
#include <netinet/ip.h>
#include <netinet/ip6.h>
#include <linux/mpls.h>
#define __FAVOR_BSD 1
#include <netinet/tcp.h>
#include <netinet/udp.h>

/******************************************************************************
 * ETH
 ******************************************************************************/

static inline int eth_hdr_get_dest(const struct ethhdr *hdr, char *buff, int size)
{
    return snprintf(buff, size, "%02x:%02x:%02x:%02x:%02x:%02x",
                    hdr->h_dest[0], hdr->h_dest[1], hdr->h_dest[2],
                    hdr->h_dest[3], hdr->h_dest[4], hdr->h_dest[5]);
}

static inline int eth_hdr_get_source(const struct ethhdr *hdr, char *buff, int size)
{
    return snprintf(buff, size, "%02x:%02x:%02x:%02x:%02x:%02x",
                    hdr->h_source[0], hdr->h_source[1], hdr->h_source[2],
                    hdr->h_source[3], hdr->h_source[4], hdr->h_source[5]);
}

static inline uint16_t eth_hdr_get_proto(const struct ethhdr *hdr)
{
    return ntohs(hdr->h_proto);
}

static inline void eth_hdr_set_dest(struct ethhdr *hdr, const char *dest)
{
    sscanf(dest, "%hhx:%hhx:%hhx:%hhx:%hhx:%hhx",
           &hdr->h_dest[0], &hdr->h_dest[1], &hdr->h_dest[2],
           &hdr->h_dest[3], &hdr->h_dest[4], &hdr->h_dest[5]);
}

static inline void eth_hdr_set_source(struct ethhdr *hdr, const char *source)
{
    sscanf(source, "%hhx:%hhx:%hhx:%hhx:%hhx:%hhx",
           &hdr->h_source[0], &hdr->h_source[1], &hdr->h_source[2],
           &hdr->h_source[3], &hdr->h_source[4], &hdr->h_source[5]);
}

static inline void eth_hdr_set_proto(struct ethhdr *hdr, uint16_t proto)
{
    hdr->h_proto = htons(proto);
}

// /usr/include/linux/if_ether.h
static inline int is_eth_proto(uint16_t proto)
{
    switch (proto)
    {
    case ETH_P_LOOP:
    case ETH_P_PUP:
    case ETH_P_PUPAT:
    case ETH_P_IP:
    case ETH_P_X25:
    case ETH_P_ARP:
    case ETH_P_BPQ:
    case ETH_P_IEEEPUP:
    case ETH_P_IEEEPUPAT:
    case ETH_P_DEC:
    case ETH_P_DNA_DL:
    case ETH_P_DNA_RC:
    case ETH_P_DNA_RT:
    case ETH_P_LAT:
    case ETH_P_DIAG:
    case ETH_P_CUST:
    case ETH_P_SCA:
    case ETH_P_TEB:
    case ETH_P_RARP:
    case ETH_P_ATALK:
    case ETH_P_AARP:
    case ETH_P_8021Q:
    case ETH_P_8021AD:
    case ETH_P_IPX:
    case ETH_P_IPV6:
    case ETH_P_PAUSE:
    case ETH_P_SLOW:
    case ETH_P_WCCP:
    case ETH_P_PPP_DISC:
    case ETH_P_PPP_SES:
    case ETH_P_MPLS_UC:
    case ETH_P_MPLS_MC:
    case ETH_P_ATMMPOA:
    case ETH_P_ATMFATE:
    case ETH_P_PAE:
    case ETH_P_AOE:
    case ETH_P_TIPC:
    case ETH_P_1588:
    case ETH_P_FCOE:
    case ETH_P_FIP:
    case ETH_P_EDSA:
    case ETH_P_802_3:
    case ETH_P_AX25:
    case ETH_P_ALL:
    case ETH_P_802_2:
    case ETH_P_SNAP:
    case ETH_P_DDCMP:
    case ETH_P_WAN_PPP:
    case ETH_P_PPP_MP:
    case ETH_P_LOCALTALK:
    case ETH_P_CAN:
    case ETH_P_PPPTALK:
    case ETH_P_TR_802_2:
    case ETH_P_MOBITEX:
    case ETH_P_CONTROL:
    case ETH_P_IRDA:
    case ETH_P_ECONET:
    case ETH_P_HDLC:
    case ETH_P_ARCNET:
    case ETH_P_DSA:
    case ETH_P_TRAILER:
    case ETH_P_PHONET:
    case ETH_P_IEEE802154:
    case 0x880B: // PPP：Point-to-Point Protoco
        return 1;
    default:
        return 0;
    }
}

static inline const char *eth_proto_to_str(uint16_t proto)
{
    switch (proto)
    {
    case ETH_P_LOOP:
        return "ETH_P_LOOP";
    case ETH_P_PUP:
        return "ETH_P_PUP";
    case ETH_P_PUPAT:
        return "ETH_P_PUPAT";
    case ETH_P_IP:
        return "ETH_P_IP";
    case ETH_P_X25:
        return "ETH_P_X25";
    case ETH_P_ARP:
        return "ETH_P_ARP";
    case ETH_P_BPQ:
        return "ETH_P_BPQ";
    case ETH_P_IEEEPUP:
        return "ETH_P_IEEEPUP";
    case ETH_P_IEEEPUPAT:
        return "ETH_P_IEEEPUPAT";
    case ETH_P_DEC:
        return "ETH_P_DEC";
    case ETH_P_DNA_DL:
        return "ETH_P_DNA_DL";
    case ETH_P_DNA_RC:
        return "ETH_P_DNA_RC";
    case ETH_P_DNA_RT:
        return "ETH_P_DNA_RT";
    case ETH_P_LAT:
        return "ETH_P_LAT";
    case ETH_P_DIAG:
        return "ETH_P_DIAG";
    case ETH_P_CUST:
        return "ETH_P_CUST";
    case ETH_P_SCA:
        return "ETH_P_SCA";
    case ETH_P_TEB:
        return "ETH_P_TEB";
    case ETH_P_RARP:
        return "ETH_P_RARP";
    case ETH_P_ATALK:
        return "ETH_P_ATALK";
    case ETH_P_AARP:
        return "ETH_P_AARP";
    case ETH_P_8021Q:
        return "ETH_P_8021Q";
    case ETH_P_8021AD:
        return "ETH_P_8021AD";
    case ETH_P_IPX:
        return "ETH_P_IPX";
    case ETH_P_IPV6:
        return "ETH_P_IPV6";
    case ETH_P_PAUSE:
        return "ETH_P_PAUSE";
    case ETH_P_SLOW:
        return "ETH_P_SLOW";
    case ETH_P_WCCP:
        return "ETH_P_WCCP";
    case ETH_P_PPP_DISC:
        return "ETH_P_PPP_DISC";
    case ETH_P_PPP_SES:
        return "ETH_P_PPP_SES";
    case ETH_P_MPLS_UC:
        return "ETH_P_MPLS_UC";
    case ETH_P_MPLS_MC:
        return "ETH_P_MPLS_MC";
    case ETH_P_ATMMPOA:
        return "ETH_P_ATMMPOA";
    case ETH_P_ATMFATE:
        return "ETH_P_ATMFATE";
    case ETH_P_PAE:
        return "ETH_P_PAE";
    case ETH_P_AOE:
        return "ETH_P_AOE";
    case ETH_P_TIPC:
        return "ETH_P_TIPC";
    case ETH_P_1588:
        return "ETH_P_1588";
    case ETH_P_FCOE:
        return "ETH_P_FCOE";
    case ETH_P_FIP:
        return "ETH_P_FIP";
    case ETH_P_EDSA:
        return "ETH_P_EDSA";
    case ETH_P_802_3:
        return "ETH_P_802_3";
    case ETH_P_AX25:
        return "ETH_P_AX25";
    case ETH_P_ALL:
        return "ETH_P_ALL";
    case ETH_P_802_2:
        return "ETH_P_802_2";
    case ETH_P_SNAP:
        return "ETH_P_SNAP";
    case ETH_P_DDCMP:
        return "ETH_P_DDCMP";
    case ETH_P_WAN_PPP:
        return "ETH_P_WAN_PPP";
    case ETH_P_PPP_MP:
        return "ETH_P_PPP_MP";
    case ETH_P_LOCALTALK:
        return "ETH_P_LOCALTALK";
    case ETH_P_CAN:
        return "ETH_P_CAN";
    case ETH_P_PPPTALK:
        return "ETH_P_PPPTALK";
    case ETH_P_TR_802_2:
        return "ETH_P_TR_802_2";
    case ETH_P_MOBITEX:
        return "ETH_P_MOBITEX";
    case ETH_P_CONTROL:
        return "ETH_P_CONTROL";
    case ETH_P_IRDA:
        return "ETH_P_IRDA";
    case ETH_P_ECONET:
        return "ETH_P_ECONET";
    case ETH_P_HDLC:
        return "ETH_P_HDLC";
    case ETH_P_ARCNET:
        return "ETH_P_ARCNET";
    case ETH_P_DSA:
        return "ETH_P_DSA";
    case ETH_P_TRAILER:
        return "ETH_P_TRAILER";
    case ETH_P_PHONET:
        return "ETH_P_PHONET";
    case ETH_P_IEEE802154:
        return "ETH_P_IEEE802154";
    case 0x880B:
        return "ETH_P_PPP";
    default:
        return "ETH_P_UNKNOWN";
    }
}

static inline int eth_hdr_to_str(const struct ethhdr *hdr, char *buf, size_t size)
{
    memset(buf, 0, size);
    char dest[18] = {0};
    char source[18] = {0};
    uint16_t proto = eth_hdr_get_proto(hdr);
    eth_hdr_get_dest(hdr, dest, sizeof(dest));
    eth_hdr_get_source(hdr, source, sizeof(source));
    return snprintf(buf, size, "ETH: source=%s dest=%s proto=%s",
                    source, dest, eth_proto_to_str(proto));
}

/******************************************************************************
 * GREv0
 ******************************************************************************/

/*
 * 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
 */

struct gre0_hdr
{
    uint16_t flags;
    uint16_t protocol;
};

struct sre
{
    uint16_t family;
    uint8_t offset;
    uint8_t length;
};

#define GRE0_FLAG_CHECKSUM 0x8000
#define GRE0_FLAG_ROUTING 0x4000
#define GRE0_FLAG_KEY 0x2000
#define GRE0_FLAG_SEQUENCE 0x1000
#define GRE0_FLAG_STRICT 0x0800
#define GRE0_FLAG_RECURSION 0x0700
#define GRE0_FLAG_VERSION 0x0007

static inline uint16_t gre0_hdr_get_flags(const struct gre0_hdr *hdr)
{
    return ntohs(hdr->flags);
}

static inline uint8_t gre0_hdr_get_checksum_flag(const struct gre0_hdr *hdr)
{
    return (ntohs(hdr->flags) & GRE0_FLAG_CHECKSUM) >> 15;
}

static inline uint8_t gre0_hdr_get_routing_flag(const struct gre0_hdr *hdr)
{
    return (ntohs(hdr->flags) & GRE0_FLAG_ROUTING) >> 14;
}

static inline uint8_t gre0_hdr_get_key_flag(const struct gre0_hdr *hdr)
{
    return (ntohs(hdr->flags) & GRE0_FLAG_KEY) >> 13;
}

static inline uint8_t gre0_hdr_get_seq_flag(const struct gre0_hdr *hdr)
{
    return (ntohs(hdr->flags) & GRE0_FLAG_SEQUENCE) >> 12;
}

static inline uint8_t gre0_hdr_get_strict_flag(const struct gre0_hdr *hdr)
{
    return (ntohs(hdr->flags) & GRE0_FLAG_STRICT) >> 11;
}

static inline uint8_t gre0_hdr_get_version(const struct gre0_hdr *hdr)
{
    return ntohs(hdr->flags) & GRE0_FLAG_VERSION;
}

// ethernet protocol
static inline uint16_t gre0_hdr_get_proto(const struct gre0_hdr *hdr)
{
    return ntohs(hdr->protocol);
}

static inline uint16_t gre0_hdr_get_checksum(const struct gre0_hdr *hdr)
{
    if (gre0_hdr_get_checksum_flag(hdr))
    {
        const char *ptr = ((const char *)hdr) + 4;
        return ntohs(*(uint16_t *)ptr);
    }
    else
    {
        return 0;
    }
}

static inline uint16_t gre0_hdr_get_offset(const struct gre0_hdr *hdr)
{
    if (gre0_hdr_get_checksum_flag(hdr))
    {
        const char *ptr = ((const char *)hdr) + 6;
        return ntohs(*(uint16_t *)ptr);
    }
    else
    {
        return 0;
    }
}

#pragma GCC diagnostic warning "-Warray-bounds" 

static inline uint32_t gre0_hdr_get_key(const struct gre0_hdr *hdr)
{
    if (gre0_hdr_get_key_flag(hdr))
    {
        const char *ptr = ((const char *)hdr) + 4;
        if (gre0_hdr_get_checksum_flag(hdr))
        {
            ptr += 4;
        }

        return ntohl(*(uint32_t *)ptr);
    }
    else
    {
        return 0;
    }
}

static inline uint32_t gre0_hdr_get_seq(const struct gre0_hdr *hdr)
{
    if (gre0_hdr_get_seq_flag(hdr))
    {
        const char *ptr = ((const char *)hdr) + 4;
        if (gre0_hdr_get_checksum_flag(hdr))
        {
            ptr += 4;
        }

        if (gre0_hdr_get_key_flag(hdr))
        {
            ptr += 4;
        }

        return ntohl(*(uint32_t *)ptr);
    }
    else
    {
        return 0;
    }
}

static inline const char *gre0_hdr_get_routing_data(const struct gre0_hdr *hdr)
{
    if (gre0_hdr_get_routing_flag(hdr))
    {
        const char *ptr = ((const char *)hdr) + 4;
        if (gre0_hdr_get_checksum_flag(hdr))
        {
            ptr += 4;
        }

        if (gre0_hdr_get_key_flag(hdr))
        {
            ptr += 4;
        }

        if (gre0_hdr_get_seq_flag(hdr))
        {
            ptr += 4;
        }

        return ptr;
    }
    else
    {
        return NULL;
    }
}

static inline uint16_t gre0_hdr_get_routing_len(const struct gre0_hdr *hdr)
{
    if (gre0_hdr_get_routing_flag(hdr))
    {
        const char *ptr = gre0_hdr_get_routing_data(hdr);

        uint16_t hdr_len = 0;
        while (1)
        {
            struct sre *sre = (struct sre *)(ptr + hdr_len);
            if (sre->length == 0)
            {
                hdr_len += sizeof(struct sre);
                break;
            }
            else
            {
                hdr_len += sizeof(struct sre) + sre->length;
            }
        }
        return hdr_len;
    }
    else
    {
        return 0;
    }
}

static inline uint16_t calc_gre0_hdr_len(const char *data, uint32_t len)
{
    if (data == NULL || len < sizeof(struct gre0_hdr))
    {
        return 0;
    }
    const struct gre0_hdr *hdr = (const struct gre0_hdr *)data;
    uint16_t hdr_len = 4;
    uint16_t flags = ntohs(hdr->flags);
    if ((flags & GRE0_FLAG_CHECKSUM) || (flags & GRE0_FLAG_ROUTING))
    {
        hdr_len += 4; // skip checksum and offset fields
    }
    if (flags & GRE0_FLAG_KEY)
    {
        hdr_len += 4; // skip key field
    }
    if (flags & GRE0_FLAG_SEQUENCE)
    {
        hdr_len += 4; // skip sequence number field
    }
    if (flags & GRE0_FLAG_ROUTING)
    {
        while (hdr_len + sizeof(struct sre) <= len)
        {
            struct sre *sre = (struct sre *)((char *)data + hdr_len);
            if (sre->length == 0)
            {
                hdr_len += sizeof(struct sre);
                break;
            }
            else
            {
                hdr_len += sizeof(struct sre) + sre->length;
            }
        }
    }
    return hdr_len;
}

static inline void gre0_hdr_set_flags(struct gre0_hdr *hdr, uint16_t flags)
{
    hdr->flags = htons(flags);
}

static inline void gre0_hdr_set_checksum_flag(struct gre0_hdr *hdr, uint8_t flag)
{
    hdr->flags = htons((ntohs(hdr->flags) & ~GRE0_FLAG_CHECKSUM) | flag << 15);
}

static inline void gre0_hdr_set_routing_flag(struct gre0_hdr *hdr, uint8_t flag)
{
    hdr->flags = htons((ntohs(hdr->flags) & ~GRE0_FLAG_ROUTING) | flag << 14);
}

static inline void gre0_hdr_set_key_flag(struct gre0_hdr *hdr, uint8_t flag)
{
    hdr->flags = htons((ntohs(hdr->flags) & ~GRE0_FLAG_KEY) | flag << 13);
}

static inline void gre0_hdr_set_seq_flag(struct gre0_hdr *hdr, uint8_t flag)
{
    hdr->flags = htons((ntohs(hdr->flags) & ~GRE0_FLAG_SEQUENCE) | flag << 12);
}

static inline void gre0_hdr_set_strict_flag(struct gre0_hdr *hdr, uint8_t flag)
{
    hdr->flags = htons((ntohs(hdr->flags) & ~GRE0_FLAG_STRICT) | flag << 11);
}

static inline void gre0_hdr_set_version(struct gre0_hdr *hdr, uint8_t version)
{
    hdr->flags = htons((ntohs(hdr->flags) & ~GRE0_FLAG_VERSION) | version);
}

static inline void gre0_hdr_set_proto(struct gre0_hdr *hdr, uint16_t proto)
{
    hdr->protocol = htons(proto);
}

static inline void gre0_hdr_set_checksum(struct gre0_hdr *hdr, uint16_t checksum)
{
    if (gre0_hdr_get_checksum_flag(hdr))
    {
        char *ptr = ((char *)hdr) + 4;
        *(uint16_t *)ptr = htons(checksum);
    }
}

static inline void gre0_hdr_set_offset(struct gre0_hdr *hdr, uint16_t offset)
{
    if (gre0_hdr_get_checksum_flag(hdr))
    {
        char *ptr = ((char *)hdr) + 6;
        *(uint16_t *)ptr = htons(offset);
    }
}

static inline void gre0_hdr_set_key(struct gre0_hdr *hdr, uint32_t key)
{
    if (gre0_hdr_get_key_flag(hdr))
    {
        char *ptr = ((char *)hdr) + 4;
        if (gre0_hdr_get_checksum_flag(hdr))
        {
            ptr += 4;
        }

        *(uint32_t *)ptr = htonl(key);
    }
}

static inline void gre0_hdr_set_seq(struct gre0_hdr *hdr, uint32_t sequence)
{
    if (gre0_hdr_get_seq_flag(hdr))
    {
        char *ptr = ((char *)hdr) + 4;
        if (gre0_hdr_get_checksum_flag(hdr))
        {
            ptr += 4;
        }

        if (gre0_hdr_get_key_flag(hdr))
        {
            ptr += 4;
        }

        *(uint32_t *)ptr = htonl(sequence);
    }
}

static inline void gre0_hdr_set_routing_data(struct gre0_hdr *hdr, const char *data, uint16_t len)
{
    if (gre0_hdr_get_routing_flag(hdr))
    {
        char *ptr = ((char *)hdr) + 4;
        if (gre0_hdr_get_checksum_flag(hdr))
        {
            ptr += 4;
        }

        if (gre0_hdr_get_key_flag(hdr))
        {
            ptr += 4;
        }

        if (gre0_hdr_get_seq_flag(hdr))
        {
            ptr += 4;
        }

        memcpy(ptr, data, len);
    }
}

static inline int gre0_hdr_to_str(const struct gre0_hdr *hdr, char *buf, size_t size)
{
    memset(buf, 0, size);

    int used = 0;
    uint16_t proto = gre0_hdr_get_proto(hdr);
    used += snprintf(buf + used, size - used, "GRE: flags=0x%04x (checksum_flag=%u, routing_flag=%u, key_flag=%u, seq_flag=%u, strict_flag=%u, version=%u), proto=%s",
                     gre0_hdr_get_flags(hdr), gre0_hdr_get_checksum_flag(hdr), gre0_hdr_get_routing_flag(hdr), gre0_hdr_get_key_flag(hdr),
                     gre0_hdr_get_seq_flag(hdr), gre0_hdr_get_strict_flag(hdr), gre0_hdr_get_version(hdr), eth_proto_to_str(proto));
    if (gre0_hdr_get_checksum_flag(hdr))
    {
        used += snprintf(buf + used, size - used, ", checksum=0x%x, offset=%u", gre0_hdr_get_checksum(hdr), gre0_hdr_get_offset(hdr));
    }
    if (gre0_hdr_get_key_flag(hdr))
    {
        used += snprintf(buf + used, size - used, ", key=%u", gre0_hdr_get_key(hdr));
    }
    if (gre0_hdr_get_seq_flag(hdr))
    {
        used += snprintf(buf + used, size - used, ", seq=%u", gre0_hdr_get_seq(hdr));
    }
    if (gre0_hdr_get_routing_flag(hdr))
    {
        used += snprintf(buf + used, size - used, ", routing_len=%u", gre0_hdr_get_routing_len(hdr));
    }

    return used;
}

/******************************************************************************
 * GREv1
 ******************************************************************************/

/*
 * 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
 */

struct gre1_hdr
{
    uint16_t flags;
    uint16_t protocol;
    uint16_t payload_length;
    uint16_t call_id;
};

#define GRE1_FLAG_CHECKSUM 0x8000
#define GRE1_FLAG_ROUTING 0x4000
#define GRE1_FLAG_KEY 0x2000
#define GRE1_FLAG_SEQUENCE 0x1000
#define GRE1_FLAG_STRICT 0x0800
#define GRE1_FLAG_RECURSION 0x0700
#define GRE1_FLAG_ACK 0x0080 /* only in special PPTPized GRE header */
#define GRE1_FLAG_VERSION 0x0007

static inline uint16_t gre1_hdr_get_flags(const struct gre1_hdr *hdr)
{
    return ntohs(hdr->flags);
}

static inline uint8_t gre1_hdr_get_seq_flag(const struct gre1_hdr *hdr)
{
    return (ntohs(hdr->flags) & GRE1_FLAG_SEQUENCE) >> 12;
}

static inline uint8_t gre1_hdr_get_ack_flag(const struct gre1_hdr *hdr)
{
    return (ntohs(hdr->flags) & GRE1_FLAG_ACK) >> 7;
}

static inline uint8_t gre1_hdr_get_version(const struct gre1_hdr *hdr)
{
    return ntohs(hdr->flags) & GRE1_FLAG_VERSION;
}

// ethernet protocol type
static inline uint16_t gre1_hdr_get_proto(const struct gre1_hdr *hdr)
{
    return ntohs(hdr->protocol);
}

static inline uint16_t gre1_hdr_get_payload_length(const struct gre1_hdr *hdr)
{
    return ntohs(hdr->payload_length);
}

static inline uint16_t gre1_hdr_get_call_id(const struct gre1_hdr *hdr)
{
    return ntohs(hdr->call_id);
}

static inline uint32_t gre1_hdr_get_seq(const struct gre1_hdr *hdr)
{
    if (gre1_hdr_get_seq_flag(hdr))
    {
        const char *ptr = ((const char *)hdr) + 8;
        return ntohl(*((uint32_t *)ptr));
    }
    else
    {
        return 0;
    }
}

static inline uint32_t gre1_hdr_get_ack(const struct gre1_hdr *hdr)
{
    if (gre1_hdr_get_ack_flag(hdr))
    {
        const char *ptr = ((const char *)hdr) + 8;
        if (gre1_hdr_get_seq_flag(hdr))
        {
            ptr += 4;
        }
        return ntohl(*((uint32_t *)ptr));
    }
    else
    {
        return 0;
    }
}

static inline uint16_t calc_gre1_hdr_len(const char *data, uint32_t len)
{
    if (data == NULL || len < sizeof(struct gre1_hdr))
    {
        return 0;
    }
    const struct gre1_hdr *hdr = (const struct gre1_hdr *)data;
    uint16_t hdr_len = 8;
    uint16_t flags = gre1_hdr_get_flags(hdr);
    if (flags & GRE1_FLAG_SEQUENCE)
    {
        hdr_len += 4;
    }
    if (flags & GRE1_FLAG_ACK)
    {
        hdr_len += 4;
    }
    return hdr_len;
}

static inline void gre1_hdr_set_flags(struct gre1_hdr *hdr, uint16_t flags)
{
    hdr->flags = htons(flags);
}

static inline void gre1_hdr_set_seq_flag(struct gre1_hdr *hdr, uint8_t seq_flag)
{
    hdr->flags = htons((ntohs(hdr->flags) & ~GRE1_FLAG_SEQUENCE) | seq_flag << 12);
}

static inline void gre1_hdr_set_ack_flag(struct gre1_hdr *hdr, uint8_t ack_flag)
{
    hdr->flags = htons((ntohs(hdr->flags) & ~GRE1_FLAG_ACK) | ack_flag << 7);
}

static inline void gre1_hdr_set_version(struct gre1_hdr *hdr, uint8_t version)
{
    hdr->flags = htons((ntohs(hdr->flags) & ~GRE1_FLAG_VERSION) | version);
}

static inline void gre1_hdr_set_proto(struct gre1_hdr *hdr, uint16_t proto)
{
    hdr->protocol = htons(proto);
}

static inline void gre1_hdr_set_payload_length(struct gre1_hdr *hdr, uint16_t payload_length)
{
    hdr->payload_length = htons(payload_length);
}

static inline void gre1_hdr_set_call_id(struct gre1_hdr *hdr, uint16_t call_id)
{
    hdr->call_id = htons(call_id);
}

static inline void gre1_hdr_set_seq(struct gre1_hdr *hdr, uint32_t seq)
{
    if (gre1_hdr_get_seq_flag(hdr))
    {
        char *ptr = ((char *)hdr) + 8;
        *((uint32_t *)ptr) = htonl(seq);
    }
}

static inline void gre1_hdr_set_ack(struct gre1_hdr *hdr, uint32_t ack)
{
    if (gre1_hdr_get_ack_flag(hdr))
    {
        char *ptr = ((char *)hdr) + 8;
        if (gre1_hdr_get_seq_flag(hdr))
        {
            ptr += 4;
        }
        *((uint32_t *)ptr) = htonl(ack);
    }
}

static inline int gre1_hdr_to_str(const struct gre1_hdr *hdr, char *buf, size_t size)
{
    memset(buf, 0, size);

    int used = 0;
    uint16_t proto = gre1_hdr_get_proto(hdr);
    used += snprintf(buf + used, size - used, "GRE: flags=0x%04x (seq_flag=%u ack_flag=%u version=%u), proto=%s, payload_length=%u, call_id=%u",
                     gre1_hdr_get_flags(hdr), gre1_hdr_get_seq_flag(hdr), gre1_hdr_get_ack_flag(hdr), gre1_hdr_get_version(hdr),
                     eth_proto_to_str(proto), gre1_hdr_get_payload_length(hdr), gre1_hdr_get_call_id(hdr));
    if (gre1_hdr_get_seq_flag(hdr))
    {
        used += snprintf(buf + used, size - used, ", seq=%u", gre1_hdr_get_seq(hdr));
    }
    if (gre1_hdr_get_ack_flag(hdr))
    {
        used += snprintf(buf + used, size - used, ", ack=%u", gre1_hdr_get_ack(hdr));
    }

    return used;
}

/******************************************************************************
 * GRE
 ******************************************************************************/

// return GRE version 0 or 1
// return 255 if not GRE
static inline uint8_t peek_gre_version(const char *data, uint32_t len)
{
    if (len < MIN(sizeof(struct gre0_hdr), sizeof(struct gre1_hdr)))
    {
        return 255;
    }

    uint16_t flag = *(uint16_t *)data;
    return ntohs(flag) & 0x0007;
}

static inline uint16_t peek_gre_proto(const char *data, uint32_t len)
{
    switch (peek_gre_version(data, len))
    {
    case 0:
        return gre0_hdr_get_proto((const struct gre0_hdr *)data);
    case 1:
        return gre1_hdr_get_proto((const struct gre1_hdr *)data);
    default:
        return 0;
    }
}

static inline uint16_t calc_gre_hdr_len(const char *data, uint32_t len)
{
    switch (peek_gre_version(data, len))
    {
    case 0:
        return calc_gre0_hdr_len(data, len);
    case 1:
        return calc_gre1_hdr_len(data, len);
    default:
        return 0;
    }
}

static inline int gre_hdr_to_str(const char *hdr, uint16_t len, char *buf, size_t size)
{
    switch (peek_gre_version(hdr, len))
    {
    case 0:
        return gre0_hdr_to_str((const struct gre0_hdr *)hdr, buf, size);
    case 1:
        return gre1_hdr_to_str((const struct gre1_hdr *)hdr, buf, size);
    default:
        return 0;
    }
}

/******************************************************************************
 * GTPv1
 ******************************************************************************/

/*
 * https://en.wikipedia.org/wiki/GPRS_Tunnelling_Protocol
 *
 *  GTP-C: Gateway GPRS Support Nodes (GGSN) <----> Serving GPRS support Nodes (SGSN)
 *  GTP-U: Radio Access Network <----> Core Network
 *
 * GTPv1:
 *  -> GTPv1-C
 *  -> GTPv1-U
 */

/*
 * GTP 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
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 *  | Ver |T|R|E|S|N|  Message Type |       Message Length          |
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 *  |                             TEID                              |
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 *  |        Sequence Number(O)     |N-PDU Number(O)|Next Ext Hdr(O)| (optional headers)
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 *
 * Message Length:
 *      a 16-bit field that indicates the length of the payload in bytes
 *      (rest of the packet following the mandatory 8-byte GTP header).
 *      Includes the optional fields.
 */

/*
 * Next Extension Headers
 *
 *   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
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 *  | Length        |                Contents                       |
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 *  |                         ...                                   |
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 *  |           Contents                            | Next Ext Hdr  |
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 *
 * Extension length
 *      an 8-bit field. This field states the length of this extension header,
 *      including the length, the contents, and the next extension header field,
 *      in 4-octet units, so the length of the extension must always be a multiple of 4.
 */

struct gtp1_hdr
{
    uint8_t flags;
    uint8_t msg_type;
    uint16_t msg_len;
    uint32_t teid;
} __attribute__((packed));

struct gtp1_hdr_long
{
    uint8_t flags;
    uint8_t msg_type;
    uint16_t msg_len;
    uint32_t teid;

    uint16_t seq;
    uint8_t npdu;
    uint8_t next_ext_hdr;
} __attribute__((packed));

#define GTP1_FLAG_N_PDU (0x01)
#define GTP1_FLAG_SEQ_NUM (0x02)
#define GTP1_FLAG_EXT_HDR (0x04)
#define GTP1_FLAG_RESERVED (0x08)
#define GTP1_FLAG_PROTOCOL (0x10)
#define GTP1_FLAG_VERSION (0xE0)

static inline uint8_t gtp1_hdr_get_flags(const struct gtp1_hdr *gtp)
{
    return gtp->flags;
}

static inline uint8_t gtp1_hdr_get_version(const struct gtp1_hdr *gtp)
{
    return (gtp->flags & GTP1_FLAG_VERSION) >> 5;
}

static inline uint8_t gtp1_hdr_get_proto(const struct gtp1_hdr *gtp)
{
    return (gtp->flags & GTP1_FLAG_PROTOCOL) >> 4;
}

static inline uint8_t gtp1_hdr_get_reserved(const struct gtp1_hdr *gtp)
{
    return (gtp->flags & GTP1_FLAG_RESERVED) >> 3;
}

static inline uint8_t gtp1_hdr_get_ext_flag(const struct gtp1_hdr *gtp)
{
    return (gtp->flags & GTP1_FLAG_EXT_HDR) >> 2;
}

static inline uint8_t gtp1_hdr_get_seq_flag(const struct gtp1_hdr *gtp)
{
    return (gtp->flags & GTP1_FLAG_SEQ_NUM) >> 1;
}

static inline uint8_t gtp1_hdr_get_npdu_flag(const struct gtp1_hdr *gtp)
{
    return (gtp->flags & GTP1_FLAG_N_PDU) >> 0;
}

static inline uint8_t gtp1_hdr_get_msg_type(const struct gtp1_hdr *gtp)
{
    return gtp->msg_type;
}

static inline uint16_t gtp1_hdr_get_msg_len(const struct gtp1_hdr *gtp)
{
    return ntohs(gtp->msg_len);
}

static inline uint32_t gtp1_hdr_get_teid(const struct gtp1_hdr *gtp)
{
    return ntohl(gtp->teid);
}

static inline uint16_t gtp1_hdr_get_seq(const struct gtp1_hdr *gtp)
{
    if (gtp1_hdr_get_seq_flag(gtp))
    {
        const struct gtp1_hdr_long *gtp_long = (const struct gtp1_hdr_long *)gtp;
        return ntohs(gtp_long->seq);
    }
    else
    {
        return 0;
    }
}

static inline uint8_t gtp1_hdr_get_npdu(const struct gtp1_hdr *gtp)
{
    if (gtp1_hdr_get_npdu_flag(gtp))
    {
        const struct gtp1_hdr_long *gtp_long = (const struct gtp1_hdr_long *)gtp;
        return gtp_long->npdu;
    }
    else
    {
        return 0;
    }
}

static inline uint8_t gtp1_hdr_get_next_ext_type(const struct gtp1_hdr *gtp)
{
    if (gtp1_hdr_get_ext_flag(gtp))
    {
        const struct gtp1_hdr_long *gtp_long = (const struct gtp1_hdr_long *)gtp;
        return gtp_long->next_ext_hdr;
    }
    else
    {
        return 0;
    }
}

// include gtp fixed header and optional headers and extension headers
static inline uint16_t calc_gtp1_hdr_len(const char *data, uint16_t len)
{
    if (data == NULL || len < sizeof(struct gtp1_hdr))
    {
        return 0;
    }

    const struct gtp1_hdr *gtp = (const struct gtp1_hdr *)data;
    if (gtp1_hdr_get_version(gtp) != 1)
    {
        return 0;
    }

    if (gtp1_hdr_get_flags(gtp) & (GTP1_FLAG_SEQ_NUM | GTP1_FLAG_N_PDU | GTP1_FLAG_EXT_HDR))
    {
        if (sizeof(struct gtp1_hdr_long) > len)
        {
            return 0;
        }

        const struct gtp1_hdr_long *gtp_long = (const struct gtp1_hdr_long *)data;
        uint8_t next_ext_hdr = gtp_long->next_ext_hdr;
        uint16_t offset = sizeof(struct gtp1_hdr_long);

        while (next_ext_hdr)
        {
            if (offset + 1 > len)
            {
                return 0;
            }
            uint16_t ext_hdr_len = *((char *)data + offset) * 4;
            if (ext_hdr_len == 0 || offset + ext_hdr_len > len)
            {
                return 0;
            }
            offset += ext_hdr_len; // skip extension header
            next_ext_hdr = *((char *)data + offset - 1);
        }

        return offset;
    }
    else
    {
        return sizeof(struct gtp1_hdr);
    }
}

static inline void gtp1_hdr_set_flags(struct gtp1_hdr *gtp, uint8_t flags)
{
    gtp->flags = flags;
}

static inline void gtp1_hdr_set_version(struct gtp1_hdr *gtp, uint8_t version)
{
    gtp->flags = (gtp->flags & ~GTP1_FLAG_VERSION) | (version << 5);
}

static inline void gtp1_hdr_set_proto(struct gtp1_hdr *gtp, uint8_t protocol_type)
{
    gtp->flags = (gtp->flags & ~GTP1_FLAG_PROTOCOL) | (protocol_type << 4);
}

static inline void gtp1_hdr_set_reserved(struct gtp1_hdr *gtp, uint8_t reserved)
{
    gtp->flags = (gtp->flags & ~GTP1_FLAG_RESERVED) | (reserved << 3);
}

static inline void gtp1_hdr_set_ext_flag(struct gtp1_hdr *gtp, uint8_t ext_flag)
{
    gtp->flags = (gtp->flags & ~GTP1_FLAG_EXT_HDR) | (ext_flag << 2);
}

static inline void gtp1_hdr_set_seq_flag(struct gtp1_hdr *gtp, uint8_t seq_flag)
{
    gtp->flags = (gtp->flags & ~GTP1_FLAG_SEQ_NUM) | (seq_flag << 1);
}

static inline void gtp1_hdr_set_npdu_flag(struct gtp1_hdr *gtp, uint8_t npdu_flag)
{
    gtp->flags = (gtp->flags & ~GTP1_FLAG_N_PDU) | (npdu_flag << 0);
}

static inline void gtp1_hdr_set_msg_type(struct gtp1_hdr *gtp, uint8_t msg_type)
{
    gtp->msg_type = msg_type;
}

static inline void gtp1_hdr_set_msg_len(struct gtp1_hdr *gtp, uint16_t msg_len)
{
    gtp->msg_len = htons(msg_len);
}

static inline void gtp1_hdr_set_teid(struct gtp1_hdr *gtp, uint32_t teid)
{
    gtp->teid = htonl(teid);
}

static inline void gtp1_hdr_set_seq(struct gtp1_hdr *gtp, uint16_t seq)
{
    if (gtp1_hdr_get_seq_flag(gtp))
    {
        struct gtp1_hdr_long *gtp_long = (struct gtp1_hdr_long *)gtp;
        gtp_long->seq = htons(seq);
    }
}

static inline void gtp1_hdr_set_npdu(struct gtp1_hdr *gtp, uint8_t npdu)
{
    if (gtp1_hdr_get_npdu_flag(gtp))
    {
        struct gtp1_hdr_long *gtp_long = (struct gtp1_hdr_long *)gtp;
        gtp_long->npdu = npdu;
    }
}

static inline void gtp1_hdr_set_next_ext_type(struct gtp1_hdr *gtp, uint8_t next_ext_hdr)
{
    if (gtp1_hdr_get_ext_flag(gtp))
    {
        struct gtp1_hdr_long *gtp_long = (struct gtp1_hdr_long *)gtp;
        gtp_long->next_ext_hdr = next_ext_hdr;
    }
}

static inline int gtp1_hdr_to_str(const struct gtp1_hdr *gtp, char *buf, size_t len)
{
    memset(buf, 0, len);

    int used = 0;
    used += snprintf(buf + used, len - used, "GTP: flags=0x%02x (version=%u, protocol=%u, reserved=%u, ext_flag=%u, seq_flag=%u, npdu_flag=%u), msg_type=0x%02x, msg_len=%u, teid=%u",
                     gtp1_hdr_get_flags(gtp),
                     gtp1_hdr_get_version(gtp),
                     gtp1_hdr_get_proto(gtp),
                     gtp1_hdr_get_reserved(gtp),
                     gtp1_hdr_get_ext_flag(gtp),
                     gtp1_hdr_get_seq_flag(gtp),
                     gtp1_hdr_get_npdu_flag(gtp),
                     gtp1_hdr_get_msg_type(gtp),
                     gtp1_hdr_get_msg_len(gtp),
                     gtp1_hdr_get_teid(gtp));

    if (gtp1_hdr_get_seq_flag(gtp))
    {
        used += snprintf(buf + used, len - used, ", seq=%u", gtp1_hdr_get_seq(gtp));
    }
    if (gtp1_hdr_get_npdu_flag(gtp))
    {
        used += snprintf(buf + used, len - used, ", npdu=%u", gtp1_hdr_get_npdu(gtp));
    }
    if (gtp1_hdr_get_ext_flag(gtp))
    {
        used += snprintf(buf + used, len - used, ", next_ext_hdr=%u", gtp1_hdr_get_next_ext_type(gtp));
    }
    return used;
}

/******************************************************************************
 * GTPv2
 ******************************************************************************/

/*
 * https://en.wikipedia.org/wiki/GPRS_Tunnelling_Protocol
 * https://www.etsi.org/deliver/etsi_ts/129200_129299/129274/08.04.00_60/ts_129274v080400p.pdf
 *
 *  GTP-C: Gateway GPRS Support Nodes (GGSN) <----> Serving GPRS support Nodes (SGSN)
 *  GTP-U: Radio Access Network <----> Core Network
 *
 * GTPv2:
 *  -> GTPv2-C
 *  -> (no GTPv2-U)
 */

/*
 * GTP version 2
 *
 *   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
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 *  | Ver |P|T|spare|  Message Type |       Message Length          |
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 *  |                            TEID (only if T=1)                 |
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 *  | Sequence Number                               | Spare         |
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 *
 * Message length:
 *      This field shall indicate the length of the message in octets excluding the mandatory of the GTP-C header (the first 4 octets).
 *      The TEID (if present) and the Sequence Number shall be included in the length count.
 */

struct gtp2_hdr
{
    uint8_t flags;
    uint8_t msg_type;
    uint16_t msg_len;
    uint32_t seq_and_spare;
} __attribute__((packed));

struct gtp2_hdr_long
{
    uint8_t flags;
    uint8_t msg_type;
    uint16_t msg_len;
    uint32_t teid;
    uint32_t seq_and_spare;
} __attribute__((packed));

#define GPT2_FLAG_SPARE (0x07)
#define GTP2_FLAG_TEID (0x08)
#define GTP2_FLAG_PIGGYBACK (0x10)
#define GTP2_FLAG_VERSION (0xE0)

static inline uint8_t gtp2_hdr_get_flags(const struct gtp2_hdr *gtp)
{
    return gtp->flags;
}

static inline uint8_t gtp2_hdr_get_version(const struct gtp2_hdr *gtp)
{
    return (gtp->flags & GTP2_FLAG_VERSION) >> 5;
}

static inline uint8_t gtp2_hdr_get_piggyback_flag(const struct gtp2_hdr *gtp)
{
    return (gtp->flags & GTP2_FLAG_PIGGYBACK) >> 4;
}

static inline uint8_t gtp2_hdr_get_teid_flag(const struct gtp2_hdr *gtp)
{
    return (gtp->flags & GTP2_FLAG_TEID) >> 3;
}

static inline uint8_t gtp2_hdr_get_spare_flag(const struct gtp2_hdr *gtp)
{
    return (gtp->flags & GPT2_FLAG_SPARE) >> 0;
}

static inline uint8_t gtp2_hdr_get_msg_type(const struct gtp2_hdr *gtp)
{
    return gtp->msg_type;
}

static inline uint16_t gtp2_hdr_get_msg_len(const struct gtp2_hdr *gtp)
{
    return ntohs(gtp->msg_len);
}

static inline uint32_t gtp2_hdr_get_teid(const struct gtp2_hdr *gtp)
{
    if (gtp2_hdr_get_teid_flag(gtp))
    {
        const struct gtp2_hdr_long *gtp_long = (const struct gtp2_hdr_long *)gtp;
        return ntohl(gtp_long->teid);
    }
    else
    {
        return 0;
    }
}

static inline uint32_t gtp2_hdr_get_seq(const struct gtp2_hdr *gtp)
{
    if (gtp2_hdr_get_teid_flag(gtp))
    {
        const struct gtp2_hdr_long *gtp_long = (const struct gtp2_hdr_long *)gtp;
        return ntohl(gtp_long->seq_and_spare) >> 8;
    }
    else
    {
        return ntohl(gtp->seq_and_spare) >> 8;
    }
}

static inline uint8_t gtp2_hdr_get_spare(const struct gtp2_hdr *gtp)
{
    if (gtp2_hdr_get_teid_flag(gtp))
    {
        const struct gtp2_hdr_long *gtp_long = (const struct gtp2_hdr_long *)gtp;
        return ntohl(gtp_long->seq_and_spare) & 0xFF;
    }
    else
    {
        return ntohl(gtp->seq_and_spare) & 0xFF;
    }
}

static inline uint16_t calc_gtp2_hdr_len(const char *data, uint16_t len)
{
    if (len < sizeof(struct gtp2_hdr))
    {
        return 0;
    }

    const struct gtp2_hdr *gtp = (const struct gtp2_hdr *)data;
    if (gtp2_hdr_get_version(gtp) == 2)
    {
        if (gtp2_hdr_get_teid_flag(gtp))
        {
            return sizeof(struct gtp2_hdr_long);
        }
        else
        {
            return sizeof(struct gtp2_hdr);
        }
    }

    return 0;
}

static inline void gtp2_hdr_set_flags(struct gtp2_hdr *gtp, uint8_t flags)
{
    gtp->flags = flags;
}

static inline void gtp2_hdr_set_version(struct gtp2_hdr *gtp, uint8_t version)
{
    gtp->flags = (gtp->flags & ~GTP2_FLAG_VERSION) | (version << 5);
}

static inline void gtp2_hdr_set_piggyback_flag(struct gtp2_hdr *gtp, uint8_t piggyback)
{
    gtp->flags = (gtp->flags & ~GTP2_FLAG_PIGGYBACK) | (piggyback << 4);
}

static inline void gtp2_hdr_set_teid_flag(struct gtp2_hdr *gtp, uint8_t teid_flag)
{
    gtp->flags = (gtp->flags & ~GTP2_FLAG_TEID) | (teid_flag << 3);
}

static inline void gtp2_hdr_set_spare_flag(struct gtp2_hdr *gtp, uint8_t spare_flag)
{
    gtp->flags = (gtp->flags & ~GPT2_FLAG_SPARE) | (spare_flag << 0);
}

static inline void gtp2_hdr_set_msg_type(struct gtp2_hdr *gtp, uint8_t msg_type)
{
    gtp->msg_type = msg_type;
}

static inline void gtp2_hdr_set_msg_len(struct gtp2_hdr *gtp, uint16_t msg_len)
{
    gtp->msg_len = htons(msg_len);
}

static inline void gtp2_hdr_set_teid(struct gtp2_hdr *gtp, uint32_t teid)
{
    if (gtp2_hdr_get_teid_flag(gtp))
    {
        struct gtp2_hdr_long *gtp_long = (struct gtp2_hdr_long *)gtp;
        gtp_long->teid = htonl(teid);
    }
}

static inline void gtp2_hdr_set_seq(struct gtp2_hdr *gtp, uint32_t seq)
{
    if (gtp2_hdr_get_teid_flag(gtp))
    {
        struct gtp2_hdr_long *gtp_long = (struct gtp2_hdr_long *)gtp;
        gtp_long->seq_and_spare = htonl((seq << 8) | gtp2_hdr_get_spare(gtp));
    }
    else
    {
        gtp->seq_and_spare = htonl((seq << 8) | gtp2_hdr_get_spare(gtp));
    }
}

static inline void gtp2_hdr_set_spare(struct gtp2_hdr *gtp, uint8_t spare)
{
    if (gtp2_hdr_get_teid_flag(gtp))
    {
        struct gtp2_hdr_long *gtp_long = (struct gtp2_hdr_long *)gtp;
        gtp_long->seq_and_spare = htonl((gtp2_hdr_get_seq(gtp) << 8) | spare);
    }
    else
    {
        gtp->seq_and_spare = htonl((gtp2_hdr_get_seq(gtp) << 8) | spare);
    }
}

static inline int gtp2_hdr_to_str(const struct gtp2_hdr *gtp, char *buf, size_t len)
{
    memset(buf, 0, len);

    int used = 0;
    used += snprintf(buf + used, len - used, "GTP: flags=0x%02x (version=%u, piggyback=%u, teid_flag=%u, spare_flag=%u), msg_type=0x%02x, msg_len=%u",
                     gtp2_hdr_get_flags(gtp),
                     gtp2_hdr_get_version(gtp),
                     gtp2_hdr_get_piggyback_flag(gtp),
                     gtp2_hdr_get_teid_flag(gtp),
                     gtp2_hdr_get_spare_flag(gtp),
                     gtp2_hdr_get_msg_type(gtp),
                     gtp2_hdr_get_msg_len(gtp));

    if (gtp2_hdr_get_teid_flag(gtp))
    {
        used += snprintf(buf + used, len - used, ", teid=%u", gtp2_hdr_get_teid(gtp));
    }
    else
    {
        used += snprintf(buf + used, len - used, ", seq=%u, spare=%u", gtp2_hdr_get_seq(gtp), gtp2_hdr_get_spare(gtp));
    }
    return used;
}

/******************************************************************************
 * GTP
 ******************************************************************************/

// return GTP version 0 or 1
// return 255 if not GTP
static inline uint8_t peek_gtp_version(const char *data, uint16_t len)
{
    if (data == NULL || len == 0)
    {
        return 255;
    }

    return ((*(uint8_t *)data) >> 5) & 0x07;
}

static inline uint16_t calc_gtp_hdr_len(const char *data, uint32_t len)
{
    switch (peek_gtp_version(data, len))
    {
    case 1:
        return calc_gtp1_hdr_len(data, len);
    case 2:
        return calc_gtp2_hdr_len(data, len);
    default:
        return 0;
    }
}

static inline int gtp_hdr_to_str(const char *hdr, uint16_t len, char *buf, size_t size)
{
    switch (peek_gtp_version(hdr, len))
    {
    case 1:
        return gtp1_hdr_to_str((const struct gtp1_hdr *)hdr, buf, size);
    case 2:
        return gtp2_hdr_to_str((const struct gtp2_hdr *)hdr, buf, size);
    default:
        return 0;
    }
}

/******************************************************************************
 * IPv4
 ******************************************************************************/

/*
 * Internet Header 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
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 *  |Version|  IHL  |Type of Service|          Total Length         |
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 *  |         Identification        |Flags|      Fragment Offset    |
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 *  |  Time to Live |    Protocol   |         Header Checksum       |
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 *  |                       Source Address                          |
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 *  |                    Destination Address                        |
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 *  |                    Options                    |    Padding    |
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 */

static inline uint8_t ip4_hdr_get_version(const struct ip *hdr)
{
    return hdr->ip_v;
}

// IP Options are included in the hdr_len field
static inline uint8_t ip4_hdr_get_hdr_len(const struct ip *hdr)
{
    return hdr->ip_hl << 2;
}

static inline uint8_t ip4_hdr_get_tos(const struct ip *hdr)
{
    return hdr->ip_tos;
}

static inline uint16_t ip4_hdr_get_total_len(const struct ip *hdr)
{
    return ntohs(hdr->ip_len);
}

static inline uint16_t ip4_hdr_get_ipid(const struct ip *hdr)
{
    return ntohs(hdr->ip_id);
}

static inline uint8_t ip4_hdr_get_flags(const struct ip *hdr)
{
    return (ntohs(hdr->ip_off) & (~IP_OFFMASK)) >> 13;
}

static inline bool ip4_hdr_get_rf_flag(const struct ip *hdr)
{
    return (ntohs(hdr->ip_off) & IP_RF) != 0;
}

static inline bool ip4_hdr_get_df_flag(const struct ip *hdr)
{
    return (ntohs(hdr->ip_off) & IP_DF) != 0;
}

static inline bool ip4_hdr_get_mf_flag(const struct ip *hdr)
{
    return (ntohs(hdr->ip_off) & IP_MF) != 0;
}

static inline uint16_t ip4_hdr_get_frag_offset(const struct ip *hdr)
{
    return (ntohs(hdr->ip_off) & IP_OFFMASK) << 3;
}

static inline uint8_t ip4_hdr_get_ttl(const struct ip *hdr)
{
    return hdr->ip_ttl;
}

static inline uint8_t ip4_hdr_get_proto(const struct ip *hdr)
{
    return hdr->ip_p;
}

static inline uint16_t ip4_hdr_get_checksum(const struct ip *hdr)
{
    return ntohs(hdr->ip_sum);
}

static inline uint32_t ip4_hdr_get_src_addr(const struct ip *hdr)
{
    return ntohl(hdr->ip_src.s_addr);
}

static inline uint32_t ip4_hdr_get_dst_addr(const struct ip *hdr)
{
    return ntohl(hdr->ip_dst.s_addr);
}

static inline struct in_addr ip4_hdr_get_src_in_addr(const struct ip *hdr)
{
    return hdr->ip_src;
}

static inline struct in_addr ip4_hdr_get_dst_in_addr(const struct ip *hdr)
{
    return hdr->ip_dst;
}

static inline uint8_t ip4_hdr_get_opt_len(const struct ip *hdr)
{
    return ip4_hdr_get_hdr_len(hdr) - sizeof(struct ip);
}

static inline const char *ip4_hdr_get_opt_data(const struct ip *hdr)
{
    if (ip4_hdr_get_opt_len(hdr) == 0)
    {
        return NULL;
    }

    return (const char *)hdr + sizeof(struct ip);
}

static inline void ip4_hdr_set_version(struct ip *hdr, uint8_t version)
{
    hdr->ip_v = version;
}

static inline void ip4_hdr_set_hdr_len(struct ip *hdr, uint8_t hdr_len)
{
    hdr->ip_hl = hdr_len >> 2;
}

static inline void ip4_hdr_set_tos(struct ip *hdr, uint8_t tos)
{
    hdr->ip_tos = tos;
}

static inline void ip4_hdr_set_total_len(struct ip *hdr, uint16_t total_len)
{
    hdr->ip_len = htons(total_len);
}

static inline void ip4_hdr_set_ipid(struct ip *hdr, uint16_t ipid)
{
    hdr->ip_id = htons(ipid);
}

static inline void ip4_hdr_set_flags(struct ip *hdr, uint8_t flags)
{
    hdr->ip_off = htons((flags << 13) | (ntohs(hdr->ip_off) & IP_OFFMASK));
}

static inline void ip4_hdr_set_rf_flag(struct ip *hdr, bool flag)
{
    if (flag)
    {
        hdr->ip_off = htons(ntohs(hdr->ip_off) | IP_RF);
    }
    else
    {
        hdr->ip_off = htons(ntohs(hdr->ip_off) & ~IP_RF);
    }
}

static inline void ip4_hdr_set_df_flag(struct ip *hdr, bool flag)
{
    if (flag)
    {
        hdr->ip_off = htons(ntohs(hdr->ip_off) | IP_DF);
    }
    else
    {
        hdr->ip_off = htons(ntohs(hdr->ip_off) & ~IP_DF);
    }
}

static inline void ip4_hdr_set_mf_flag(struct ip *hdr, bool flag)
{
    if (flag)
    {
        hdr->ip_off = htons(ntohs(hdr->ip_off) | IP_MF);
    }
    else
    {
        hdr->ip_off = htons(ntohs(hdr->ip_off) & ~IP_MF);
    }
}

static inline void ip4_hdr_set_frag_offset(struct ip *hdr, uint16_t frag_offset)
{
    hdr->ip_off = htons((frag_offset >> 3) | (ntohs(hdr->ip_off) & ~IP_OFFMASK));
}

static inline void ip4_hdr_set_ttl(struct ip *hdr, uint8_t ttl)
{
    hdr->ip_ttl = ttl;
}

static inline void ip4_hdr_set_protocol(struct ip *hdr, uint8_t protocol)
{
    hdr->ip_p = protocol;
}

static inline void ip4_hdr_set_checksum(struct ip *hdr, uint16_t checksum)
{
    hdr->ip_sum = htons(checksum);
}

static inline void ip4_hdr_set_src_addr(struct ip *hdr, uint32_t saddr)
{
    hdr->ip_src.s_addr = htonl(saddr);
}

static inline void ip4_hdr_set_dst_addr(struct ip *hdr, uint32_t daddr)
{
    hdr->ip_dst.s_addr = htonl(daddr);
}

static inline void ip4_hdr_set_src_in_addr(struct ip *hdr, struct in_addr saddr)
{
    hdr->ip_src = saddr;
}

static inline void ip4_hdr_set_dst_in_addr(struct ip *hdr, struct in_addr daddr)
{
    hdr->ip_dst = daddr;
}

static inline void ip4_hdr_set_opt_len(struct ip *hdr, uint8_t opt_len)
{
    ip4_hdr_set_hdr_len(hdr, opt_len + sizeof(struct ip));
}

// must be called after ip4_hdr_set_opt_len
static inline void ip4_hdr_set_opt_data(struct ip *hdr, const char *opt_data)
{
    if (opt_data)
    {
        memcpy((char *)hdr + sizeof(struct ip), opt_data, ip4_hdr_get_opt_len(hdr));
    }
}

// /usr/include/netinet/in.h
static inline int is_ip_proto(uint16_t proto)
{
    switch (proto)
    {
    case IPPROTO_IP:
    case IPPROTO_ICMP:
    case IPPROTO_IGMP:
    case IPPROTO_IPIP:
    case IPPROTO_TCP:
    case IPPROTO_EGP:
    case IPPROTO_PUP:
    case IPPROTO_UDP:
    case IPPROTO_IDP:
    case IPPROTO_TP:
    case IPPROTO_DCCP:
    case IPPROTO_IPV6:
    case IPPROTO_RSVP:
    case IPPROTO_GRE:
    case IPPROTO_ESP:
    case IPPROTO_AH:
    case IPPROTO_MTP:
    case IPPROTO_BEETPH:
    case IPPROTO_ENCAP:
    case IPPROTO_PIM:
    case IPPROTO_COMP:
    case IPPROTO_SCTP:
    case IPPROTO_UDPLITE:
    case IPPROTO_MPLS:
    case IPPROTO_ETHERNET:
    case IPPROTO_RAW:
    case IPPROTO_MPTCP:
        return 1;
    default:
        return 0;
    }
}

static inline const char *ip_proto_to_str(uint16_t proto)
{
    switch (proto)
    {
    case IPPROTO_IP:
        return "IPPROTO_IP";
    case IPPROTO_ICMP:
        return "IPPROTO_ICMP";
    case IPPROTO_IGMP:
        return "IPPROTO_IGMP";
    case IPPROTO_IPIP:
        return "IPPROTO_IPIP";
    case IPPROTO_TCP:
        return "IPPROTO_TCP";
    case IPPROTO_EGP:
        return "IPPROTO_EGP";
    case IPPROTO_PUP:
        return "IPPROTO_PUP";
    case IPPROTO_UDP:
        return "IPPROTO_UDP";
    case IPPROTO_IDP:
        return "IPPROTO_IDP";
    case IPPROTO_TP:
        return "IPPROTO_TP";
    case IPPROTO_DCCP:
        return "IPPROTO_DCCP";
    case IPPROTO_IPV6:
        return "IPPROTO_IPV6";
    case IPPROTO_RSVP:
        return "IPPROTO_RSVP";
    case IPPROTO_GRE:
        return "IPPROTO_GRE";
    case IPPROTO_ESP:
        return "IPPROTO_ESP";
    case IPPROTO_AH:
        return "IPPROTO_AH";
    case IPPROTO_MTP:
        return "IPPROTO_MTP";
    case IPPROTO_BEETPH:
        return "IPPROTO_BEETPH";
    case IPPROTO_ENCAP:
        return "IPPROTO_ENCAP";
    case IPPROTO_PIM:
        return "IPPROTO_PIM";
    case IPPROTO_COMP:
        return "IPPROTO_COMP";
    case IPPROTO_SCTP:
        return "IPPROTO_SCTP";
    case IPPROTO_UDPLITE:
        return "IPPROTO_UDPLITE";
    case IPPROTO_MPLS:
        return "IPPROTO_MPLS";
    case IPPROTO_ETHERNET:
        return "IPPROTO_ETHERNET";
    case IPPROTO_RAW:
        return "IPPROTO_RAW";
    case IPPROTO_MPTCP:
        return "IPPROTO_MPTCP";
    default:
        return "IPPROTO_UNKNOWN";
    }
}

static inline int ip4_hdr_to_str(const struct ip *hdr, char *buf, size_t size)
{
    memset(buf, 0, size);
    char src_addr_str[INET6_ADDRSTRLEN] = {0};
    char dst_addr_str[INET6_ADDRSTRLEN] = {0};

    uint16_t proto = ip4_hdr_get_proto(hdr);
    struct in_addr src_addr = ip4_hdr_get_src_in_addr(hdr);
    struct in_addr dst_addr = ip4_hdr_get_dst_in_addr(hdr);
    inet_ntop(AF_INET, &src_addr, src_addr_str, sizeof(src_addr_str));
    inet_ntop(AF_INET, &dst_addr, dst_addr_str, sizeof(dst_addr_str));

    return snprintf(buf, size, "IPv4: version=%u hdr_len=%u tos=%u total_len=%u ipid=%u flags=%u(rf=%u df=%u mf=%u) frag_offset=%u ttl=%u proto=%s checksum=0x%x src_addr=%s dst_addr=%s opt_len=%u",
                    ip4_hdr_get_version(hdr), ip4_hdr_get_hdr_len(hdr), ip4_hdr_get_tos(hdr),
                    ip4_hdr_get_total_len(hdr), ip4_hdr_get_ipid(hdr), ip4_hdr_get_flags(hdr),
                    ip4_hdr_get_rf_flag(hdr), ip4_hdr_get_df_flag(hdr), ip4_hdr_get_mf_flag(hdr),
                    ip4_hdr_get_frag_offset(hdr), ip4_hdr_get_ttl(hdr), ip_proto_to_str(proto),
                    ip4_hdr_get_checksum(hdr), src_addr_str, dst_addr_str, ip4_hdr_get_opt_len(hdr));
}

/******************************************************************************
 * IPv6
 ******************************************************************************/

/*
 * IPv6 Header 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
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 *  |Version| Traffic Class |           Flow Label                  |
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 *  |         Payload Length        |  Next Header  |   Hop Limit   |
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 *  |                                                               |
 *  +                                                               +
 *  |                                                               |
 *  +                         Source Address                        +
 *  |                                                               |
 *  +                                                               +
 *  |                                                               |
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 *  |                                                               |
 *  +                                                               +
 *  |                                                               |
 *  +                      Destination Address                      +
 *  |                                                               |
 *  +                                                               +
 *  |                                                               |
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 *
 * Fragment Header
 *
 *   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  |   Reserved    |      Fragment Offset    |Res|M|
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 *  |                         Identification                        |
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 */

static inline uint8_t ip6_hdr_get_version(const struct ip6_hdr *hdr)
{
    return (ntohl(hdr->ip6_flow) & 0xf0000000) >> 28;
}

static inline uint8_t ip6_hdr_get_traffic_class(const struct ip6_hdr *hdr)
{
    return (ntohl(hdr->ip6_flow) & 0x0ff00000) >> 20;
}

static inline uint32_t ip6_hdr_get_flow_label(const struct ip6_hdr *hdr)
{
    return ntohl(hdr->ip6_flow) & 0x000fffff;
}

static inline uint16_t ip6_hdr_get_payload_len(const struct ip6_hdr *hdr)
{
    return ntohs(hdr->ip6_plen);
}

static inline uint8_t ip6_hdr_get_next_header(const struct ip6_hdr *hdr)
{
    return hdr->ip6_nxt;
}

static inline uint8_t ip6_hdr_get_hop_limit(const struct ip6_hdr *hdr)
{
    return hdr->ip6_hlim;
}

static inline struct in6_addr ip6_hdr_get_src_in6_addr(const struct ip6_hdr *hdr)
{
    return hdr->ip6_src;
}

static inline struct in6_addr ip6_hdr_get_dst_in6_addr(const struct ip6_hdr *hdr)
{
    return hdr->ip6_dst;
}

static inline struct ip6_frag *ip6_hdr_get_frag_ext(const struct ip6_hdr *hdr)
{
    if (hdr->ip6_nxt != IPPROTO_FRAGMENT)
    {
        return NULL;
    }
    return (struct ip6_frag *)((char *)hdr + sizeof(struct ip6_hdr));
}

static inline void ip6_hdr_set_version(struct ip6_hdr *hdr, uint8_t version)
{
    hdr->ip6_flow = htonl((ntohl(hdr->ip6_flow) & 0x0fffffff) | (version << 28));
}

static inline void ip6_hdr_set_traffic_class(struct ip6_hdr *hdr, uint8_t traffic_class)
{
    hdr->ip6_flow = htonl((ntohl(hdr->ip6_flow) & 0xf00fffff) | (traffic_class << 20));
}

static inline void ip6_hdr_set_flow_label(struct ip6_hdr *hdr, uint32_t flow_label)
{
    hdr->ip6_flow = htonl((ntohl(hdr->ip6_flow) & 0xfff00000) | flow_label);
}

static inline void ip6_hdr_set_payload_len(struct ip6_hdr *hdr, uint16_t payload_len)
{
    hdr->ip6_plen = htons(payload_len);
}

static inline void ip6_hdr_set_next_header(struct ip6_hdr *hdr, uint8_t next_header)
{
    hdr->ip6_nxt = next_header;
}

static inline void ip6_hdr_set_hop_limit(struct ip6_hdr *hdr, uint8_t hop_limit)
{
    hdr->ip6_hlim = hop_limit;
}

static inline void ip6_hdr_set_src_in6_addr(struct ip6_hdr *hdr, struct in6_addr src_addr)
{
    hdr->ip6_src = src_addr;
}

static inline void ip6_hdr_set_dst_in6_addr(struct ip6_hdr *hdr, struct in6_addr dst_addr)
{
    hdr->ip6_dst = dst_addr;
}

static inline uint8_t ipv6_frag_get_next_header(const struct ip6_frag *frag)
{
    return frag->ip6f_nxt;
}

static inline uint16_t ipv6_frag_get_offset(const struct ip6_frag *frag)
{
    return ntohs(frag->ip6f_offlg & IP6F_OFF_MASK);
}

static inline uint32_t ipv6_frag_get_ident(const struct ip6_frag *frag)
{
    return ntohl(frag->ip6f_ident);
}

static inline bool ipv6_frag_get_more(const struct ip6_frag *frag)
{
    return (frag->ip6f_offlg & IP6F_MORE_FRAG);
}

static inline void ipv6_frag_set_next_header(struct ip6_frag *frag, uint8_t next_header)
{
    frag->ip6f_nxt = next_header;
}

static inline void ipv6_frag_set_offset(struct ip6_frag *frag, uint16_t offset)
{
    frag->ip6f_offlg = (frag->ip6f_offlg & ~IP6F_OFF_MASK) | htons(offset);
}

static inline void ipv6_frag_set_ident(struct ip6_frag *frag, uint32_t ident)
{
    frag->ip6f_ident = htonl(ident);
}

static inline void ipv6_frag_set_more(struct ip6_frag *frag, bool more)
{
    if (more)
    {
        frag->ip6f_offlg |= IP6F_MORE_FRAG;
    }
    else
    {
        frag->ip6f_offlg &= ~IP6F_MORE_FRAG;
    }
}

static inline int ip6_hdr_to_str(const struct ip6_hdr *hdr, char *buf, size_t size)
{
    memset(buf, 0, size);
    char src_addr_str[INET6_ADDRSTRLEN] = {0};
    char dst_addr_str[INET6_ADDRSTRLEN] = {0};

    struct in6_addr src_addr = ip6_hdr_get_src_in6_addr(hdr);
    struct in6_addr dst_addr = ip6_hdr_get_dst_in6_addr(hdr);
    inet_ntop(AF_INET6, &src_addr, src_addr_str, INET6_ADDRSTRLEN);
    inet_ntop(AF_INET6, &dst_addr, dst_addr_str, INET6_ADDRSTRLEN);

    return snprintf(buf, size, "IPv6: version=%u traffic_class=%u flow_label=%u payload_len=%u next_header=%u hop_limit=%u src_addr=%s dst_addr=%s",
                    ip6_hdr_get_version(hdr), ip6_hdr_get_traffic_class(hdr), ip6_hdr_get_flow_label(hdr), ip6_hdr_get_payload_len(hdr),
                    ip6_hdr_get_next_header(hdr), ip6_hdr_get_hop_limit(hdr), src_addr_str, dst_addr_str);
}

/******************************************************************************
 * L2TP
 ******************************************************************************/

struct l2tp_hdr
{
    uint16_t flags;
    // other option fields
};

#define L2TP_CONTROL_BIT 0x8000
#define L2TP_LENGTH_BIT 0x4000
#define L2TP_SEQUENCE_BIT 0x0800
#define L2TP_OFFSET_BIT 0x0200
#define L2TP_PRIORITY_BIT 0x0100
#define L2TP_VERSION 0x000f

static inline uint8_t l2tp_hdr_get_ver(const struct l2tp_hdr *hdr)
{
    return ntohs(hdr->flags) & L2TP_VERSION;
}

// 1: control message
// 0: data message
static inline uint8_t l2tp_hdr_get_type(const struct l2tp_hdr *hdr)
{
    return (ntohs(hdr->flags) & L2TP_CONTROL_BIT) >> 15;
}

/*
 * Layer Two Tunneling Protocol "L2TP" (V2)
 *
 *   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)
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 *
 * https://datatracker.ietf.org/doc/html/rfc2661
 */

static inline uint16_t calc_udp_l2tpv2_hdr_len(const char *data, uint16_t len)
{
    const struct l2tp_hdr *hdr = (const struct l2tp_hdr *)data;
    uint16_t flags = ntohs(hdr->flags);

    // ctrl message
    if (flags & L2TP_CONTROL_BIT)
    {
        if ((flags & L2TP_LENGTH_BIT) == 0 || (flags & L2TP_SEQUENCE_BIT) == 0 ||
            (flags & L2TP_OFFSET_BIT) != 0 || (flags & L2TP_PRIORITY_BIT) != 0)
        {
            return 0;
        }
        else
        {
            return ntohs(*((uint16_t *)(data + 2)));
        }
    }
    // data message
    else
    {
        uint16_t skip_len = 2; // skip flags field
        if (flags & L2TP_LENGTH_BIT)
        {
            skip_len += 2; // skip length field
        }
        skip_len += 2; // skip tunnel id field
        skip_len += 2; // skip session id field
        if (flags & L2TP_SEQUENCE_BIT)
        {
            skip_len += 2; // skip ns field
            skip_len += 2; // skip nr field
        }
        if (flags & L2TP_OFFSET_BIT)
        {
            if (skip_len + 2 > len)
            {
                return 0;
            }
            uint16_t offset = ntohs(*((uint16_t *)(data + skip_len)));
            if (offset == 0)
            {
                skip_len += 2; // skip offset field
            }
            else
            {
                skip_len = offset;
            }
        }
        return skip_len;
    }
}

/*
 * Figure 4.1.1.1: L2TPv3 Session Header Over IP
 *
 *   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
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 *  |                           Session ID                          |
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 *  |               Cookie (optional, maximum 64 bits)...
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 *                                                                  |
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 *
 * Figure 4.1.1.2: L2TPv3 Control Message Header Over IP
 *
 *   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
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 *  |                      (32 bits of zeros)                       |
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 *  |T|L|x|x|S|x|x|x|x|x|x|x|  Ver  |             Length            |
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 *  |                     Control Connection ID                     |
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 *  |               Ns              |               Nr              |
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 *
 * Note: Unlike L2TP over UDP, which uses the T bit to distinguish between
 * L2TP control and data packets, L2TP over IP uses the reserved Session
 * ID of zero (0) when sending control messages.
 *
 * https://www.rfc-editor.org/rfc/rfc3931.html
 */

static inline uint16_t calc_ip_l2tpv3_hdr_len(const char *data, uint16_t len)
{
    if (len < 4)
    {
        return 0;
    }
    uint32_t session_id = ntohl(*((uint32_t *)data));
    // data message
    if (session_id)
    {
        // TODO The optional Cookie field contains a variable-length value (maximum 64 bits)
        // TODO L2-Specific Sublayer 4 bytes
        return 4 + 4;
    }
    // control message
    else
    {
        if (len < 16)
        {
            return 0;
        }
        uint16_t flags = ntohs(*((uint16_t *)(data + 4)));
        if ((flags & L2TP_LENGTH_BIT) == 0 || (flags & L2TP_SEQUENCE_BIT) == 0)
        {
            return 0;
        }
        else
        {
            return ntohs(*((uint16_t *)(data + 4 + 2)));
        }
    }
}

/*
 * Layer Two Tunneling Protocol - Version 3 (L2TPv3)
 *
 * Figure 3.2.1: L2TP Control Message Header
 *
 *   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|x|x|x|x|x|x|  Ver  |             Length            |
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 *  |                     Control Connection ID                     |
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 *  |               Ns              |               Nr              |
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 *
 * Figure 4.1.2.1: L2TPv3 Session Header over UDP
 *
 *   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|x|x|x|x|x|x|x|x|x|x|x|  Ver  |          Reserved             |
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 *  |                           Session ID                          |
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 *  |               Cookie (optional, maximum 64 bits)...
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 *                                                                  |
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 *
 * https://www.rfc-editor.org/rfc/rfc3931.html
 */

static inline uint16_t calc_udp_l2tpv3_hdr_len(const char *data, uint16_t len)
{
    if (len < 8)
    {
        return 0;
    }

    const struct l2tp_hdr *hdr = (const struct l2tp_hdr *)data;
    uint16_t flags = ntohs(hdr->flags);

    // ctrl message
    if (flags & L2TP_CONTROL_BIT)
    {
        if ((flags & L2TP_LENGTH_BIT) == 0 || (flags & L2TP_SEQUENCE_BIT) == 0)
        {
            return 0;
        }
        else
        {
            return ntohs(*((uint16_t *)(data + 2)));
        }
    }
    // data message
    else
    {
        // TODO The optional Cookie field contains a variable-length value (maximum 64 bits)
        // TODO L2-Specific Sublayer 4 bytes
        return 8 + 4;
    }
}

static inline int l2tp_hdr_to_str(const struct l2tp_hdr *hdr, char *buf, size_t size)
{
    memset(buf, 0, size);
    return snprintf(buf, size, "L2TP: type=%s version=%u",
                    l2tp_hdr_get_type(hdr) ? "control" : "data", l2tp_hdr_get_ver(hdr));
}

/******************************************************************************
 * MPLS
 ******************************************************************************/

/*
 * Reference: RFC 5462, RFC 3032
 *
 *   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                  | TC  |S|       TTL     |
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 */

static inline uint32_t mpls_label_get_label(const struct mpls_label *hdr)
{
    return ((ntohl(hdr->entry) & MPLS_LS_LABEL_MASK) >> MPLS_LS_LABEL_SHIFT);
}

static inline uint8_t mpls_label_get_tc(const struct mpls_label *hdr)
{
    return ((ntohl(hdr->entry) & MPLS_LS_TC_MASK) >> MPLS_LS_TC_SHIFT);
}

static inline uint8_t mpls_label_get_bos(const struct mpls_label *hdr)
{
    return ((ntohl(hdr->entry) & MPLS_LS_S_MASK) >> MPLS_LS_S_SHIFT);
}

static inline uint8_t mpls_label_get_ttl(const struct mpls_label *hdr)
{
    return ((ntohl(hdr->entry) & MPLS_LS_TTL_MASK) >> MPLS_LS_TTL_SHIFT);
}

static inline void mpls_label_set_label(struct mpls_label *hdr, uint32_t label)
{
    hdr->entry = htonl((ntohl(hdr->entry) & ~MPLS_LS_LABEL_MASK) | (label << MPLS_LS_LABEL_SHIFT));
}

static inline void mpls_label_set_tc(struct mpls_label *hdr, uint8_t tc)
{
    hdr->entry = htonl((ntohl(hdr->entry) & ~MPLS_LS_TC_MASK) | (tc << MPLS_LS_TC_SHIFT));
}

static inline void mpls_label_set_bos(struct mpls_label *hdr, uint8_t bos)
{
    hdr->entry = htonl((ntohl(hdr->entry) & ~MPLS_LS_S_MASK) | (bos << MPLS_LS_S_SHIFT));
}

static inline void mpls_label_set_ttl(struct mpls_label *hdr, uint8_t ttl)
{
    hdr->entry = htonl((ntohl(hdr->entry) & ~MPLS_LS_TTL_MASK) | (ttl << MPLS_LS_TTL_SHIFT));
}

static inline int mpls_label_to_str(const struct mpls_label *hdr, char *buf, size_t size)
{
    return snprintf(buf, size, "MPLS: label=%u tc=%u bos=%u ttl=%u",
                    mpls_label_get_label(hdr), mpls_label_get_tc(hdr),
                    mpls_label_get_bos(hdr), mpls_label_get_ttl(hdr));
}

/******************************************************************************
 * TCP
 ******************************************************************************/

/*
 * TCP Header 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
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 *  |          Source Port          |       Destination Port        |
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 *  |                        Sequence Number                        |
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 *  |                    Acknowledgment Number                      |
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 *  |  Data |           |U|A|P|R|S|F|                               |
 *  | Offset| Reserved  |R|C|S|S|Y|I|            Window             |
 *  |       |           |G|K|H|T|N|N|                               |
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 *  |           Checksum            |         Urgent Pointer        |
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 *  |                    Options                    |    Padding    |
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 *  |                             data                              |
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 */

static inline uint16_t tcp_hdr_get_src_port(const struct tcphdr *hdr)
{
    return ntohs(hdr->th_sport);
}

static inline uint16_t tcp_hdr_get_dst_port(const struct tcphdr *hdr)
{
    return ntohs(hdr->th_dport);
}

static inline uint32_t tcp_hdr_get_seq(const struct tcphdr *hdr)
{
    return ntohl(hdr->th_seq);
}

static inline uint32_t tcp_hdr_get_ack(const struct tcphdr *hdr)
{
    return ntohl(hdr->th_ack);
}

static inline uint8_t tcp_hdr_get_hdr_len(const struct tcphdr *hdr)
{
    return hdr->th_off << 2;
}

static inline uint8_t tcp_hdr_get_flags(const struct tcphdr *hdr)
{
    return hdr->th_flags;
}

static inline bool tcp_hdr_get_urg_flag(const struct tcphdr *hdr)
{
    return hdr->th_flags & TH_URG;
}

static inline bool tcp_hdr_get_ack_flag(const struct tcphdr *hdr)
{
    return hdr->th_flags & TH_ACK;
}

static inline bool tcp_hdr_get_push_flag(const struct tcphdr *hdr)
{
    return hdr->th_flags & TH_PUSH;
}

static inline bool tcp_hdr_get_rst_flag(const struct tcphdr *hdr)
{
    return hdr->th_flags & TH_RST;
}

static inline bool tcp_hdr_get_syn_flag(const struct tcphdr *hdr)
{
    return hdr->th_flags & TH_SYN;
}

static inline bool tcp_hdr_get_fin_flag(const struct tcphdr *hdr)
{
    return hdr->th_flags & TH_FIN;
}

static inline uint16_t tcp_hdr_get_window(const struct tcphdr *hdr)
{
    return ntohs(hdr->th_win);
}

static inline uint16_t tcp_hdr_get_checksum(const struct tcphdr *hdr)
{
    return ntohs(hdr->th_sum);
}

static inline uint16_t tcp_hdr_get_urg_ptr(const struct tcphdr *hdr)
{
    return ntohs(hdr->th_urp);
}

static inline uint16_t tcp_hdr_get_opt_len(const struct tcphdr *hdr)
{
    return tcp_hdr_get_hdr_len(hdr) - sizeof(struct tcphdr);
}

static inline const char *tcp_hdr_get_opt_data(const struct tcphdr *hdr)
{
    if (tcp_hdr_get_opt_len(hdr) == 0)
    {
        return NULL;
    }

    return ((const char *)hdr) + sizeof(struct tcphdr);
}

static inline void tcp_hdr_set_src_port(struct tcphdr *hdr, uint16_t port)
{
    hdr->th_sport = htons(port);
}

static inline void tcp_hdr_set_dst_port(struct tcphdr *hdr, uint16_t port)
{
    hdr->th_dport = htons(port);
}

static inline void tcp_hdr_set_seq(struct tcphdr *hdr, uint32_t seq)
{
    hdr->th_seq = htonl(seq);
}

static inline void tcp_hdr_set_ack(struct tcphdr *hdr, uint32_t ack)
{
    hdr->th_ack = htonl(ack);
}

static inline void tcp_hdr_set_hdr_len(struct tcphdr *hdr, uint8_t offset)
{
    hdr->th_off = offset >> 2;
}

static inline void tcp_hdr_set_flags(struct tcphdr *hdr, uint8_t flags)
{
    hdr->th_flags = flags;
}

static inline void tcp_hdr_set_urg_flag(struct tcphdr *hdr, bool flag)
{
    if (flag)
    {
        hdr->th_flags |= TH_URG;
    }
    else
    {
        hdr->th_flags &= ~TH_URG;
    }
}

static inline void tcp_hdr_set_ack_flag(struct tcphdr *hdr, bool flag)
{
    if (flag)
    {
        hdr->th_flags |= TH_ACK;
    }
    else
    {
        hdr->th_flags &= ~TH_ACK;
    }
}

static inline void tcp_hdr_set_push_flag(struct tcphdr *hdr, bool flag)
{
    if (flag)
    {
        hdr->th_flags |= TH_PUSH;
    }
    else
    {
        hdr->th_flags &= ~TH_PUSH;
    }
}

static inline void tcp_hdr_set_rst_flag(struct tcphdr *hdr, bool flag)
{
    if (flag)
    {
        hdr->th_flags |= TH_RST;
    }
    else
    {
        hdr->th_flags &= ~TH_RST;
    }
}

static inline void tcp_hdr_set_syn_flag(struct tcphdr *hdr, bool flag)
{
    if (flag)
    {
        hdr->th_flags |= TH_SYN;
    }
    else
    {
        hdr->th_flags &= ~TH_SYN;
    }
}

static inline void tcp_hdr_set_fin_flag(struct tcphdr *hdr, bool flag)
{
    if (flag)
    {
        hdr->th_flags |= TH_FIN;
    }
    else
    {
        hdr->th_flags &= ~TH_FIN;
    }
}

static inline void tcp_hdr_set_window(struct tcphdr *hdr, uint16_t window)
{
    hdr->th_win = htons(window);
}

static inline void tcp_hdr_set_checksum(struct tcphdr *hdr, uint16_t checksum)
{
    hdr->th_sum = htons(checksum);
}

static inline void tcp_hdr_set_urg_ptr(struct tcphdr *hdr, uint16_t ptr)
{
    hdr->th_urp = htons(ptr);
}

static inline void tcp_hdr_set_opt_len(struct tcphdr *hdr, uint16_t len)
{
    hdr->th_off = (sizeof(struct tcphdr) + len) >> 2;
}

// must be called after tcp_hdr_set_opt_len
static inline void tcp_hdr_set_opt_data(struct tcphdr *hdr, const char *ptr)
{
    memcpy((char *)hdr + sizeof(struct tcphdr), ptr, tcp_hdr_get_opt_len(hdr));
}

static inline int tcp_hdr_to_str(const struct tcphdr *hdr, char *buf, size_t size)
{
    memset(buf, 0, size);
    return snprintf(buf, size, "TCP: src_port=%u dst_port=%u seq=%u ack=%u hdr_len=%u flags=0x%02x(%s%s%s%s%s%s) window=%u checksum=0x%x urg_ptr=%u opt_len=%u",
                    tcp_hdr_get_src_port(hdr), tcp_hdr_get_dst_port(hdr),
                    tcp_hdr_get_seq(hdr), tcp_hdr_get_ack(hdr),
                    tcp_hdr_get_hdr_len(hdr), tcp_hdr_get_flags(hdr),
                    tcp_hdr_get_urg_flag(hdr) ? "URG " : "",
                    tcp_hdr_get_ack_flag(hdr) ? "ACK " : "",
                    tcp_hdr_get_push_flag(hdr) ? "PSH " : "",
                    tcp_hdr_get_rst_flag(hdr) ? "RST " : "",
                    tcp_hdr_get_syn_flag(hdr) ? "SYN " : "",
                    tcp_hdr_get_fin_flag(hdr) ? "FIN " : "",
                    tcp_hdr_get_window(hdr), tcp_hdr_get_checksum(hdr),
                    tcp_hdr_get_urg_ptr(hdr), tcp_hdr_get_opt_len(hdr));
}

/******************************************************************************
 * UDP
 ******************************************************************************/

/*
 * User Datagram Header 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
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 *  |          Source Port          |       Destination Port        |
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 *  |           Length              |         Checksum              |
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 *  |                             Data                              |
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 */

static inline uint16_t udp_hdr_get_src_port(const struct udphdr *hdr)
{
    return ntohs(hdr->uh_sport);
}

static inline uint16_t udp_hdr_get_dst_port(const struct udphdr *hdr)
{
    return ntohs(hdr->uh_dport);
}

static inline uint16_t udp_hdr_get_total_len(const struct udphdr *hdr)
{
    return ntohs(hdr->uh_ulen);
}

static inline uint16_t udp_hdr_get_checksum(const struct udphdr *hdr)
{
    return ntohs(hdr->uh_sum);
}

static inline void udp_hdr_set_src_port(struct udphdr *hdr, uint16_t port)
{
    hdr->uh_sport = htons(port);
}

static inline void udp_hdr_set_dst_port(struct udphdr *hdr, uint16_t port)
{
    hdr->uh_dport = htons(port);
}

static inline void udp_hdr_set_total_len(struct udphdr *hdr, uint16_t len)
{
    hdr->uh_ulen = htons(len);
}

static inline void udp_hdr_set_checksum(struct udphdr *hdr, uint16_t sum)
{
    hdr->uh_sum = htons(sum);
}

static inline int udp_hdr_to_str(const struct udphdr *hdr, char *buf, size_t size)
{
    memset(buf, 0, size);
    return snprintf(buf, size, "UDP: src_port=%u dst_port=%u total_len=%u checksum=0x%x",
                    udp_hdr_get_src_port(hdr), udp_hdr_get_dst_port(hdr),
                    udp_hdr_get_total_len(hdr), udp_hdr_get_checksum(hdr));
}

/******************************************************************************
 * VLAN
 ******************************************************************************/

/*
 * VLAN Header 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
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 *  | Pri |I|    VLAN ID            |           Ethertype           |
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 */

struct vlan_hdr
{
    uint16_t vlan_tci;
    uint16_t vlan_ethertype;
};

static inline uint8_t vlan_hdr_get_priority(const struct vlan_hdr *hdr)
{
    return (ntohs(hdr->vlan_tci) & 0xe000) >> 13;
}

static inline uint8_t vlan_hdr_get_dei(const struct vlan_hdr *hdr)
{
    return (ntohs(hdr->vlan_tci) & 0x1000) >> 12;
}

static inline uint16_t vlan_hdr_get_vid(const struct vlan_hdr *hdr)
{
    return ntohs(hdr->vlan_tci) & 0x0fff;
}

static inline uint16_t vlan_hdr_get_ethertype(const struct vlan_hdr *hdr)
{
    return ntohs(hdr->vlan_ethertype);
}

static inline void vlan_hdr_set_priority(struct vlan_hdr *hdr, uint8_t priority)
{
    hdr->vlan_tci = htons((ntohs(hdr->vlan_tci) & ~0xe000) | (priority << 13));
}

static inline void vlan_hdr_set_dei(struct vlan_hdr *hdr, uint8_t dei)
{
    hdr->vlan_tci = htons((ntohs(hdr->vlan_tci) & ~0x1000) | (dei << 12));
}

static inline void vlan_hdr_set_vid(struct vlan_hdr *hdr, uint16_t vid)
{
    hdr->vlan_tci = htons((ntohs(hdr->vlan_tci) & ~0x0fff) | vid);
}

static inline void vlan_hdr_set_ethertype(struct vlan_hdr *hdr, uint16_t ethertype)
{
    hdr->vlan_ethertype = htons(ethertype);
}

static inline int vlan_hdr_to_str(const struct vlan_hdr *hdr, char *buf, size_t size)
{
    memset(buf, 0, size);
    uint16_t proto = vlan_hdr_get_ethertype(hdr);
    return snprintf(buf, size, "VLAN: priority=%u dei=%u id=%u ethertype=%s",
                    vlan_hdr_get_priority(hdr), vlan_hdr_get_dei(hdr),
                    vlan_hdr_get_vid(hdr), eth_proto_to_str(proto));
}

/******************************************************************************
 * VXLAN
 ******************************************************************************/

/*
 * VXLAN Header
 *
 *   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
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 *  |R|R|R|R|I|R|R|R|            Reserved                           |
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 *  |                VXLAN Network Identifier (VNI) |   Reserved    |
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 */

struct vxlan_hdr
{
    uint32_t vxlan_flags;
    uint32_t vxlan_vni;
};

static inline uint8_t vxlan_hdr_get_flags(const struct vxlan_hdr *hdr)
{
    return (ntohl(hdr->vxlan_flags) & 0xff000000) >> 24;
}

static inline uint32_t vxlan_hdr_get_vni(const struct vxlan_hdr *hdr)
{
    return (ntohl(hdr->vxlan_vni) & 0xffffff00) >> 8;
}

static inline void vxlan_hdr_set_flags(struct vxlan_hdr *hdr, uint8_t flags)
{
    hdr->vxlan_flags = htonl((ntohl(hdr->vxlan_flags) & ~0xff000000) | (flags << 24));
}

static inline void vxlan_hdr_set_vni(struct vxlan_hdr *hdr, uint32_t vni)
{
    hdr->vxlan_vni = htonl((ntohl(hdr->vxlan_vni) & ~0xffffff00) | (vni << 8));
}

static inline int vxlan_hdr_to_str(const struct vxlan_hdr *hdr, char *buf, size_t size)
{
    memset(buf, 0, size);
    return snprintf(buf, size, "VXLAN: flags=%u vni=%u",
                    vxlan_hdr_get_flags(hdr), vxlan_hdr_get_vni(hdr));
}

#ifdef __cplusplus
}
#endif