candump: Enable HW timestamping before using it

[can-utils.git] / isotpdump.c

diff --git a/isotpdump.c b/isotpdump.c
index b2de8c23275d38831ae76ad6dfa58abd83069257..b2b650aa7996b48627a381d1702703f705c2f193 100644 (file)
--- a/isotpdump.c
+++ b/isotpdump.c
@@ -60,6 +60,7 @@
 #define NO_CAN_ID 0xFFFFFFFFU
 
 const char fc_info [4][9] = { "CTS", "WT", "OVFLW", "reserved" };
+const int canfd_on = 1;
 
 void print_usage(char *prg)
 {
@@ -80,7 +81,7 @@ int main(int argc, char **argv)
        int s;
        struct sockaddr_can addr;
        struct can_filter rfilter[2];
-       struct can_frame frame;
+       struct canfd_frame frame;
        int nbytes, i;
        canid_t src = NO_CAN_ID;
        canid_t dst = NO_CAN_ID;
@@ -94,8 +95,7 @@ int main(int argc, char **argv)
        int color = 0;
        int timestamp = 0;
        int datidx = 0;
-       struct ifreq ifr;
-       int ifindex;
+       unsigned long fflen = 0;
        struct timeval tv, last_tv;
        unsigned int n_pci;
        int opt;
@@ -179,6 +179,8 @@ int main(int argc, char **argv)
                return 1;
        }
 
+       /* try to switch the socket into CAN FD mode */
+       setsockopt(s, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &canfd_on, sizeof(canfd_on));
 
        if (src & CAN_EFF_FLAG) {
                rfilter[0].can_id   = src & (CAN_EFF_MASK | CAN_EFF_FLAG);
@@ -198,12 +200,8 @@ int main(int argc, char **argv)
 
        setsockopt(s, SOL_CAN_RAW, CAN_RAW_FILTER, &rfilter, sizeof(rfilter));
 
-       strcpy(ifr.ifr_name, argv[optind]);
-       ioctl(s, SIOCGIFINDEX, &ifr);
-       ifindex = ifr.ifr_ifindex;
-
        addr.can_family = AF_CAN;
-       addr.can_ifindex = ifindex;
+       addr.can_ifindex = if_nametoindex(argv[optind]);
 
        if (bind(s, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
                perror("bind");
@@ -211,12 +209,12 @@ int main(int argc, char **argv)
        }
 
        while (1) {
-
-               if ((nbytes = read(s, &frame, sizeof(struct can_frame))) < 0) {
+               nbytes = read(s, &frame, sizeof(frame));
+               if (nbytes < 0) {
                        perror("read");
                        return 1;
-               } else if (nbytes < sizeof(struct can_frame)) {
-                       fprintf(stderr, "read: incomplete CAN frame\n");
+               } else if (nbytes != CAN_MTU && nbytes != CANFD_MTU) {
+                       fprintf(stderr, "read: incomplete CAN frame %zu %d\n", sizeof(frame), nbytes);
                        return 1;
                } else {
 
@@ -283,21 +281,37 @@ int main(int argc, char **argv)
                        if (ext)
                                printf("{%02X}", frame.data[0]);
 
-                       printf("  [%d]  ", frame.can_dlc);
+                       if (nbytes == CAN_MTU)
+                               printf("  [%d]  ", frame.len);
+                       else
+                               printf(" [%02d]  ", frame.len);
 
                        datidx = 0;
                        n_pci = frame.data[ext];
            
                        switch (n_pci & 0xF0) {
                        case 0x00:
-                               printf("[SF] ln: %-4d data:", n_pci & 0x0F);
-                               datidx = ext+1;
+                               if (n_pci & 0xF) {
+                                       printf("[SF] ln: %-4d data:", n_pci & 0xF);
+                                       datidx = ext+1;
+                               } else {
+                                       printf("[SF] ln: %-4d data:", frame.data[ext + 1]);
+                                       datidx = ext+2;
+                               }
                                break;
 
                        case 0x10:
-                               printf("[FF] ln: %-4d data:",
-                                      ((n_pci & 0x0F)<<8) + frame.data[ext+1] );
-                               datidx = ext+2;
+                               fflen = ((n_pci & 0x0F)<<8) + frame.data[ext+1];
+                               if (fflen)
+                                       datidx = ext+2;
+                               else {
+                                       fflen = (frame.data[ext+2]<<24) +
+                                               (frame.data[ext+3]<<16) +
+                                               (frame.data[ext+4]<<8) +
+                                               frame.data[ext+5];
+                                       datidx = ext+6;
+                               }
+                               printf("[FF] ln: %-4lu data:", fflen);
                                break;
 
                        case 0x20:
@@ -332,16 +346,16 @@ int main(int argc, char **argv)
                                printf("[??]");
                        }
 
-                       if (datidx && frame.can_dlc > datidx) {
+                       if (datidx && frame.len > datidx) {
                                printf(" ");
-                               for (i = datidx; i < frame.can_dlc; i++) {
+                               for (i = datidx; i < frame.len; i++) {
                                        printf("%02X ", frame.data[i]);
                                }
 
                                if (asc) {
-                                       printf("%*s", ((7-ext) - (frame.can_dlc-datidx))*3 + 5 ,
+                                       printf("%*s", ((7-ext) - (frame.len-datidx))*3 + 5 ,
                                               "-  '");
-                                       for (i = datidx; i < frame.can_dlc; i++) {
+                                       for (i = datidx; i < frame.len; i++) {
                                                printf("%c",((frame.data[i] > 0x1F) &&
                                                             (frame.data[i] < 0x7F))?
                                                       frame.data[i] : '.');