hexagram/src/capture.c

#include <stdlib.h>
#include <string.h>
#include <inttypes.h>
#include <unistd.h>
#include <fcntl.h>
#include <sys/stat.h>
#include <net/if.h>

#include <hexagram/capture.h>

struct _hexagram_capture {
    int fd;
    uint32_t endian;
};

hexagram_capture *hexagram_capture_open_fd(int fd, int flags) {
    hexagram_capture *capture;
    hexagram_capture_header header;

    if ((capture = malloc(sizeof(*capture))) == NULL) {
        goto error_malloc;
    }

    if ((flags & O_WRONLY) && !(flags & O_APPEND)) {
        memcpy(header.magic, HEXAGRAM_CAPTURE_MAGIC, strlen(HEXAGRAM_CAPTURE_MAGIC));

        header.endian = HEXAGRAM_CAPTURE_ENDIAN;

        if (write(fd, &header, sizeof(header)) < 0) {
            goto error_io;
        }
    } else if (flags == O_RDONLY) {
        ssize_t readlen;

        if ((readlen = read(fd, &header, sizeof(header))) < 0) {
            goto error_io;
        }

        if (memcmp(header.magic, HEXAGRAM_CAPTURE_MAGIC, sizeof(header.magic)) != 0) {
            goto error_invalid_format;
        }

        switch (header.endian) {
            case HEXAGRAM_CAPTURE_ENDIAN:
            case HEXAGRAM_CAPTURE_ENDIAN_SWAPPED:
                break;

            default:
                goto error_invalid_format;
        }
    }

    capture->fd     = fd;
    capture->endian = header.endian;

    return capture;

error_invalid_format:
error_io:
    free(capture);

error_malloc:
    return NULL;
}

hexagram_capture *hexagram_capture_open_file(const char *file, int flags) {
    int fd = (flags & O_CREAT)?
        open(file, flags, 0644):
        open(file, flags);

    if (fd < 0) {
        return NULL;
    }

    return hexagram_capture_open_fd(fd, flags);
}

void hexagram_capture_destroy(hexagram_capture *capture) {
    memset(capture, '\0', sizeof(*capture));

    free(capture);
}

void hexagram_capture_close(hexagram_capture *capture) {
    close(capture->fd);

    hexagram_capture_destroy(capture);
}

ssize_t hexagram_capture_read(hexagram_capture *capture,
                              uint32_t *timestamp_hi,
                              uint32_t *timestamp_lo,
                              struct can_frame *frame) {
    ssize_t len;
    hexagram_capture_frame data;

    if ((len = read(capture->fd, &data, sizeof(data))) < 0) {
        goto error_io;
    } else if (len < sizeof(data)) {
        goto error_io;
    }

    if (timestamp_hi) *timestamp_hi = data.timestamp_hi;
    if (timestamp_lo) *timestamp_lo = data.timestamp_lo;

    memcpy(frame, &data.frame, sizeof(data.frame));

    return len;

error_io:
    return -1;
}

ssize_t hexagram_capture_write(hexagram_capture *capture,
                               struct can_frame *frame,
                               struct timeval *timestamp) {
    hexagram_capture_frame data;
    uint64_t usec;

    if (timestamp == NULL) {
        struct timeval now;

        if (gettimeofday(&now, NULL) < 0) {
            goto error_gettimeofday;
        }

        usec = now.tv_usec + now.tv_sec * 1000000;
    } else {
        usec = timestamp->tv_usec + timestamp->tv_sec * 1000000;
    }

    data.timestamp_hi = usec & 0xffffffff00000000 >> 32;
    data.timestamp_lo = usec & 0x00000000ffffffff;

    memcpy(&data.frame, frame, sizeof(data.frame));

    return write(capture->fd, &data, sizeof(data));

error_gettimeofday:
    return -1;
}

int hexagram_capture_replay(hexagram_capture *capture,
                            hexagram_can_if *can_if,
                            float speed) {
    ssize_t len;

    uint32_t timestamp[2],
             usec_last = 0;

    struct can_frame frame;

    while (1) {
        if ((len = hexagram_capture_read(capture,
                                         &timestamp[0],
                                         &timestamp[1],
                                         &frame)) < 0) {
            goto error_io;
        } else if (len == 0) {
            break;
        }

        if (usec_last) {
            usleep(speed * (useconds_t)(timestamp[1] - usec_last));
        }

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

        usec_last = timestamp[1];
    }

    return 0;

error_io:
    return -1;
}