hexagram/src/capture.c

#include <stdlib.h>
#include <string.h>
#include <inttypes.h>
#include <unistd.h>
#include <fcntl.h>
#include <sys/stat.h>
#include <sys/ioctl.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,
                              struct timeval *timestamp,
                              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 && len < sizeof(data)) {
        goto error_io;
    }

    timestamp->tv_sec  = data.sec;
    timestamp->tv_usec = data.usec;

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

    return len;

error_io:
    return -1;
}

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

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

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

        data.sec  = now.tv_sec;
        data.usec = now.tv_usec;
    } else {
        data.sec  = timestamp->tv_sec;
        data.usec = timestamp->tv_usec;
    }

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

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

error_gettimeofday:
    return -1;
}

int hexagram_capture_save(hexagram_capture *capture,
                          hexagram_can_if *can_if) {
    struct can_frame frame;

    while (hexagram_can_if_read(can_if, &frame) > 0) {
        struct timeval timestamp;

        if (ioctl(can_if->sock, SIOCGSTAMP, &timestamp) < 0) {
            goto error_io;
        }

        if (hexagram_capture_write(capture, &timestamp, &frame) < 0) {
            goto error_io;
        }
    }

    return 0;

error_io:
    return -1;
}

int hexagram_capture_replay(hexagram_capture *capture,
                            hexagram_can_if *can_if,
                            float speed) {
    struct timeval timestamp;

    uint64_t usec_last = 0,
             usec;

    struct can_frame frame;

    while (1) {
        ssize_t len = hexagram_capture_read(capture,
                                            &timestamp,
                                            &frame);

        if (len < 0) {
            goto error_io;
        } else if (len == 0) {
            break;
        }

        usec = timestamp.tv_sec * 1000000
             + timestamp.tv_usec;

        if (usec_last) {
            usleep(speed * (useconds_t)(usec - usec_last));
        }

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

        usec_last = usec;
    }

    return 0;

error_io:
    return -1;
}