From 293fab4d83decb6b881019ea15cf0a88628cb14d Mon Sep 17 00:00:00 2001 From: GZZ8 Date: Tue, 14 May 2024 11:00:59 +0200 Subject: [PATCH] RealCAN implementation Replay pre-recorded CAN Bus dumps respecting the original relative timestamps. Fix code style Conform to codebase style as per maintainer's suggestions. Refactor code logic according to suggestions for PR #521 --- canplayer.c | 97 ++++++++++++++++++++++++++++++++++++++++++++++++++--- 1 file changed, 93 insertions(+), 4 deletions(-) diff --git a/canplayer.c b/canplayer.c index dd22560b..c48481a0 100644 --- a/canplayer.c +++ b/canplayer.c @@ -48,6 +48,7 @@ #include #include #include +#include #include #include @@ -89,6 +90,12 @@ const int canfx_on = 1; extern int optind, opterr, optopt; +struct sleep { + struct timeval *sleep_vector; + size_t idx; + size_t size; +}; + static void print_usage(char *prg) { fprintf(stderr, "%s - replay a compact CAN frame logfile to CAN devices.\n", prg); @@ -117,6 +124,8 @@ static void print_usage(char *prg) "loopback of sent CAN frames)\n"); fprintf(stderr, " -v (verbose: print " "sent CAN frames)\n"); + fprintf(stderr, " -r (real-time: send " + "CAN frames in real-time)\n"); fprintf(stderr, " -h (show " "this help message)\n\n"); fprintf(stderr, "Interface assignment:\n"); @@ -254,6 +263,53 @@ static int add_assignment(char *mode, int socket, char *txname, return 0; } +static void load_gaps_from_file(FILE *fp, struct sleep *timestamps) +{ + char *line = NULL; + size_t len = 0; + ssize_t read; + struct timeval *current; + struct timeval init_offset; + init_offset.tv_sec = 0; + init_offset.tv_usec = 0; + + while ((read = getline(&line, &len, fp)) != -1) { + if (timestamps->idx == timestamps->size) { + timestamps->size *= 2; + timestamps->sleep_vector = realloc(timestamps->sleep_vector, timestamps->size * sizeof(*(timestamps->sleep_vector))); + if (!timestamps->sleep_vector) { + fprintf(stderr, "Failed to realloc the timestamps vector!\n"); + exit(1); + } + } + current = ×tamps->sleep_vector[timestamps->idx]; + sscanf(line, "(%lu.%lu)", &(current->tv_sec), &(current->tv_usec)); + + if (timestamps->idx == 0 && (current->tv_sec != 0 || current->tv_usec != 0)){ + init_offset.tv_sec = current->tv_sec; + init_offset.tv_usec = current->tv_usec; + } + + if (timestamps->idx > 0 && (init_offset.tv_sec > 0 || init_offset.tv_usec > 0)){ + current->tv_sec -= init_offset.tv_sec; + current->tv_usec -= init_offset.tv_usec; + } + + timestamps->idx++; + } + free(line); +} + +static void init_timestamps(struct sleep *timestamps, size_t init_size) +{ + timestamps->size = init_size; + timestamps->sleep_vector = calloc(init_size, sizeof(*(timestamps->sleep_vector))); + if (!timestamps->sleep_vector) { + fprintf(stderr, "Failed to create the timestamps vector!\n"); + exit(1); + } +} + int main(int argc, char **argv) { static char buf[BUFSZ], device[DEVSZ], afrbuf[AFRSZ]; @@ -280,8 +336,11 @@ int main(int argc, char **argv) int eof, txmtu, i, j; char *fret; unsigned long long sec, usec; + bool gap_from_file = false; + struct sleep timestamps; + struct timeval send_time, act_time, init_time; - while ((opt = getopt(argc, argv, "I:l:tin:g:s:xvh")) != -1) { + while ((opt = getopt(argc, argv, "I:l:tin:g:s:xvrh")) != -1) { switch (opt) { case 'I': infile = fopen(optarg, "r"); @@ -336,6 +395,17 @@ int main(int argc, char **argv) verbose++; break; + case 'r': + if (isatty(fileno(infile))) { + fprintf(stderr, "Specify an input file for option -r !\n"); + exit(EXIT_FAILURE); + } + gap_from_file = true; /* using time delta from file */ + init_timestamps(×tamps, 1); + load_gaps_from_file(infile, ×tamps); + timestamps.idx = 0; /*to avoid warning accessing idx variable*/ + break; + case 'h': print_usage(basename(argv[0])); exit(EXIT_SUCCESS); @@ -368,8 +438,10 @@ int main(int argc, char **argv) printf("interactive mode: press ENTER to process next CAN frame ...\n"); } - sleep_ts.tv_sec = gap / 1000; - sleep_ts.tv_nsec = (gap % 1000) * 1000000; + if (!gap_from_file) { + sleep_ts.tv_sec = gap / 1000; + sleep_ts.tv_nsec = (gap % 1000) * 1000000; + } /* open socket */ if ((s = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) { @@ -513,6 +585,23 @@ int main(int argc, char **argv) addr.can_family = AF_CAN; addr.can_ifindex = txidx; /* send via this interface */ + if (gap_from_file && timestamps.idx > 0) { + send_time = timestamps.sleep_vector[timestamps.idx]; + gettimeofday(&act_time, NULL); + timersub(&act_time, &init_time, &act_time); + + while (timercmp(&act_time, &send_time, <)){ + gettimeofday(&act_time, NULL); + timersub(&act_time, &init_time, &act_time); + } + } + if (gap_from_file && timestamps.idx == 0) { + gettimeofday(&init_time, NULL); + gettimeofday(&act_time, NULL); + timersub(&act_time, &init_time, &act_time); + } + timestamps.idx++; + if (sendto(s, &cu, txmtu, 0, (struct sockaddr *)&addr, sizeof(addr)) != txmtu) { perror("sendto"); return 1; @@ -570,7 +659,7 @@ int main(int argc, char **argv) } /* while frames_to_send ... */ - if (nanosleep(&sleep_ts, NULL)) + if (!gap_from_file && nanosleep(&sleep_ts, NULL)) return 1; delay_loops++; /* private statistics */