From fd2a01bc8a84835da407d8a1003b84028497ed5a Mon Sep 17 00:00:00 2001
From: Michal Sojka <sojkam1@fel.cvut.cz>
Date: Tue, 30 Nov 2010 16:40:34 +0100
Subject: [PATCH] latester can work with different msg lengths

+ other modifications
---
 latester/latester.c | 56 ++++++++++++++++++++++++++++++++-------------
 1 file changed, 40 insertions(+), 16 deletions(-)

diff --git a/latester/latester.c b/latester/latester.c
index 55234dd..c94837c 100644
--- a/latester/latester.c
+++ b/latester/latester.c
@@ -59,13 +59,14 @@ struct options {
 	unsigned oneattime;
 	FILE *file;
 	FILE *histogram;
+	int length;
 };
 
 struct options opt = {
 	.id = 10,
 	.period_us = 0,
 	.timeout_ms = 1000,
-
+	.length = 2,
 };
 
 int num_interfaces = 0;
@@ -78,6 +79,7 @@ struct msg_info {
 	struct timespec ts_sent, ts_sent_kern;
 	struct timespec ts_rx_onwire, ts_rx_onwire_kern;
 	struct timespec ts_rx_final, ts_rx_final_kern;
+	struct can_frame sent, received;
 };
 
 #define MAX_INFOS 10000
@@ -85,15 +87,27 @@ struct msg_info msg_infos[MAX_INFOS];
 
 struct histogram histogram;
 
+int can_frame_sprintf(char *buf, struct can_frame *frame)
+{
+	char datastr[17];
+	int i;
+	for (i=0; i<8; i++)
+		sprintf(&datastr[i*2], "%02x", frame->data[i]);
+	sprintf(buf, "id=0x%x, len=%d, data=%s",
+		frame->can_id, frame->can_dlc, datastr);
+}
+
 static inline struct msg_info *frame2info(struct can_frame *frame)
 {
 	uint16_t idx;
-	if (frame->can_dlc == 2) {
+	if (frame->can_dlc >= 2) {
 		memcpy(&idx, frame->data, sizeof(idx));
 		if (idx >= MAX_INFOS)
 			error(1, 0, "%s idx too high", __FUNCTION__);
-	} else
+	} else {
+		
 		error(1, 0, "%s error", __FUNCTION__);
+	}
 	return &msg_infos[idx];
 }
 
@@ -108,24 +122,28 @@ void msg_info_print(FILE *f, struct msg_info *mi)
 	struct timespec diff;
 	void *local = talloc_new (NULL);
 	static long num = 0;
+	char sent[64], received[64];
+
+	can_frame_sprintf(sent, &mi->sent);
+	can_frame_sprintf(received, &mi->received);
 
 #define S(ts) tstamp_str(local, &ts)
 #define DIFF(a, b) (timespec_subtract(&diff, &b, &a), S(diff))
 
 	if (num_interfaces == 2)
-		fprintf(f, "%ld: %s -> %s (%s) = %s (%s)\n",
-		       num, S(mi->ts_sent), S(mi->ts_rx_final_kern), S(mi->ts_rx_final),
+		fprintf(f, "%ld: %s [%s] -> %s (%s) [%s] = %s (%s)\n",
+			num, S(mi->ts_sent), sent, S(mi->ts_rx_final_kern), S(mi->ts_rx_final), received,
 		       DIFF(mi->ts_sent, mi->ts_rx_final_kern),
 		       DIFF(mi->ts_sent, mi->ts_rx_final));
 	else
-		fprintf(f, "%ld: %s -> %s (%s) -> %s (%s) = %s (%s), %s (%s)\n",
-		       num, S(mi->ts_sent),
-		       S(mi->ts_rx_onwire_kern), S(mi->ts_rx_onwire),
-		       S(mi->ts_rx_final_kern), S(mi->ts_rx_final),
-		       DIFF(mi->ts_sent, mi->ts_rx_onwire_kern),
-		       DIFF(mi->ts_sent, mi->ts_rx_onwire),
-		       DIFF(mi->ts_rx_onwire_kern, mi->ts_rx_final_kern),
-		       DIFF(mi->ts_rx_onwire, mi->ts_rx_final));
+		fprintf(f, "%ld: %s [%s] -> %s (%s) -> %s (%s) [%s] = %s (%s), %s (%s)\n",
+			num, S(mi->ts_sent), sent,
+			S(mi->ts_rx_onwire_kern), S(mi->ts_rx_onwire),
+			S(mi->ts_rx_final_kern), S(mi->ts_rx_final), received,
+			DIFF(mi->ts_sent, mi->ts_rx_onwire_kern),
+			DIFF(mi->ts_sent, mi->ts_rx_onwire),
+			DIFF(mi->ts_rx_onwire_kern, mi->ts_rx_final_kern),
+			DIFF(mi->ts_rx_onwire, mi->ts_rx_final));
 #undef S
 #undef DIFF
 	num++;
@@ -232,7 +250,7 @@ static inline int sock_get_if_index(int s, const char *if_name)
 
 static inline get_tstamp(struct timespec *ts)
 {
-	clock_gettime(CLOCK_REALTIME, ts);
+	clock_gettime(CLOCK_MONOTONIC, ts);
 }
 
 int send_frame(int socket)
@@ -243,7 +261,8 @@ int send_frame(int socket)
 	static int curr_msg = -1;
 	int i;
 	uint16_t idx;
-	
+
+	MEMSET_ZERO(frame);
 	i = curr_msg+1;
 	while (msg_infos[i].id != 0 && i != curr_msg) {
 		i++;
@@ -256,7 +275,9 @@ int send_frame(int socket)
 		curr_msg = i;
 
 	frame.can_id = opt.id;
-	frame.can_dlc = 2;
+	if (opt.length < 2)
+		error(1, 0, "Length < 2 is not yet supported");
+	frame.can_dlc = opt.length;
 	idx = curr_msg;
 	memcpy(frame.data, &idx, sizeof(idx));
 	mi = frame2info(&frame);
@@ -264,6 +285,7 @@ int send_frame(int socket)
 	mi->id = frame.can_id;
 	mi->length = frame.can_dlc;
 	get_tstamp(&mi->ts_sent);
+	mi->sent = frame;
 	ret = write(socket, &frame, sizeof(frame));
 	return ret;
 }
@@ -367,6 +389,7 @@ void process_final_rx(int s)
 	mi = frame2info(&frame);
 	mi->ts_rx_final_kern = ts_kern;
 	mi->ts_rx_final = ts_user;
+	mi->received = frame;
 
 	if (opt.histogram)
 		histogram_add(&histogram, get_msg_latency_us(mi));
@@ -504,6 +527,7 @@ struct poptOption optionsTable[] = {
 	{ "verbose",'v', POPT_ARG_NONE,			  	  NULL, 'v',   	     "Send the next message only when the previous was finally received"},
 	{ "file",   'f', POPT_ARG_STRING, 			  NULL, 'f',   	     "File where to store results", "filename"},
 	{ "histogram", 'h', POPT_ARG_STRING, 			  NULL, 'h',   	     "Store histogram in file", "filename"},
+	{ "length", 'l', POPT_ARG_INT|POPT_ARGFLAG_SHOW_DEFAULT,  &opt.length, 0,    "The length of generated messages", "bytes"},
 	POPT_AUTOHELP
 	{ NULL, 0, 0, NULL, 0 }
 };
-- 
2.39.2