candump: Enable HW timestamping before using it

[can-utils.git] / candump.c

diff --git a/candump.c b/candump.c
index eab949509adb4ef8414ee32291825d131b9bce80..c983b71186eaaef1e8b896b96443e4edf611e8b1 100644 (file)
--- a/candump.c
+++ b/candump.c
@@ -61,10 +61,15 @@
 
 #include <linux/can.h>
 #include <linux/can/raw.h>
+#include <linux/net_tstamp.h>
 
 #include "terminal.h"
 #include "lib.h"
 
+#ifndef SIOCSHWTSTAMP
+# define SIOCSHWTSTAMP 0x89b0
+#endif
+
 #define MAXSOCK 16    /* max. number of CAN interfaces given on the cmdline */
 #define MAXIFNAMES 30 /* size of receive name index to omit ioctls */
 #define MAXCOL 6      /* number of different colors for colorized output */
@@ -108,6 +113,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 +213,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 +249,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 +261,10 @@ int main(int argc, char **argv)
                        }
                        break;
 
+               case 'H':
+                       hw_timestamp = 1;
+                       break;
+
                case 'c':
                        color++;
                        break;
@@ -499,14 +510,16 @@ int main(int argc, char **argv)
                                setsockopt(s[i], SOL_CAN_RAW, CAN_RAW_ERR_FILTER,
                                           &err_mask, sizeof(err_mask));
 
+                       if (join_filter && setsockopt(s[i], SOL_CAN_RAW, CAN_RAW_JOIN_FILTERS,
+                                                     &join_filter, sizeof(join_filter)) < 0) {
+                               perror("setsockopt CAN_RAW_JOIN_FILTERS not supported by your Linux Kernel");
+                               return 1;
+                       }
+
                        if (numfilter)
                                setsockopt(s[i], SOL_CAN_RAW, CAN_RAW_FILTER,
                                           rfilter, numfilter * sizeof(struct can_filter));
 
-                       if (join_filter)
-                               setsockopt(s[i], SOL_CAN_RAW, CAN_RAW_JOIN_FILTERS,
-                                          &join_filter, sizeof(join_filter));
-
                        free(rfilter);
 
                } /* if (nptr) */
@@ -546,13 +559,36 @@ 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 {
+                               struct ifreq hwtstamp;
+                               struct hwtstamp_config hwconfig;
+
+                               memset(&hwtstamp, 0, sizeof(hwtstamp));
+                               strncpy(hwtstamp.ifr_name, ifr.ifr_name, sizeof(hwtstamp.ifr_name));
+                               hwtstamp.ifr_data = (void *)&hwconfig;
+                               memset(&hwconfig, 0, sizeof(hwconfig));
+                               hwconfig.tx_type = HWTSTAMP_TX_OFF;
+                               hwconfig.rx_filter = HWTSTAMP_FILTER_ALL;
+
+                               if (ioctl(s[i], SIOCSHWTSTAMP, &hwtstamp) < 0) {
+                                       perror("SIOCSHWTSTAMP");
+                                       return 1;
+                               }
 
-                       if (setsockopt(s[i], SOL_SOCKET, SO_TIMESTAMP,
-                                      &timestamp_on, sizeof(timestamp_on)) < 0) {
-                               perror("setsockopt SO_TIMESTAMP");
-                               return 1;
+                               const int so_timestamping_flags = SOF_TIMESTAMPING_RAW_HARDWARE;
+
+                               if (setsockopt(s[i], SOL_SOCKET, SO_TIMESTAMPING,
+                                              &so_timestamping_flags,
+                                              sizeof(so_timestamping_flags)) < 0)
+                                       perror("setsockopt SO_TIMESTAMPING");
                        }
                }
 
@@ -678,6 +714,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 */