From 51462af93db3196fbc11a580e312315d79145dda Mon Sep 17 00:00:00 2001
From: Michal Sojka <sojkam1@fel.cvut.cz>
Date: Fri, 26 Aug 2016 13:19:59 +0200
Subject: [PATCH] candump: Add support for hardware timestamping

Signed-off-by: Michal Sojka <sojkam1@fel.cvut.cz>
---
 candump.c | 33 +++++++++++++++++++++++++++------
 1 file changed, 27 insertions(+), 6 deletions(-)

diff --git a/candump.c b/candump.c
index 5daf622..bd9a3cd 100644
--- a/candump.c
+++ b/candump.c
@@ -61,6 +61,7 @@
 
 #include <linux/can.h>
 #include <linux/can/raw.h>
+#include <linux/net_tstamp.h>
 
 #include "terminal.h"
 #include "lib.h"
@@ -108,6 +109,7 @@ void print_usage(char *prg)
 	fprintf(stderr, "\nUsage: %s [options] <CAN interface>+\n", prg);
 	fprintf(stderr, "  (use CTRL-C to terminate %s)\n\n", prg);
 	fprintf(stderr, "Options: -t <type>   (timestamp: (a)bsolute/(d)elta/(z)ero/(A)bsolute w date)\n");
+	fprintf(stderr, "         -H          (use hardware timestamping)\n");
 	fprintf(stderr, "         -c          (increment color mode level)\n");
 	fprintf(stderr, "         -i          (binary output - may exceed 80 chars/line)\n");
 	fprintf(stderr, "         -a          (enable additional ASCII output)\n");
@@ -207,6 +209,7 @@ int main(int argc, char **argv)
 	int bridge = 0;
 	useconds_t bridge_delay = 0;
 	unsigned char timestamp = 0;
+	int hw_timestamp = 0;
 	unsigned char dropmonitor = 0;
 	unsigned char extra_msg_info = 0;
 	unsigned char silent = SILENT_INI;
@@ -242,7 +245,7 @@ int main(int argc, char **argv)
 	last_tv.tv_sec  = 0;
 	last_tv.tv_usec = 0;
 
-	while ((opt = getopt(argc, argv, "t:ciaSs:b:B:u:ldxLn:r:heT:?")) != -1) {
+	while ((opt = getopt(argc, argv, "t:ciaSs:b:B:u:ldxLn:r:heT:H?")) != -1) {
 		switch (opt) {
 		case 't':
 			timestamp = optarg[0];
@@ -254,6 +257,10 @@ int main(int argc, char **argv)
 			}
 			break;
 
+		case 'H':
+			hw_timestamp = 1;
+			break;
+
 		case 'c':
 			color++;
 			break;
@@ -548,13 +555,21 @@ int main(int argc, char **argv)
 		}
 
 		if (timestamp || log || logfrmt) {
+			if (hw_timestamp == 0) {
+				const int timestamp_on = 1;
 
-			const int timestamp_on = 1;
+				if (setsockopt(s[i], SOL_SOCKET, SO_TIMESTAMP,
+					       &timestamp_on, sizeof(timestamp_on)) < 0) {
+					perror("setsockopt SO_TIMESTAMP");
+					return 1;
+				}
+			} else {
+				const int so_timestamping_flags = SOF_TIMESTAMPING_RAW_HARDWARE;
 
-			if (setsockopt(s[i], SOL_SOCKET, SO_TIMESTAMP,
-				       &timestamp_on, sizeof(timestamp_on)) < 0) {
-				perror("setsockopt SO_TIMESTAMP");
-				return 1;
+				if (setsockopt(s[i], SOL_SOCKET, SO_TIMESTAMPING,
+					       &so_timestamping_flags,
+					       sizeof(so_timestamping_flags)) < 0)
+					perror("setsockopt SO_TIMESTAMPING");
 			}
 		}
 
@@ -680,6 +695,12 @@ int main(int argc, char **argv)
 						tv = *(struct timeval *)CMSG_DATA(cmsg);
 					else if (cmsg->cmsg_type == SO_RXQ_OVFL)
 						dropcnt[i] = *(__u32 *)CMSG_DATA(cmsg);
+					else if (cmsg->cmsg_type == SO_TIMESTAMPING) {
+						struct timespec *ts = (struct timespec *) CMSG_DATA(cmsg);
+						tv.tv_sec = ts[2].tv_sec;
+						tv.tv_usec = ts[2].tv_nsec / 1000;
+					}
+
 				}
 
 				/* check for (unlikely) dropped frames on this specific socket */
-- 
2.39.2