X-Git-Url: http://rtime.felk.cvut.cz/gitweb/can-benchmark.git/blobdiff_plain/3bd8177e9074df7ad74134a49072a4097026e38f..dc7f5a3374014b178414d75c09b9484403f186cc:/latester/latester.c

diff --git a/latester/latester.c b/latester/latester.c
index 60e106b..b396fd2 100644
--- a/latester/latester.c
+++ b/latester/latester.c
@@ -34,6 +34,7 @@
 #include "canframelen.h"
 #include <linux/can.h>
 #include <linux/can/raw.h>
+#include <linux/net_tstamp.h>
 
 #include "histogram.h"
 
@@ -93,6 +94,7 @@ int num_interfaces = 0;
 int count = 0;			/* Number of sent messages */
 unsigned msg_in_progress = 0;
 int completion_pipe[2];
+struct timespec can_tsdiff = {0, 0}; /* can_rx2_ts - can_rx1_ts */
 
 struct msg_info {
 	canid_t id;
@@ -305,7 +307,7 @@ static inline int sock_get_if_index(int s, const char *if_name)
 
 static inline void get_tstamp(struct timespec *ts)
 {
-	clock_gettime(CLOCK_REALTIME, ts);
+	clock_gettime(CLOCK_MONOTONIC/*REALTIME*/, ts);
 }
 
 
@@ -474,6 +476,7 @@ void receive(int s, struct can_frame *frame, struct timespec *ts_kern, struct ti
 	struct sockaddr_can addr;
 	int nbytes;
 	static uint64_t dropcnt = 0;
+	bool have_hwtstamp = 0;
 
 	iov.iov_base = frame;
 	msg.msg_name = &addr;
@@ -499,8 +502,15 @@ void receive(int s, struct can_frame *frame, struct timespec *ts_kern, struct ti
 	for (cmsg = CMSG_FIRSTHDR(&msg);
 	     cmsg && (cmsg->cmsg_level == SOL_SOCKET);
 	     cmsg = CMSG_NXTHDR(&msg,cmsg)) {
-		if (cmsg->cmsg_type == SO_TIMESTAMPNS)
-			memcpy(ts_kern, CMSG_DATA(cmsg), sizeof(struct timespec));
+		if (cmsg->cmsg_type == SO_TIMESTAMPNS) {
+			if(!have_hwtstamp)
+				memcpy(ts_kern, CMSG_DATA(cmsg), sizeof(struct timespec));
+		}
+		else if (cmsg->cmsg_type == SO_TIMESTAMPING) {
+			struct timespec *ts = (struct timespec *) CMSG_DATA(cmsg);
+			memcpy(ts_kern, &ts[2], sizeof(struct timespec));
+			have_hwtstamp = 1;
+		}
 		else if (cmsg->cmsg_type == SO_RXQ_OVFL) {
 			uint32_t ovfl;
 			memcpy(&ovfl, CMSG_DATA(cmsg), sizeof(ovfl));
@@ -548,6 +558,62 @@ void process_final_rx(int s)
 		error(1, errno, "completion_pipe write");
 }
 
+void determine_can_timing_offset(struct pollfd *pfd, int nifc)
+{
+	int uiofd;
+	volatile uint32_t *can_crossbar;
+	uint32_t oldreg;
+	struct can_frame frame;
+	struct timespec ts_rx1, ts_rx2, ts_dummy;
+	int res;
+
+	if(nifc < 2)
+		return;
+
+	uiofd = open("/dev/uio0", O_RDWR);
+	if(uiofd < 0)
+	{
+		perror("determine_can_timing_offset: /dev/uio0");
+		return;
+	}
+
+	can_crossbar = (volatile uint32_t *) mmap(NULL, 4096, PROT_READ | PROT_WRITE, MAP_SHARED, uiofd, 0);
+	if(can_crossbar == MAP_FAILED)
+	{
+		perror("mmap");
+		goto cleanup1;
+	}
+
+	// Bit: 20   19 18 17 16 14  13  12  11  10   9   8   7   6   5   4   3   2   1   0
+	//      STBY E3 E2 E1 E0 I3      I2      I1      I0      C3      C2      C1      C0
+	// C*: can* (outer can physical interface) connection line assignment (0-3)
+	// I*: ifc* (inner can network interface) connection line assignment (0-3)
+	// E*: can* output enable: 1 = connect to physical interface, 0 = disconnect from physical interface, conenect RX and TX together
+	// STBY: CAN Transceivers standby mode
+	oldreg = can_crossbar[0];
+	can_crossbar[0] = 0x00000000; // connect all interfaces together, disable output, disable standby
+
+	frame.can_id = 0x00;
+	frame.can_dlc = 4;
+	memcpy(frame.data, "SYNC", 4);
+	res = write(pfd[0].fd, &frame, sizeof(frame));
+	if(res != sizeof(frame))
+	{
+		perror("write");
+		goto cleanup2;
+	}
+
+	receive(pfd[1].fd, &frame, &ts_rx1, &ts_dummy);
+	receive(pfd[2].fd, &frame, &ts_rx2, &ts_dummy);
+	timespec_subtract(&can_tsdiff, &ts_rx2, &ts_rx1);
+
+cleanup2:
+	can_crossbar[0] = oldreg;
+	munmap((void*)can_crossbar, 4096);
+cleanup1:
+	close(uiofd);
+}
+
 void *measure_thread(void *arg)
 {
 	int s, i, ret;
@@ -577,10 +643,20 @@ void *measure_thread(void *arg)
 		if (bind(s, (struct sockaddr *)&addr, sizeof(addr)) < 0)
 			error(1, errno, "bind");
 
-		const int timestamp_on = 1;
-		if (setsockopt(s, SOL_SOCKET, SO_TIMESTAMPNS,
-			       &timestamp_on, sizeof(timestamp_on)) < 0)
-			error(1, errno, "setsockopt SO_TIMESTAMP");
+		// TODO: set HW TSTAMP flags?
+		const int so_timestamping_flags = SOF_TIMESTAMPING_RAW_HARDWARE;
+		if (setsockopt(s, SOL_SOCKET, SO_TIMESTAMPING,
+			   &so_timestamping_flags,
+			   sizeof(so_timestamping_flags)) < 0)
+		{
+			perror("setsockopt SO_TIMESTAMPING");
+			fprintf(stderr, "Falling back to SW timestamps.\n");
+
+			const int timestamp_on = 1;
+			if (setsockopt(s, SOL_SOCKET, SO_TIMESTAMPNS,
+					&timestamp_on, sizeof(timestamp_on)) < 0)
+				error(1, errno, "setsockopt SO_TIMESTAMPNS");
+		}
 
 		const int dropmonitor_on = 1;
 		if (setsockopt(s, SOL_SOCKET, SO_RXQ_OVFL,
@@ -594,6 +670,9 @@ void *measure_thread(void *arg)
 			pfd[i].events = POLLIN;
 	}
 
+	determine_can_timing_offset(pfd, num_interfaces);
+
+
 	set_sched_policy_and_prio(SCHED_FIFO, 40);
 
 #define SEND() send_and_check(pfd[0].fd)