From 66f4990a1eb121f1a6b60e9056589d87aa08a792 Mon Sep 17 00:00:00 2001 From: XANTRONIX Development Date: Tue, 12 Feb 2019 00:00:13 -0600 Subject: [PATCH] Well that flipping works. Good night! --- examples/Makefile | 2 +- examples/convert.c | 4 +- examples/pcapread.c | 2 +- examples/replay.c | 131 +++++++++++++++++++++++++++++++++++++ include/hexagram/capture.h | 16 +++-- src/capture.c | 41 +++++++----- 6 files changed, 169 insertions(+), 27 deletions(-) create mode 100644 examples/replay.c diff --git a/examples/Makefile b/examples/Makefile index 6894689..1cf04b5 100644 --- a/examples/Makefile +++ b/examples/Makefile @@ -7,7 +7,7 @@ INCLUDE_PATH = ../include CFLAGS += -I$(INCLUDE_PATH) LDFLAGS = -L../src -lhexagram -EXAMPLES = convert pcapread +EXAMPLES = convert pcapread replay RM = /bin/rm diff --git a/examples/convert.c b/examples/convert.c index f4e9797..25cf212 100644 --- a/examples/convert.c +++ b/examples/convert.c @@ -108,8 +108,8 @@ static ssize_t handle_block_packet(hexagram_pcapng_stream *stream, total += readlen; } - usec = (uint64_t)header.timestamp[0] << 32 - | (uint64_t)header.timestamp[1]; + usec = ((uint64_t)header.timestamp[0] << 32) + | (uint64_t)header.timestamp[1]; timestamp.tv_sec = usec / 1000000; timestamp.tv_usec = usec % 1000000; diff --git a/examples/pcapread.c b/examples/pcapread.c index 794978a..8ecd26e 100644 --- a/examples/pcapread.c +++ b/examples/pcapread.c @@ -151,7 +151,7 @@ static ssize_t handle_block_packet(hexagram_pcapng_stream *stream, ((struct can_frame *)&body)->can_id, ((struct can_frame *)&body)->can_dlc); - if (write(data->sock, (struct can_frame *)body, sizeof(struct can_frame)) < 0) { + if (write(data->sock, body, sizeof(struct can_frame)) < 0) { goto error_io; } diff --git a/examples/replay.c b/examples/replay.c new file mode 100644 index 0000000..62e4e07 --- /dev/null +++ b/examples/replay.c @@ -0,0 +1,131 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +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: %s canif [file.can]\n", argv[0]); + + exit(1); +} + +static int capture_replay(hexagram_capture *capture, int sock) { + ssize_t len; + uint32_t timestamp[2], + usec_last = 0; + struct can_frame frame; + + while (1) { + if ((len = hexagram_capture_read(capture, + ×tamp[0], + ×tamp[1], + &frame)) < 0) { + goto error_io; + } else if (len == 0) { + break; + } + + if (usec_last) { + usleep((useconds_t)(timestamp[1] - usec_last)); + } + + if (write(sock, &frame, sizeof(struct can_frame)) < 0) { + goto error_io; + } + + usec_last = timestamp[1]; + } + + return 0; + +error_io: + return -1; +} + +int main(int argc, char **argv) { + struct sockaddr_can addr; + struct ifreq ifr; + int sock; + + hexagram_capture *capture; + + if (argc == 2) { + if ((capture = hexagram_capture_open_fd(fileno(stdin), O_RDONLY)) == NULL) { + goto error_capture_open; + } + } else if (argc == 3) { + if ((capture = hexagram_capture_open_file(argv[2], O_RDONLY)) == NULL) { + perror("hexagram_capture_open_file()"); + + goto error_capture_open; + } + } else { + usage(argc, argv, NULL); + exit(1); + } + + if ((sock = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) { + perror("socket()"); + + goto error_socket; + } + + strcpy(ifr.ifr_name, argv[1]); + + ioctl(sock, SIOCGIFINDEX, &ifr); + + addr.can_family = AF_CAN; + addr.can_ifindex = ifr.ifr_ifindex; + + if (bind(sock, (struct sockaddr *)&addr, sizeof(addr)) < 0) { + perror("bind()"); + + goto error_bind; + } + + if (capture_replay(capture, sock) < 0) { + perror("capture_replay()"); + + goto error_capture_replay; + } + + close(sock); + + if (argc == 3) { + hexagram_capture_close(capture); + } + + return 0; + +error_capture_replay: +error_bind: + close(sock); + +error_socket: + if (argc == 3) { + hexagram_capture_close(capture); + } + +error_capture_open: + return 1; +} diff --git a/include/hexagram/capture.h b/include/hexagram/capture.h index 2c4da35..7bfb4db 100644 --- a/include/hexagram/capture.h +++ b/include/hexagram/capture.h @@ -29,13 +29,15 @@ hexagram_capture *hexagram_capture_open_file(const char *file, int flags); void hexagram_capture_destroy(hexagram_capture *capture); -int hexagram_capture_read(hexagram_capture *capture, - uint32_t *timestamp_hi, - uint32_t *timestamp_lo, - struct can_frame *frame); +void hexagram_capture_close(hexagram_capture *capture); -int hexagram_capture_write(hexagram_capture *capture, - struct can_frame *frame, - struct timeval *timestamp); +ssize_t hexagram_capture_read(hexagram_capture *capture, + uint32_t *timestamp_hi, + uint32_t *timestamp_lo, + struct can_frame *frame); + +ssize_t hexagram_capture_write(hexagram_capture *capture, + struct can_frame *frame, + struct timeval *timestamp); #endif /* _HEXAGRAM_CAPTURE_H */ diff --git a/src/capture.c b/src/capture.c index ccb3433..bc44095 100644 --- a/src/capture.c +++ b/src/capture.c @@ -1,5 +1,6 @@ #include #include +#include #include #include #include @@ -20,7 +21,7 @@ hexagram_capture *hexagram_capture_open_fd(int fd, int flags) { goto error_malloc; } - if (flags & O_WRONLY && !(flags & O_APPEND)) { + if ((flags & O_WRONLY) && !(flags & O_APPEND)) { memcpy(header.magic, HEXAGRAM_CAPTURE_MAGIC, strlen(HEXAGRAM_CAPTURE_MAGIC)); header.endian = HEXAGRAM_CAPTURE_ENDIAN; @@ -28,7 +29,7 @@ hexagram_capture *hexagram_capture_open_fd(int fd, int flags) { if (write(fd, &header, sizeof(header)) < 0) { goto error_io; } - } else if (flags & O_RDONLY) { + } else if (flags == O_RDONLY) { ssize_t readlen; if ((readlen = read(fd, &header, sizeof(header))) < 0) { @@ -80,10 +81,16 @@ void hexagram_capture_destroy(hexagram_capture *capture) { free(capture); } -int hexagram_capture_read(hexagram_capture *capture, - uint32_t *timestamp_hi, - uint32_t *timestamp_lo, - struct can_frame *frame) { +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; @@ -93,21 +100,22 @@ int hexagram_capture_read(hexagram_capture *capture, goto error_io; } - *timestamp_hi = data.timestamp_hi; - *timestamp_lo = data.timestamp_lo; + if (timestamp_hi) *timestamp_hi = data.timestamp_hi; + if (timestamp_lo) *timestamp_lo = data.timestamp_lo; memcpy(frame, &data.frame, sizeof(data.frame)); - return 0; + return len; error_io: return -1; } -int hexagram_capture_write(hexagram_capture *capture, - struct can_frame *frame, - struct timeval *timestamp) { +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; @@ -116,13 +124,14 @@ int hexagram_capture_write(hexagram_capture *capture, goto error_gettimeofday; } - data.timestamp_hi = now.tv_usec & 0xffffffff00000000 >> 32; - data.timestamp_lo = now.tv_usec & 0x00000000ffffffff; + usec = now.tv_usec + now.tv_sec * 1000000; } else { - data.timestamp_hi = timestamp->tv_usec & 0xffffffff00000000 >> 32; - data.timestamp_lo = timestamp->tv_usec & 0x00000000ffffffff; + 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));