stellar-stellar/infra/packet_manager/packet_dump.c

#include <sys/time.h>

#include "packet_parser.h"
#include "packet_helper.h"
#include "packet_internal.h"

struct pcap_pkt_hdr
{
    unsigned int tv_sec;  // time stamp
    unsigned int tv_usec; // time stamp
    unsigned int caplen;  // length of portion present
    unsigned int len;     // length this packet (off wire)
};

struct pcap_file_hdr
{
    unsigned int magic;
    unsigned short version_major;
    unsigned short version_minor;
    unsigned int thiszone; // gmt to local correction
    unsigned int sigfigs;  // accuracy of timestamps
    unsigned int snaplen;  // max length saved portion of each pkt
    unsigned int linktype; // data link type (LINKTYPE_*)
};

int packet_dump_pcap(const struct packet *pkt, const char *file)
{
    const char *data = packet_get_raw_data(pkt);
    uint16_t len = packet_get_raw_len(pkt);

    struct pcap_pkt_hdr pkt_hdr = {};
    struct pcap_file_hdr file_hdr = {
        .magic = 0xA1B2C3D4,
        .version_major = 0x0002,
        .version_minor = 0x0004,
        .thiszone = 0,
        .sigfigs = 0,
        .snaplen = 0xFFFF,
        .linktype = 1};

    FILE *fp = fopen(file, "w+");
    if (fp == NULL)
    {
        return -1;
    }

    struct timeval ts = {};
    gettimeofday(&ts, NULL);

    pkt_hdr.tv_sec = ts.tv_sec;
    pkt_hdr.tv_usec = ts.tv_usec;
    pkt_hdr.caplen = len;
    pkt_hdr.len = len;

    fwrite(&file_hdr, sizeof(struct pcap_file_hdr), 1, fp);
    fwrite(&pkt_hdr, sizeof(struct pcap_pkt_hdr), 1, fp);
    fwrite(data, 1, len, fp);
    fflush(fp);
    fclose(fp);

    return 0;
}

void packet_dump_hex(const struct packet *pkt, int fd)
{
    uint16_t len = packet_get_raw_len(pkt);
    const char *data = packet_get_raw_data(pkt);

    hexdump_to_fd(fd, 0, data, len);
}

int packet_dump_str(const struct packet *pkt, char *buff, int size)
{
    if (pkt == NULL)
    {
        return 0;
    }

    int old = 0;
    int used = 0;
    memset(buff, 0, size);
    used += snprintf(buff + used, size - used, "packet: %p, data_ptr: %p, data_len: %u, trim_len: %u, layers_used: %d, layers_size: %d\n",
                     pkt, pkt->data_ptr, pkt->data_len, pkt->trim_len,
                     pkt->layers_used, pkt->layers_size);
    for (uint8_t i = 0; i < pkt->layers_used; i++)
    {
        const struct layer_internal *layer = &pkt->layers[i];
        used += snprintf(buff + used, size - used, "layer[%u]: %p, proto: %s, header: {offset: %u, ptr: %p, len: %u}, payload: {ptr: %p, len: %u}\n",
                         i, layer, layer_proto_to_str(layer->proto), layer->hdr_offset,
                         layer->hdr_ptr, layer->hdr_len, layer->pld_ptr, layer->pld_len);
        used += snprintf(buff + used, size - used, "          ");
        old = used;
        switch (layer->proto)
        {
        case LAYER_PROTO_ETHER:
            used += eth_hdr_to_str((const struct ethhdr *)layer->hdr_ptr, buff + used, size - used);
            break;
        case LAYER_PROTO_PWETH:
            break;
        case LAYER_PROTO_PPP:
            break;
        case LAYER_PROTO_L2TP:
            used += l2tp_hdr_to_str((const struct l2tp_hdr *)layer->hdr_ptr, buff + used, size - used);
            break;
        case LAYER_PROTO_VLAN:
            used += vlan_hdr_to_str((const struct vlan_hdr *)layer->hdr_ptr, buff + used, size - used);
            break;
        case LAYER_PROTO_PPPOE:
            break;
        case LAYER_PROTO_MPLS:
            used += mpls_label_to_str((const struct mpls_label *)layer->hdr_ptr, buff + used, size - used);
            break;
        case LAYER_PROTO_IPV4:
            used += ip4_hdr_to_str((const struct ip *)layer->hdr_ptr, buff + used, size - used);
            break;
        case LAYER_PROTO_IPV6:
            used += ip6_hdr_to_str((const struct ip6_hdr *)layer->hdr_ptr, buff + used, size - used);
            break;
        case LAYER_PROTO_IPAH:
            break;
        case LAYER_PROTO_GRE:
            used += gre_hdr_to_str(layer->hdr_ptr, layer->hdr_len, buff + used, size - used);
            break;
        case LAYER_PROTO_UDP:
            used += udp_hdr_to_str((const struct udphdr *)layer->hdr_ptr, buff + used, size - used);
            break;
        case LAYER_PROTO_TCP:
            used += tcp_hdr_to_str((const struct tcphdr *)layer->hdr_ptr, buff + used, size - used);
            break;
        case LAYER_PROTO_ICMP:
            break;
        case LAYER_PROTO_ICMP6:
            break;
        case LAYER_PROTO_VXLAN:
            used += vxlan_hdr_to_str((const struct vxlan_hdr *)layer->hdr_ptr, buff + used, size - used);
            break;
        case LAYER_PROTO_GTP_C:
        case LAYER_PROTO_GTP_U:
            used += gtp_hdr_to_str(layer->hdr_ptr, layer->hdr_len, buff + used, size - used);
            break;
        default:
            break;
        }
        if (old != used && i != pkt->layers_used - 1)
        {
            used += snprintf(buff + used, size - used, "\n");
        }
    }

    return used;
}

void packet_print(const struct packet *pkt)
{
    char buff[4096] = {};
    packet_dump_str(pkt, buff, sizeof(buff));
    printf("%s\n", buff);
}