From e478abc95b9f02afeeb4bb531cad4fd14952213a Mon Sep 17 00:00:00 2001
From: GZZ8 <giovanni.gambiglianizoccoli@unimore.it>
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 | 84 +++++++++++++++++++++++++++++++++++++++++++++++++----
 1 file changed, 79 insertions(+), 5 deletions(-)

diff --git a/canplayer.c b/canplayer.c
index eb9c569b..33d3ca77 100644
--- a/canplayer.c
+++ b/canplayer.c
@@ -48,6 +48,7 @@
 #include <string.h>
 #include <time.h>
 #include <unistd.h>
+#include <stdbool.h>
 
 #include <linux/can.h>
 #include <linux/can/raw.h>
@@ -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);
@@ -116,7 +123,9 @@ static void print_usage(char *prg)
 	fprintf(stderr, "         -x           (disable local "
 			"loopback of sent CAN frames)\n");
 	fprintf(stderr, "         -v           (verbose: print "
-			"sent CAN frames)\n\n");
+			"sent CAN frames)\n");
+	fprintf(stderr, "         -r           (real-time: send "
+			"CAN frames in real-time)\n\n");
 	fprintf(stderr, "Interface assignment:\n");
 	fprintf(stderr, " 0..n assignments like <write-if>=<log-if>\n\n");
 	fprintf(stderr, " e.g. vcan2=can0  (send frames received from can0 on "
@@ -252,6 +261,38 @@ static int add_assignment(char *mode, int socket, char *txname,
 	return 0;
 }
 
+void load_gaps_from_file(FILE *fp, struct sleep *timestamps)
+{
+	char *line = NULL;
+	size_t len = 0;
+	ssize_t read;
+
+	while ((read = getline(&line, &len, fp)) != -1) {
+		if (timestamps->idx == timestamps->size) {
+			timestamps->size++;
+			timestamps->sleep_vector = realloc(timestamps->sleep_vector, timestamps->size * sizeof(struct timeval));
+			if (!timestamps->sleep_vector) {
+				fprintf(stderr, "Failed to create the timestamps vector!\n");
+				exit(1);
+			}
+		}
+		sscanf(line, "(%lu.%lu)", &timestamps->sleep_vector[timestamps->idx].tv_sec, &timestamps->sleep_vector[timestamps->idx].tv_usec);
+		timestamps->idx++;
+	}
+	free(line);
+}
+
+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);
+	}
+	timestamps->idx = 0;
+}
+
 int main(int argc, char **argv)
 {
 	static char buf[BUFSZ], device[DEVSZ], afrbuf[AFRSZ];
@@ -278,8 +319,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:xv?")) != -1) {
+	while ((opt = getopt(argc, argv, "I:l:tin:g:s:xvr?")) != -1) {
 		switch (opt) {
 		case 'I':
 			infile = fopen(optarg, "r");
@@ -334,6 +378,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(&timestamps, 1);
+			load_gaps_from_file(infile, &timestamps);
+			timestamps.idx = 0;
+			break;
+
 		case '?':
 		default:
 			print_usage(basename(argv[0]));
@@ -362,8 +417,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) {
@@ -507,6 +564,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;
@@ -564,7 +638,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 */