hexagram/bin/pcapreplay.c

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdarg.h>
#include <inttypes.h>
#include <fcntl.h>
#include <unistd.h>
#include <endian.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <sys/ioctl.h>

#include <net/if.h>
#include <linux/can.h>
#include <linux/can/raw.h>

#include <hexagram/pcapng.h>

#include "pcapreplay.h"

struct pcapinfo {
    int sock;
    useconds_t last_time;
};

char *hexagram_arglist_pcapreplay(void) {
    return "canif [file.pcapng]";
}

static void usage(int argc, char **argv, const char *message, ...) {
    if (message) {
        va_list args;

        va_start(args, message);
        vfprintf(stderr, message, args);
        va_end(args);
    }

    fprintf(stderr, "usage: hexagram %s %s\n",
        argv[0], hexagram_arglist_pcapreplay());

    exit(1);
}

static ssize_t handle_option(hexagram_pcapng_stream *stream,
                             uint32_t type,
                             uint16_t code,
                             uint16_t len,
                             void *data) {
    ssize_t readlen;
    uint8_t buf[65535];

    if ((readlen = read(stream->fd, buf, (size_t)len)) < 0) {
        goto error_io;
    }

    if (type == HEXAGRAM_PCAPNG_BLOCK_IF) {
        if (code == HEXAGRAM_PCAPNG_OPTION_IF_TSRESOL) {
            stream->if_tsresol = buf[0];

            printf("Timestamp resolution is %d\n", buf[0]);
        }
    }

    stream->error = HEXAGRAM_PCAPNG_ERROR_OK;

    return readlen;

error_io:
    stream->error = HEXAGRAM_PCAPNG_ERROR_IO;

    return -1;
}

static ssize_t handle_block_if(hexagram_pcapng_stream *stream,
                               uint32_t type,
                               size_t len,
                               void *data) {
    hexagram_pcapng_if iface;

    ssize_t readlen,
            total = 0;

    size_t remaining = len;

    /*
     * Read the interface block header.
     */
    if ((readlen = read(stream->fd, &iface, sizeof(iface))) < 0) {
        stream->error = HEXAGRAM_PCAPNG_ERROR_IO;

        goto error_io;
    } else {
        remaining -= readlen;
        total     += readlen;
    }

    if ((readlen = hexagram_pcapng_read_options(stream, handle_option, type, remaining, data)) < 0) {
        stream->error = HEXAGRAM_PCAPNG_ERROR_IO;

        goto error_io;
    } else {
        total += readlen;
    }

    return total;

error_io:
    return -1;
}

static ssize_t handle_block_packet(hexagram_pcapng_stream *stream,
                                   uint32_t type,
                                   size_t len,
                                   struct pcapinfo *data) {
    hexagram_pcapng_packet header;
    struct can_frame frame;

    ssize_t readlen,
            total = 0;

    size_t remaining = len;

    /*
     * Read the packet block header so that we may determine the size of the
     * payload to continue to read.
     */
    if ((readlen = read(stream->fd, &header, sizeof(header))) < 0) {
        stream->error = HEXAGRAM_PCAPNG_ERROR_IO;

        goto error_io;
    } else {
        remaining -= readlen;
        total     += readlen;
    }

    /*
     * Read the packet body into our scratchpad.
     */
    if (header.caplen > sizeof(frame)) {
        if (lseek(stream->fd, header.caplen, SEEK_CUR) < 0) {
            stream->error = HEXAGRAM_PCAPNG_ERROR_IO;

            goto error_io;
        }
    } else if ((readlen = read(stream->fd, &frame, header.caplen)) < 0) {
        stream->error = HEXAGRAM_PCAPNG_ERROR_IO;

        goto error_io;
    }

    remaining -= readlen;
    total     += readlen;

    if (header.caplen <= sizeof(frame)) {
        if (data->last_time) {
            usleep((useconds_t)header.timestamp[1] - data->last_time);
        }

        data->last_time = (useconds_t)header.timestamp[1];

        frame.can_id = be32toh(frame.can_id);

        if (write(data->sock, &frame, sizeof(struct can_frame)) < 0) {
            goto error_io;
        }
    }

    if (header.caplen % sizeof(uint32_t)) {
        size_t padding = sizeof(uint32_t) - (header.caplen % sizeof(uint32_t));

        if (lseek(stream->fd, padding, SEEK_CUR) < 0) {
            stream->error = HEXAGRAM_PCAPNG_ERROR_IO;

            goto error_io;
        } else {
            remaining -= padding;
            total     += padding;
        }
    }

    /*
     * The remaining data here should be pcapng option values, and since we do
     * not presently require them, we can safely seek past them.
     */
    if (lseek(stream->fd, remaining, SEEK_CUR) < 0) {
        stream->error = HEXAGRAM_PCAPNG_ERROR_IO;

        goto error_io;
    } else {
        total += remaining;
    }

    stream->error = HEXAGRAM_PCAPNG_ERROR_OK;

    return total;

error_io:
    stream->error = HEXAGRAM_PCAPNG_ERROR_IO;

    return -1;
}

static ssize_t handle_block(hexagram_pcapng_stream *stream,
                            uint32_t type,
                            size_t len,
                            void *data) {
    switch (type) {
        case HEXAGRAM_PCAPNG_BLOCK_IF:
            return handle_block_if(stream, type, len, data);

        case HEXAGRAM_PCAPNG_BLOCK_PACKET:
            return handle_block_packet(stream, type, len, (struct pcapinfo *)data);

        default: break;
    }

    if (lseek(stream->fd, len, SEEK_CUR) < 0) {
        stream->error = HEXAGRAM_PCAPNG_ERROR_IO;

        goto error_io;
    }

    return len;

error_io:
    return -1;
}

int hexagram_main_pcapreplay(int argc, char **argv) {
    int fd;

    struct sockaddr_can addr;
    struct ifreq ifr;

    struct pcapinfo data = {
        .last_time = 0
    };

    hexagram_pcapng_stream *stream;

    if (argc == 2) {
        fd = fileno(stdin);
    } else if (argc == 3) {
        if ((fd = open(argv[2], O_RDONLY)) < 0) {
            perror("open()");

            goto error_open;
        }
    } else {
        usage(argc, argv, NULL);
        exit(1);
    }

    if ((data.sock = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
        perror("socket()");

        goto error_socket;
    }

    strcpy(ifr.ifr_name, argv[1]);

    ioctl(data.sock, SIOCGIFINDEX, &ifr);

    addr.can_family  = AF_CAN;
    addr.can_ifindex = ifr.ifr_ifindex;

    if (bind(data.sock, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
        perror("bind()");

        goto error_bind;
    }

    if ((stream = hexagram_pcapng_stream_open_fd(fd)) < 0) {
        goto error_pcapng_stream_open_fd;
    }

    if (hexagram_pcapng_stream_read(stream, handle_block, &data) < 0) {
        perror("hexagram_pcapng_stream_read()");

        goto error_pcapng_stream_read;
    }

    hexagram_pcapng_stream_destroy(stream);

    close(data.sock);

    if (argc == 2) {
        close(fd);
    }

    return 0;

error_pcapng_stream_read:
    hexagram_pcapng_stream_destroy(stream);

error_pcapng_stream_open_fd:
error_bind:
    close(data.sock);

error_socket:
    if (argc == 2) {
        close(fd);
    }

    close(fd);

error_open:
    return 1;
}