Change bisect-run script

[can-benchmark.git] / latester / latester.c

diff --git a/latester/latester.c b/latester/latester.c
index 60e106b564d8b9f636ab4c02d59899eee510527f..b396fd287408a6e4d9d2b262831691eeb7bd386e 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"
 
@@ -93,6 +94,7 @@ int num_interfaces = 0;
 int count = 0;                 /* Number of sent messages */
 unsigned msg_in_progress = 0;
 int completion_pipe[2];
+struct timespec can_tsdiff = {0, 0}; /* can_rx2_ts - can_rx1_ts */
 
 struct msg_info {
        canid_t id;
@@ -305,7 +307,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 +476,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 +502,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));
@@ -548,6 +558,62 @@ void process_final_rx(int s)
                error(1, errno, "completion_pipe write");
 }
 
+void determine_can_timing_offset(struct pollfd *pfd, int nifc)
+{
+       int uiofd;
+       volatile uint32_t *can_crossbar;
+       uint32_t oldreg;
+       struct can_frame frame;
+       struct timespec ts_rx1, ts_rx2, ts_dummy;
+       int res;
+
+       if(nifc < 2)
+               return;
+
+       uiofd = open("/dev/uio0", O_RDWR);
+       if(uiofd < 0)
+       {
+               perror("determine_can_timing_offset: /dev/uio0");
+               return;
+       }
+
+       can_crossbar = (volatile uint32_t *) mmap(NULL, 4096, PROT_READ | PROT_WRITE, MAP_SHARED, uiofd, 0);
+       if(can_crossbar == MAP_FAILED)
+       {
+               perror("mmap");
+               goto cleanup1;
+       }
+
+       // Bit: 20   19 18 17 16 14  13  12  11  10   9   8   7   6   5   4   3   2   1   0
+       //      STBY E3 E2 E1 E0 I3      I2      I1      I0      C3      C2      C1      C0
+       // C*: can* (outer can physical interface) connection line assignment (0-3)
+       // I*: ifc* (inner can network interface) connection line assignment (0-3)
+       // E*: can* output enable: 1 = connect to physical interface, 0 = disconnect from physical interface, conenect RX and TX together
+       // STBY: CAN Transceivers standby mode
+       oldreg = can_crossbar[0];
+       can_crossbar[0] = 0x00000000; // connect all interfaces together, disable output, disable standby
+
+       frame.can_id = 0x00;
+       frame.can_dlc = 4;
+       memcpy(frame.data, "SYNC", 4);
+       res = write(pfd[0].fd, &frame, sizeof(frame));
+       if(res != sizeof(frame))
+       {
+               perror("write");
+               goto cleanup2;
+       }
+
+       receive(pfd[1].fd, &frame, &ts_rx1, &ts_dummy);
+       receive(pfd[2].fd, &frame, &ts_rx2, &ts_dummy);
+       timespec_subtract(&can_tsdiff, &ts_rx2, &ts_rx1);
+
+cleanup2:
+       can_crossbar[0] = oldreg;
+       munmap((void*)can_crossbar, 4096);
+cleanup1:
+       close(uiofd);
+}
+
 void *measure_thread(void *arg)
 {
        int s, i, ret;
@@ -577,10 +643,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,
@@ -594,6 +670,9 @@ void *measure_thread(void *arg)
                        pfd[i].events = POLLIN;
        }
 
+       determine_can_timing_offset(pfd, num_interfaces);
+
+
        set_sched_policy_and_prio(SCHED_FIFO, 40);
 
 #define SEND() send_and_check(pfd[0].fd)