stellar-stellar/src/packet/packet_utils.cpp

#include <stdlib.h>

#include "log.h"
#include "tuple.h"
#include "tcp_utils.h"
#include "udp_utils.h"
#include "ip4_utils.h"
#include "ip6_utils.h"
#include "packet_def.h"
#include "packet_utils.h"

#define PACKET_UTILS_LOG_ERROR(format, ...) LOG_ERROR("packet utils", format, ##__VA_ARGS__)

/******************************************************************************
 * metadata utils
 ******************************************************************************/

void packet_set_sids(struct packet *pkt, const struct sids *sids)
{
    pkt->meta.sids = *sids;
}

const struct sids *packet_get_sids(const struct packet *pkt)
{
    return &pkt->meta.sids;
}

void packet_prepend_sids(struct packet *pkt, const struct sids *sids)
{
    if (pkt->meta.sids.used + sids->used > MAX_SIDS)
    {
        PACKET_UTILS_LOG_ERROR("sids overflow");
        return;
    }
    else
    {
        for (int i = pkt->meta.sids.used - 1; i >= 0; i--)
        {
            pkt->meta.sids.sid[i + sids->used] = pkt->meta.sids.sid[i];
        }
        for (int i = 0; i < sids->used; i++)
        {
            pkt->meta.sids.sid[i] = sids->sid[i];
        }
        pkt->meta.sids.used += sids->used;
    }
}

void packet_set_session_id(struct packet *pkt, uint64_t id)
{
    pkt->meta.session_id = id;
}

uint64_t packet_get_session_id(const struct packet *pkt)
{
    return pkt->meta.session_id;
}

void packet_set_domain(struct packet *pkt, uint64_t domain)
{
    pkt->meta.domain = domain;
}

uint64_t packet_get_domain(const struct packet *pkt)
{
    return pkt->meta.domain;
}

void packet_set_link_id(struct packet *pkt, uint16_t id)
{
    pkt->meta.link_id = id;
}

uint16_t packet_get_link_id(const struct packet *pkt)
{
    return pkt->meta.link_id;
}

void packet_set_ctrl(struct packet *pkt, uint8_t ctrl)
{
    pkt->meta.is_ctrl = ctrl;
}

uint8_t packet_is_ctrl(const struct packet *pkt)
{
    return pkt->meta.is_ctrl;
}

void packet_set_direction(struct packet *pkt, enum packet_direction dir)
{
    pkt->meta.direction = dir;
}

enum packet_direction packet_get_direction(const struct packet *pkt)
{
    return pkt->meta.direction;
}

void packet_set_action(struct packet *pkt, enum packet_action action)
{
    pkt->meta.action = action;
}

enum packet_action packet_get_action(const struct packet *pkt)
{
    return pkt->meta.action;
}


/******************************************************************************
 * tuple uitls
 ******************************************************************************/

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

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

        if (layer->proto == LAYER_PROTO_IPV4)
        {
            const struct ip *ip4_hdr = (const struct ip *)layer->hdr_ptr;
            tuple->addr_family = AF_INET;
            tuple->src_addr.v4 = ip4_hdr_get_src_in_addr(ip4_hdr);
            tuple->dst_addr.v4 = ip4_hdr_get_dst_in_addr(ip4_hdr);
            return 0;
        }
        if (layer->proto == LAYER_PROTO_IPV6)
        {
            const struct ip6_hdr *ip6_hdr = (const struct ip6_hdr *)layer->hdr_ptr;
            tuple->addr_family = AF_INET6;
            tuple->src_addr.v6 = ip6_hdr_get_src_in6_addr(ip6_hdr);
            tuple->dst_addr.v6 = ip6_hdr_get_dst_in6_addr(ip6_hdr);
            return 0;
        }
    }

    return -1;
}

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

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

        if (layer->proto == LAYER_PROTO_IPV4)
        {
            const struct ip *ip4_hdr = (const struct ip *)layer->hdr_ptr;
            tuple->addr_family = AF_INET;
            tuple->src_addr.v4 = ip4_hdr_get_src_in_addr(ip4_hdr);
            tuple->dst_addr.v4 = ip4_hdr_get_dst_in_addr(ip4_hdr);
            return 0;
        }
        if (layer->proto == LAYER_PROTO_IPV6)
        {
            const struct ip6_hdr *ip6_hdr = (const struct ip6_hdr *)layer->hdr_ptr;
            tuple->addr_family = AF_INET6;
            tuple->src_addr.v6 = ip6_hdr_get_src_in6_addr(ip6_hdr);
            tuple->dst_addr.v6 = ip6_hdr_get_dst_in6_addr(ip6_hdr);
            return 0;
        }
    }

    return -1;
}

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

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

        // first get L4 layer
        if (layer->proto == LAYER_PROTO_UDP)
        {
            const struct udphdr *udp_hdr = (const struct udphdr *)layer->hdr_ptr;
            tuple->src_port = udp_hdr->uh_sport;
            tuple->dst_port = udp_hdr->uh_dport;
            layer_l4 = layer;
            continue;
        }
        if (layer->proto == LAYER_PROTO_TCP)
        {
            const struct tcphdr *tcp_hdr = (const struct tcphdr *)layer->hdr_ptr;
            tuple->src_port = tcp_hdr->th_sport;
            tuple->dst_port = tcp_hdr->th_dport;
            layer_l4 = layer;
            continue;
        }

        // second get L3 layer
        if (layer->proto == LAYER_PROTO_IPV4)
        {
            const struct ip *ip4_hdr = (const struct ip *)layer->hdr_ptr;
            tuple->addr_family = AF_INET;
            tuple->src_addr.v4 = ip4_hdr_get_src_in_addr(ip4_hdr);
            tuple->dst_addr.v4 = ip4_hdr_get_dst_in_addr(ip4_hdr);
            layer_l3 = layer;
            break;
        }
        if (layer->proto == LAYER_PROTO_IPV6)
        {
            const struct ip6_hdr *ip6_hdr = (const struct ip6_hdr *)layer->hdr_ptr;
            tuple->addr_family = AF_INET6;
            tuple->src_addr.v6 = ip6_hdr_get_src_in6_addr(ip6_hdr);
            tuple->dst_addr.v6 = ip6_hdr_get_dst_in6_addr(ip6_hdr);
            layer_l3 = layer;
            break;
        }
    }

    if (layer_l3 && layer_l4 && layer_l4 - layer_l3 == 1)
    {
        return 0;
    }
    else
    {
        return -1;
    }
}

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

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

        // first get L3 layer
        if (layer->proto == LAYER_PROTO_IPV4)
        {
            const struct ip *ip4_hdr = (const struct ip *)layer->hdr_ptr;
            tuple->addr_family = AF_INET;
            tuple->src_addr.v4 = ip4_hdr_get_src_in_addr(ip4_hdr);
            tuple->dst_addr.v4 = ip4_hdr_get_dst_in_addr(ip4_hdr);
            layer_l3 = layer;
            continue;
        }
        if (layer->proto == LAYER_PROTO_IPV6)
        {
            const struct ip6_hdr *ip6_hdr = (const struct ip6_hdr *)layer->hdr_ptr;
            tuple->addr_family = AF_INET6;
            tuple->src_addr.v6 = ip6_hdr_get_src_in6_addr(ip6_hdr);
            tuple->dst_addr.v6 = ip6_hdr_get_dst_in6_addr(ip6_hdr);
            layer_l3 = layer;
            continue;
        }

        // second get L4 layer
        if (layer->proto == LAYER_PROTO_UDP)
        {
            const struct udphdr *udp_hdr = (const struct udphdr *)layer->hdr_ptr;
            tuple->src_port = udp_hdr->uh_sport;
            tuple->dst_port = udp_hdr->uh_dport;
            layer_l4 = layer;
            break;
        }
        if (layer->proto == LAYER_PROTO_TCP)
        {
            const struct tcphdr *tcp_hdr = (const struct tcphdr *)layer->hdr_ptr;
            tuple->src_port = tcp_hdr->th_sport;
            tuple->dst_port = tcp_hdr->th_dport;
            layer_l4 = layer;
            break;
        }
    }

    if (layer_l3 && layer_l4 && layer_l4 - layer_l3 == 1)
    {
        return 0;
    }
    else
    {
        return -1;
    }
}

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

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

        // first get L4 layer
        if (layer->proto == LAYER_PROTO_UDP)
        {
            const struct udphdr *udp_hdr = (const struct udphdr *)layer->hdr_ptr;
            tuple->ip_proto = IPPROTO_UDP;
            tuple->src_port = udp_hdr->uh_sport;
            tuple->dst_port = udp_hdr->uh_dport;
            layer_l4 = layer;
            continue;
        }
        if (layer->proto == LAYER_PROTO_TCP)
        {
            const struct tcphdr *tcp_hdr = (const struct tcphdr *)layer->hdr_ptr;
            tuple->ip_proto = IPPROTO_TCP;
            tuple->src_port = tcp_hdr->th_sport;
            tuple->dst_port = tcp_hdr->th_dport;
            layer_l4 = layer;
            continue;
        }

        // second get L3 layer
        if (layer->proto == LAYER_PROTO_IPV4)
        {
            const struct ip *ip4_hdr = (const struct ip *)layer->hdr_ptr;
            tuple->addr_family = AF_INET;
            tuple->src_addr.v4 = ip4_hdr_get_src_in_addr(ip4_hdr);
            tuple->dst_addr.v4 = ip4_hdr_get_dst_in_addr(ip4_hdr);
            layer_l3 = layer;
            break;
        }
        if (layer->proto == LAYER_PROTO_IPV6)
        {
            const struct ip6_hdr *ip6_hdr = (const struct ip6_hdr *)layer->hdr_ptr;
            tuple->addr_family = AF_INET6;
            tuple->src_addr.v6 = ip6_hdr_get_src_in6_addr(ip6_hdr);
            tuple->dst_addr.v6 = ip6_hdr_get_dst_in6_addr(ip6_hdr);
            layer_l3 = layer;
            break;
        }
    }

    if (layer_l3 && layer_l4 && layer_l4 - layer_l3 == 1)
    {
        tuple->domain = packet_get_domain(pkt);
        return 0;
    }
    else
    {
        return -1;
    }
}

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

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

        // first get L3 layer
        if (layer->proto == LAYER_PROTO_IPV4)
        {
            const struct ip *ip4_hdr = (const struct ip *)layer->hdr_ptr;
            tuple->addr_family = AF_INET;
            tuple->src_addr.v4 = ip4_hdr_get_src_in_addr(ip4_hdr);
            tuple->dst_addr.v4 = ip4_hdr_get_dst_in_addr(ip4_hdr);
            layer_l3 = layer;
            continue;
        }
        if (layer->proto == LAYER_PROTO_IPV6)
        {
            const struct ip6_hdr *ip6_hdr = (const struct ip6_hdr *)layer->hdr_ptr;
            tuple->addr_family = AF_INET6;
            tuple->src_addr.v6 = ip6_hdr_get_src_in6_addr(ip6_hdr);
            tuple->dst_addr.v6 = ip6_hdr_get_dst_in6_addr(ip6_hdr);
            layer_l3 = layer;
            continue;
        }

        // second get L4 layer
        if (layer->proto == LAYER_PROTO_UDP)
        {
            const struct udphdr *udp_hdr = (const struct udphdr *)layer->hdr_ptr;
            tuple->ip_proto = IPPROTO_UDP;
            tuple->src_port = udp_hdr->uh_sport;
            tuple->dst_port = udp_hdr->uh_dport;
            layer_l4 = layer;
            break;
        }
        if (layer->proto == LAYER_PROTO_TCP)
        {
            const struct tcphdr *tcp_hdr = (const struct tcphdr *)layer->hdr_ptr;
            tuple->ip_proto = IPPROTO_TCP;
            tuple->src_port = tcp_hdr->th_sport;
            tuple->dst_port = tcp_hdr->th_dport;
            layer_l4 = layer;
            break;
        }
    }

    if (layer_l3 && layer_l4 && layer_l4 - layer_l3 == 1)
    {
        tuple->domain = packet_get_domain(pkt);
        return 0;
    }
    else
    {
        return -1;
    }
}

/******************************************************************************
 * other uitls
 ******************************************************************************/

const char *packet_get_raw_data(const struct packet *pkt)
{
    return pkt->data_ptr;
}

uint16_t packet_get_raw_len(const struct packet *pkt)
{
    return pkt->data_len;
}

const char *packet_get_payload(const struct packet *pkt)
{
    if (pkt == NULL || pkt->layers_used == 0)
    {
        return NULL;
    }

    if (pkt->layers[pkt->layers_used - 1].pld_len)
    {
        return pkt->layers[pkt->layers_used - 1].pld_ptr;
    }
    else
    {
        return NULL;
    }
}

uint16_t packet_get_payload_len(const struct packet *pkt)
{
    if (pkt==NULL || pkt->layers_used == 0)
    {
        return 0;
    }
    return pkt->layers[pkt->layers_used - 1].pld_len;
}

struct packet *packet_new(uint16_t pkt_len)
{
    struct packet *pkt = (struct packet *)malloc( sizeof(struct packet) + pkt_len);
    if (pkt == NULL)
    {
        return NULL;
    }
    pkt->data_len = pkt_len;
    pkt->data_ptr = (const char *)pkt + sizeof(struct packet);
    pkt->need_free = 1;

    return pkt;
}

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

    struct packet *dup_pkt = packet_new(pkt->data_len);
    if (dup_pkt == NULL)
    {
        return NULL;
    }

    dup_pkt->layers_used = pkt->layers_used;
    dup_pkt->layers_size = pkt->layers_size;

    memcpy((char *)dup_pkt->data_ptr, pkt->data_ptr, pkt->data_len);
    dup_pkt->data_len = pkt->data_len;
    packet_set_action(dup_pkt, PACKET_ACTION_DROP);

    for (int8_t i = 0; i < pkt->layers_used; i++)
    {
        dup_pkt->layers[i].proto = pkt->layers[i].proto;
        dup_pkt->layers[i].hdr_ptr = dup_pkt->data_ptr + pkt->layers[i].hdr_offset;
        dup_pkt->layers[i].pld_ptr = dup_pkt->data_ptr + pkt->layers[i].hdr_offset + pkt->layers[i].hdr_len;
        dup_pkt->layers[i].hdr_offset = pkt->layers[i].hdr_offset;
        dup_pkt->layers[i].hdr_len = pkt->layers[i].hdr_len;
        dup_pkt->layers[i].pld_len = pkt->layers[i].pld_len;
    }

    // update frag_layer
    if (pkt->frag_layer)
    {
        dup_pkt->frag_layer = &dup_pkt->layers[pkt->frag_layer - pkt->layers];
    }

    return dup_pkt;
}

void packet_free(struct packet *pkt)
{
    if (pkt && pkt->need_free)
    {
        free((void *)pkt);
    }
}

int packet_is_fragment(const struct packet *pkt)
{
    return (pkt->frag_layer) ? 1 : 0;
}

void layer_convert(const struct raw_layer *in, struct layer *out)
{
    if (in == NULL || out == NULL)
    {
        return;
    }

    out->proto = in->proto;
    out->hdr_len = in->hdr_len;
    out->hdr.raw = (char *)in->hdr_ptr;
}

void packet_set_user_data(struct packet *pkt, void *data)
{
    pkt->user_data=data;
}

void *packet_get_user_data(struct packet *pkt)
{
    return pkt->user_data;
}