310 lines
6.9 KiB
C
310 lines
6.9 KiB
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 "replaypcap.h"
|
|
|
|
struct pcapinfo {
|
|
int sock;
|
|
useconds_t last_time;
|
|
};
|
|
|
|
char *hexagram_arglist_replaypcap(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_replaypcap());
|
|
|
|
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;
|
|
uint8_t body[65535];
|
|
|
|
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 ((readlen = read(stream->fd, &body, header.caplen)) < 0) {
|
|
stream->error = HEXAGRAM_PCAPNG_ERROR_IO;
|
|
|
|
goto error_io;
|
|
} else {
|
|
remaining -= readlen;
|
|
total += readlen;
|
|
}
|
|
|
|
if (data->last_time) {
|
|
usleep((useconds_t)header.timestamp[1] - data->last_time);
|
|
}
|
|
|
|
data->last_time = (useconds_t)header.timestamp[1];
|
|
|
|
((struct can_frame *)body)->can_id = be32toh(((struct can_frame *)body)->can_id);
|
|
|
|
printf("Read packet %"PRIu32" bytes time hi %"PRIu32" lo %"PRIu32" id %8"PRIx32" payload %d\n",
|
|
header.caplen, header.timestamp[0], header.timestamp[1],
|
|
((struct can_frame *)&body)->can_id,
|
|
((struct can_frame *)&body)->can_dlc);
|
|
|
|
if (write(data->sock, body, 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;
|
|
}
|
|
|
|
printf("Read block type %08"PRIx32" len %zu\n",
|
|
type, len);
|
|
|
|
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_replaypcap(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;
|
|
}
|