stellar-stellar/src/packet/packet_build.cpp

#include <time.h>

#include "log.h"
#include "checksum.h"
#include "tcp_utils.h"
#include "udp_utils.h"
#include "ip4_utils.h"
#include "ip6_utils.h"
#include "gtp_utils.h"
#include "packet_def.h"
#include "packet_utils.h"
#include "packet_layer.h"
#include "packet_parse.h"
#include "packet_build.h"

#define PACKET_BUILD_LOG_DEBUG(format, ...) LOG_DEBUG("packet build", format, ##__VA_ARGS__)
#define PACKET_BUILD_LOG_ERROR(format, ...) LOG_ERROR("packet build", format, ##__VA_ARGS__)

struct fingerprint
{
    // TODO
    uint16_t ip_id;
    uint8_t ip_ttl;
    uint16_t tcp_win;
};

static inline void calc_packet_fingerprint(struct fingerprint *finger)
{
#define RANGE(rand, start, end) (start + rand % (end - start + 1)) // [start, end]
    struct timespec time;
    clock_gettime(CLOCK_MONOTONIC, &time);
    uint64_t random = 0x013579ABCDEF ^ time.tv_nsec;
    finger->ip_id = (uint16_t)(RANGE(random, 32767, 65535));
    finger->ip_ttl = (uint8_t)(RANGE(random, 48, 120));
    finger->tcp_win = (uint16_t)(RANGE(random, 1000, 1460));
}

static void update_tcp_hdr(struct tcphdr *tcp, uint32_t seq, uint32_t ack, uint16_t win, uint8_t flags, uint16_t opts_len)
{
    tcp_hdr_set_seq(tcp, seq);
    tcp_hdr_set_ack(tcp, ack);
    tcp_hdr_set_hdr_len(tcp, sizeof(struct tcphdr) + opts_len);
    tcp_hdr_set_flags(tcp, flags);
    tcp_hdr_set_window(tcp, win);
    tcp_hdr_set_urg_ptr(tcp, 0);
    tcp_hdr_set_checksum(tcp, 0);
}

static void update_udp_hdr(struct udphdr *udp, int trim_len)
{
    uint16_t total = udp_hdr_get_total_len(udp);
    udp_hdr_set_total_len(udp, total - trim_len);
    udp_hdr_set_checksum(udp, 0);
}

static void update_ip4_hdr(struct ip *ip, uint16_t ipid, uint8_t ttl, int trim_len)
{
    int hdr_len = ip4_hdr_get_hdr_len(ip);
    uint16_t total = ip4_hdr_get_total_len(ip);
    ip4_hdr_set_total_len(ip, total - trim_len);
    ip4_hdr_set_ipid(ip, ipid);
    ip4_hdr_set_ttl(ip, ttl);
    ip->ip_sum = 0;
    ip->ip_sum = checksum((const char *)ip, hdr_len);
}

static void update_ip6_hdr(struct ip6_hdr *ip6, int trim_len)
{
    uint16_t len = ip6_hdr_get_payload_len(ip6);
    ip6_hdr_set_payload_len(ip6, len - trim_len);
}

static void update_gtp1_hdr(struct gtp1_hdr *gtp, int trim_len)
{
    uint16_t msg_len = gtp1_hdr_get_msg_len(gtp);
    gtp1_hdr_set_msg_len(gtp, msg_len - trim_len);
    if (gtp1_hdr_get_seq_flag(gtp) && gtp1_hdr_get_seq(gtp))
    {
        PACKET_BUILD_LOG_ERROR("imiated packets may be dropped by intermediate devices, the GTPv1 layer requires a sequence number");
    }
}

static void update_gtp2_hdr(struct gtp2_hdr *gtp, int trim_len)
{
    uint16_t msg_len = gtp2_hdr_get_msg_len(gtp);
    gtp2_hdr_set_msg_len(gtp, msg_len - trim_len);
    if (gtp2_hdr_get_seq(gtp))
    {
        PACKET_BUILD_LOG_ERROR("imiated packets may be dropped by intermediate devices, the GTPv2 layer requires a sequence number");
    }
}

static void update_packet_hdr(const struct packet *origin_pkt,
                              char *new_pkt_data, uint16_t new_pkt_len, int trim_len,
                              uint32_t tcp_seq, uint32_t tcp_ack, uint8_t tcp_flags, uint16_t tcp_opts_len)
{
    uint8_t gtp_version = 0;
    char *curr_hdr_ptr = NULL;
    struct tcphdr *tcp = NULL;
    struct udphdr *udp = NULL;
    struct ip *ip4 = NULL;
    struct ip6_hdr *ip6 = NULL;
    struct gtp1_hdr *gtp1 = NULL;
    struct gtp2_hdr *gtp2 = NULL;
    struct raw_layer *curr_layer = NULL;
    struct raw_layer *last_layer = NULL;
    struct fingerprint finger = {0};
    calc_packet_fingerprint(&finger);
    int count = packet_get_layer_count(origin_pkt);
    for (int i = count - 1; i >= 0; i--)
    {
        curr_layer = (struct raw_layer *)packet_get_raw_layer(origin_pkt, i);
        curr_hdr_ptr = new_pkt_data + curr_layer->hdr_offset;
        switch (curr_layer->proto)
        {
        case LAYER_PROTO_TCP:
            tcp = (struct tcphdr *)curr_hdr_ptr;
            update_tcp_hdr(tcp, tcp_seq, tcp_ack, finger.tcp_win, tcp_flags, tcp_opts_len);
            break;
        case LAYER_PROTO_UDP:
            udp = (struct udphdr *)curr_hdr_ptr;
            update_udp_hdr(udp, trim_len);
            break;
        case LAYER_PROTO_IPV4:
            ip4 = (struct ip *)curr_hdr_ptr;
            last_layer = (struct raw_layer *)packet_get_raw_layer(origin_pkt, i + 1);
            if (last_layer->proto == LAYER_PROTO_TCP)
            {
                tcp = (struct tcphdr *)(new_pkt_data + last_layer->hdr_offset);
                tcp->th_sum = checksum_v4(tcp, new_pkt_len - last_layer->hdr_offset, IPPROTO_TCP, &ip4->ip_src, &ip4->ip_dst);
            }
            if (last_layer->proto == LAYER_PROTO_UDP)
            {
                udp = (struct udphdr *)(new_pkt_data + last_layer->hdr_offset);
                udp->uh_sum = checksum_v4(udp, new_pkt_len - last_layer->hdr_offset, IPPROTO_UDP, &ip4->ip_src, &ip4->ip_dst);
            }
            update_ip4_hdr(ip4, finger.ip_id, finger.ip_ttl, trim_len);
            break;
        case LAYER_PROTO_IPV6:
            ip6 = (struct ip6_hdr *)curr_hdr_ptr;
            last_layer = (struct raw_layer *)packet_get_raw_layer(origin_pkt, i + 1);
            if (last_layer->proto == LAYER_PROTO_TCP)
            {
                tcp = (struct tcphdr *)(new_pkt_data + last_layer->hdr_offset);
                tcp->th_sum = checksum_v6(tcp, new_pkt_len - last_layer->hdr_offset, IPPROTO_TCP, &ip6->ip6_src, &ip6->ip6_dst);
            }
            if (last_layer->proto == LAYER_PROTO_UDP)
            {
                udp = (struct udphdr *)(new_pkt_data + last_layer->hdr_offset);
                udp->uh_sum = checksum_v6(udp, new_pkt_len - last_layer->hdr_offset, IPPROTO_UDP, &ip6->ip6_src, &ip6->ip6_dst);
            }
            update_ip6_hdr(ip6, trim_len);
            break;
        case LAYER_PROTO_GTP_C: /* fall through */
        case LAYER_PROTO_GTP_U:
            gtp_version = peek_gtp_version(new_pkt_data + curr_layer->hdr_offset, curr_layer->hdr_len);
            if (gtp_version == 1)
            {
                gtp1 = (struct gtp1_hdr *)curr_hdr_ptr;
                update_gtp1_hdr(gtp1, trim_len);
            }
            if (gtp_version == 2)
            {
                gtp2 = (struct gtp2_hdr *)curr_hdr_ptr;
                update_gtp2_hdr(gtp2, trim_len);
            }
            break;
        case LAYER_PROTO_GRE:
            // TODO
            break;
        default:
            break;
        }
    }
}

/*
 * tcp_seq: the sequence number of the new TCP packet (in host byte order)
 * tcp_ack: the acknowledgment number of the new TCP packet (in host byte order)
 * tcp_options_len: the length of the options (must be a multiple of 4)
 */
struct packet *imitate_tcp_packet(const struct packet *origin_pkt, uint32_t tcp_seq, uint32_t tcp_ack, uint8_t tcp_flags,
                                  const char *tcp_options, uint16_t tcp_options_len,
                                  const char *tcp_payload, uint16_t tcp_payload_len)
{
    // check arguments
    if (origin_pkt == NULL ||
        (tcp_options == NULL && tcp_options_len != 0) || (tcp_options != NULL && tcp_options_len == 0) ||
        (tcp_payload == NULL && tcp_payload_len != 0) || (tcp_payload != NULL && tcp_payload_len == 0) ||
        (tcp_options_len && tcp_options_len % 4 != 0))
    {
        PACKET_BUILD_LOG_ERROR("imitate TCP packet failed, invalid arguments");
        return NULL;
    }

    // check the innermost layer of the original packet
    int layers = packet_get_layer_count(origin_pkt);
    const struct raw_layer *tcp_layer = packet_get_raw_layer(origin_pkt, layers - 1);
    if (tcp_layer == NULL || tcp_layer->proto != LAYER_PROTO_TCP)
    {
        PACKET_BUILD_LOG_ERROR("imitate TCP packet failed, the innermost layer of the original packet is not TCP");
        return NULL;
    }

    // calculate the new packet length
    int trim_len = tcp_layer->hdr_len + tcp_layer->pld_len - tcp_options_len - tcp_payload_len - sizeof(struct tcphdr);
    uint16_t new_pkt_len = origin_pkt->data_len - origin_pkt->trim_len - trim_len;
    struct packet *new_pkt = packet_new(new_pkt_len);
    if (new_pkt == NULL)
    {
        PACKET_BUILD_LOG_ERROR("imitate TCP packet failed, no space to allocate new packet");
        return NULL;
    }

    // copy the data to the new packet
    char *new_pkt_data = (char *)packet_get_raw_data(new_pkt);
    memcpy(new_pkt_data, packet_get_raw_data(origin_pkt), tcp_layer->hdr_offset + sizeof(struct tcphdr));
    if (tcp_options_len)
    {
        memcpy(new_pkt_data + tcp_layer->hdr_offset + sizeof(struct tcphdr), tcp_options, tcp_options_len);
    }
    memcpy(new_pkt_data + tcp_layer->hdr_offset + sizeof(struct tcphdr) + tcp_options_len, tcp_payload, tcp_payload_len);

    // update the headers of the new packet
    update_packet_hdr(origin_pkt, new_pkt_data, new_pkt_len, trim_len, tcp_seq, tcp_ack, tcp_flags, tcp_options_len);

    packet_parse(new_pkt, new_pkt_data, new_pkt_len);
    memcpy(&new_pkt->meta, &origin_pkt->meta, sizeof(struct metadata));
    new_pkt->meta.origin_ctx = NULL;

    return new_pkt;
}

struct packet *imitate_udp_packet(const struct packet *origin_pkt, const char *udp_payload, uint16_t udp_payload_len)
{
    // check arguments
    if (origin_pkt == NULL || (udp_payload == NULL && udp_payload_len != 0) || (udp_payload != NULL && udp_payload_len == 0))
    {
        PACKET_BUILD_LOG_ERROR("imitate UDP packet failed, invalid arguments");
        return NULL;
    }

    // check the innermost layer of the original packet
    int layers = packet_get_layer_count(origin_pkt);
    const struct raw_layer *udp_layer = packet_get_raw_layer(origin_pkt, layers - 1);
    if (udp_layer == NULL || udp_layer->proto != LAYER_PROTO_UDP)
    {
        PACKET_BUILD_LOG_ERROR("imitate UDP packet failed, the innermost layer of the original packet is not UDP");
        return NULL;
    }

    // calculate the new packet length
    int trim_len = udp_layer->hdr_len + udp_layer->pld_len - udp_payload_len - sizeof(struct udphdr);
    uint16_t new_pkt_len = origin_pkt->data_len - origin_pkt->trim_len - trim_len;
    struct packet *new_pkt = packet_new(new_pkt_len);
    if (new_pkt == NULL)
    {
        PACKET_BUILD_LOG_ERROR("imitate UDP packet failed, no space to allocate new packet");
        return NULL;
    }

    // copy the data to the new packet
    char *new_pkt_data = (char *)packet_get_raw_data(new_pkt);
    memcpy(new_pkt_data, packet_get_raw_data(origin_pkt), udp_layer->hdr_offset + sizeof(struct udphdr));
    memcpy(new_pkt_data + udp_layer->hdr_offset + sizeof(struct udphdr), udp_payload, udp_payload_len);

    // update the headers of the new packet
    update_packet_hdr(origin_pkt, new_pkt_data, new_pkt_len, trim_len, 0, 0, 0, 0);

    packet_parse(new_pkt, new_pkt_data, new_pkt_len);
    memcpy(&new_pkt->meta, &origin_pkt->meta, sizeof(struct metadata));
    new_pkt->meta.origin_ctx = NULL;

    return new_pkt;
}

struct packet *craft_packet_from_scratch(const struct layer layers[], uint16_t layer_count, const char *payload, uint16_t payload_len)
{
    // check arguments
    if (layers == NULL || layer_count == 0 || (payload == NULL && payload_len != 0) || (payload != NULL && payload_len == 0))
    {
        PACKET_BUILD_LOG_ERROR("craft packet from scratch failed, invalid arguments");
        return NULL;
    }

    // calculate the new packet length
    uint16_t new_pkt_len = 0;
    for (int i = 0; i < layer_count; i++)
    {
        if (layers[i].hdr.raw == NULL || layers[i].hdr_len == 0)
        {
            PACKET_BUILD_LOG_ERROR("craft packet from scratch failed, the header of layer %d is invalid", i);
            return NULL;
        }
        new_pkt_len += layers[i].hdr_len;
    }
    new_pkt_len += payload_len;
    struct packet *new_pkt = packet_new(new_pkt_len);
    if (new_pkt == NULL)
    {
        PACKET_BUILD_LOG_ERROR("craft packet from scratch failed, no space to allocate new packet");
        return NULL;
    }

    // copy the data to the new packet
    char *new_pkt_data = (char *)packet_get_raw_data(new_pkt);
    int offset = 0;
    for (int i = 0; i < layer_count; i++)
    {
        memcpy(new_pkt_data + offset, layers[i].hdr.raw, layers[i].hdr_len);
        offset += layers[i].hdr_len;
    }
    memcpy(new_pkt_data + offset, payload, payload_len);

    // update the headers of the new packet
    uint8_t gtp_version = 0;
    uint16_t curr_hdr_len = 0;
    char *curr_hdr_ptr = NULL;
    struct tcphdr *tcp = NULL;
    struct udphdr *udp = NULL;
    struct ip *ip4 = NULL;
    struct ip6_hdr *ip6 = NULL;
    struct gtp1_hdr *gtp1 = NULL;
    struct gtp2_hdr *gtp2 = NULL;
    // update checksums and lengths
    uint16_t curr_payload_len = payload_len;
    for (int i = layer_count - 1; i >= 0; i--)
    {
        curr_hdr_len = layers[i].hdr_len;
        curr_hdr_ptr = new_pkt_data + new_pkt_len - curr_hdr_len - curr_payload_len;
        switch (layers[i].proto)
        {
        case LAYER_PROTO_TCP:
            tcp = (struct tcphdr *)curr_hdr_ptr;
            // update the TCP header
            tcp_hdr_set_hdr_len(tcp, curr_hdr_len);
            tcp_hdr_set_checksum(tcp, 0);
            curr_payload_len += curr_hdr_len;
            break;
        case LAYER_PROTO_UDP:
            udp = (struct udphdr *)curr_hdr_ptr;
            // update the UDP header
            udp_hdr_set_total_len(udp, curr_hdr_len + curr_payload_len);
            udp_hdr_set_checksum(udp, 0);
            curr_payload_len += curr_hdr_len;
            break;
        case LAYER_PROTO_IPV4:
            ip4 = (struct ip *)curr_hdr_ptr;
            // update the checksums of the upper layer
            if (i + 1 < layer_count && layers[i + 1].proto == LAYER_PROTO_TCP)
            {
                tcp = (struct tcphdr *)(new_pkt_data + new_pkt_len - curr_payload_len);
                tcp->th_sum = checksum_v4(tcp, curr_payload_len, IPPROTO_TCP, &ip4->ip_src, &ip4->ip_dst);
            }
            if (i + 1 < layer_count && layers[i + 1].proto == LAYER_PROTO_UDP)
            {
                udp = (struct udphdr *)(new_pkt_data + new_pkt_len - curr_payload_len);
                udp->uh_sum = checksum_v4(udp, curr_payload_len, IPPROTO_UDP, &ip4->ip_src, &ip4->ip_dst);
            }
            // update the IPv4 header
            ip4_hdr_set_hdr_len(ip4, curr_hdr_len);
            ip4_hdr_set_total_len(ip4, curr_hdr_len + curr_payload_len);
            ip4->ip_sum = 0;
            ip4->ip_sum = checksum((const char *)ip4, curr_hdr_len);
            curr_payload_len += curr_hdr_len;
            break;
        case LAYER_PROTO_IPV6:
            ip6 = (struct ip6_hdr *)curr_hdr_ptr;
            // update the checksums of the upper layer
            if (i + 1 < layer_count && layers[i + 1].proto == LAYER_PROTO_TCP)
            {
                tcp = (struct tcphdr *)(new_pkt_data + new_pkt_len - curr_payload_len);
                tcp->th_sum = checksum_v6(tcp, curr_payload_len, IPPROTO_TCP, &ip6->ip6_src, &ip6->ip6_dst);
            }
            if (i + 1 < layer_count && layers[i + 1].proto == LAYER_PROTO_UDP)
            {
                udp = (struct udphdr *)(new_pkt_data + new_pkt_len - curr_payload_len);
                udp->uh_sum = checksum_v6(udp, curr_payload_len, IPPROTO_UDP, &ip6->ip6_src, &ip6->ip6_dst);
            }
            // update the IPv6 header
            ip6_hdr_set_payload_len(ip6, curr_hdr_len + curr_payload_len - sizeof(struct ip6_hdr));
            curr_payload_len += curr_hdr_len;
            break;
        case LAYER_PROTO_GTP_C: /* fall through */
        case LAYER_PROTO_GTP_U:
            gtp_version = peek_gtp_version(curr_hdr_ptr, curr_hdr_len);
            if (gtp_version == 1)
            {
                gtp1 = (struct gtp1_hdr *)curr_hdr_ptr;
                // update the GTP header
                gtp1_hdr_set_msg_len(gtp1, curr_hdr_len + curr_payload_len - sizeof(struct gtp1_hdr));
            }
            if (gtp_version == 2)
            {
                gtp2 = (struct gtp2_hdr *)curr_hdr_ptr;
                // update the GTP header
                gtp2_hdr_set_msg_len(gtp2, curr_hdr_len + curr_payload_len - 4);
            }
            curr_payload_len += curr_hdr_len;
            break;
        case LAYER_PROTO_GRE:
            // TODO
            curr_payload_len += curr_hdr_len;
            break;
        default:
            curr_payload_len += curr_hdr_len;
            break;
        }
    }

    packet_parse(new_pkt, new_pkt_data, new_pkt_len);
    // no metadata for the new packet from scratch
    new_pkt->meta.origin_ctx = NULL;

    return new_pkt;
}