stellar-stellar/src/packet/gre1_utils.h

#pragma once

#ifdef __cplusplus
extern "C"
{
#endif

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

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

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

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

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

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

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

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

#ifdef __cplusplus
}
#endif