From: Martin Jerabek <jerabma7@fel.cvut.cz>
Date: Mon, 30 May 2016 16:49:54 +0000 (+0200)
Subject: latester: support for hardware timestamps via SO_TIMESTAMPING, userspace stamps using... 
X-Git-Url: http://rtime.felk.cvut.cz/gitweb/can-benchmark.git/commitdiff_plain/d5e98779e6641341bb138c0c131196249a1bc42d

latester: support for hardware timestamps via SO_TIMESTAMPING, userspace stamps using CLOCK_MONOTONIC
---

diff --git a/latester/latester.c b/latester/latester.c
index 60e106b..a6f1453 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"
 
@@ -305,7 +306,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 +475,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 +501,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));
@@ -577,10 +586,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,