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

diff --git a/latester/latester.c b/latester/latester.c
index 60e106b564d8b9f636ab4c02d59899eee510527f..a6f14538ea96cb9903be821e70c08b9b16c901f7 100644 (file)
--- 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,