#pragma once

#ifdef __cplusplus
extern "C"
{
#endif

#include <stdio.h>
#include <string.h>
#include <arpa/inet.h>

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

/******************************************************************************
 * get
 ******************************************************************************/

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;
}

/******************************************************************************
 * set
 ******************************************************************************/

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);
    }
}

/******************************************************************************
 * print
 ******************************************************************************/

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;
}

#ifdef __cplusplus
}
#endif