]> rtime.felk.cvut.cz Git - can-benchmark.git/blobdiff - latester/latester.c
Another bug
[can-benchmark.git] / latester / latester.c
index d09b1ab1a3027fedf670a657e4f6ee348306d10a..090676023da5b739e98d9445cafd7d7d1e5cd3c2 100644 (file)
@@ -1,6 +1,6 @@
 /**************************************************************************/
 /* CAN latency tester                                                     */
-/* Copyright (C) 2010, 2011 Michal Sojka, DCE FEE CTU Prague                    */
+/* Copyright (C) 2010, 2011, 2012, 2013 Michal Sojka, DCE FEE CTU Prague  */
 /* License: GPLv2                                                        */
 /**************************************************************************/
 
@@ -67,6 +67,7 @@ struct options {
        /* Temporary variables */
        FILE *f_msgs;
        FILE *f_hist;
+       FILE *f_hist_gw;
        FILE *f_stat;
 };
 
@@ -98,12 +99,13 @@ struct msg_info {
        struct timespec ts_rx_onwire, ts_rx_onwire_kern;
        struct timespec ts_rx_final, ts_rx_final_kern;
        struct can_frame sent, received;
+       unsigned lat_measured_us, tx_time_us;
 };
 
 #define MAX_INFOS 10000
 struct msg_info msg_infos[MAX_INFOS];
 
-struct histogram histogram;
+struct histogram histogram, histogram_gw;
 
 void sprint_canframe(char *buf , struct can_frame *cf, int sep) {
        /* documentation see lib.h */
@@ -195,16 +197,51 @@ crc_t crc_update(crc_t crc, uint32_t data, size_t bit_len)
     return crc;
 }
 
+uint32_t calc_bitmap_crc(uint32_t *bitmap, unsigned start, unsigned end)
+{
+    crc_t crc = 0;
+    crc = crc_update(crc, bitmap[0] << start, 32 - start);
+    crc = crc_update(crc, bitmap[1], 32);
+    crc = crc_update(crc, bitmap[2], end - 64 > 32 ? 32 : end - 64); /* FIXME: This seems to be broken for end < 64 */
+    crc = crc_update(crc, bitmap[3], end > 96 ? end - 96 : 0);
+    return (uint32_t)htons(crc << 17);
+}
+
+void write_crc_to_bitmap(uint32_t crc, uint32_t *bitmap, struct can_frame *frame)
+{
+    unsigned index = frame->can_id & CAN_EFF_FLAG ? 2 : 1;
+    if (frame->can_dlc < 4)
+        bitmap[index] |= crc >> (frame->can_dlc*8);
+    if (frame->can_dlc == 3)
+        bitmap[index + 1] |= crc << 8;
+    if (frame->can_dlc >= 4 && frame->can_dlc < 8)
+    {
+        bitmap[index + 1] |= crc >> (frame->can_dlc*8);
+        if (frame->can_dlc == 7)
+            bitmap[index + 2] = crc << 8;
+    }
+    else if (frame->can_dlc == 8)
+        bitmap[index + 2] = crc;
+}
+
 unsigned calc_stuff_bits(struct can_frame *frame) {
        uint32_t bitmap[4];
        unsigned start = 0, end;
        uint32_t mask, ones = 0, basemask = 0xf8000000;
        unsigned stuffed = 0;
        memset(bitmap, 0, sizeof(bitmap));
-       crc_t crc = 0;
-       uint32_t crcx;
+       uint32_t crc;
 
+/*     char received[64]; */
+/*     sprint_canframe(received, frame, true); */
+/*     printf("Frame: %s\n", received); */
+       
        if (frame->can_id & CAN_EFF_FLAG) {
+               // bit 31                              0
+               // [0] |...........................sBBBB| s = SOF, B = Base ID (11 bits)
+               // [1] |BBBBBBBSIEEEEEEEEEEEEEEEEEER10DLC4| S = SRR, I = IDE, E = Extended ID (18 bits), DLC4 = DLC
+               // [2] |00000000111111112222222233333333| Data bytes, MSB first
+               // [3] |44444444555555556666666677777777|
                bitmap[0] =
                        ((frame->can_id & CAN_EFF_MASK) >> 25);
                bitmap[1] =
@@ -214,33 +251,27 @@ unsigned calc_stuff_bits(struct can_frame *frame) {
                        (!!(frame->can_id & CAN_RTR_FLAG)) << 6         |
                        0 << 4                                          |
                        frame->can_dlc & 0xf;
-               bitmap[2] = htonl(((uint32_t*)frame->data)[0]);
-               bitmap[3] = htonl(((uint32_t*)frame->data)[1]);
+               memcpy(&bitmap[2], &frame->data, frame->can_dlc);
                start = 27;
                end = 64 + 8*frame->can_dlc;
-               crc = crc_update(crc, bitmap[0] << start, 32-start);
-               crc = crc_update(crc, bitmap[1], 32);
-               crc = crc_update(crc, bitmap[2], end - 64 > 32 ? 32 : end - 64);
-               crc = crc_update(crc, bitmap[3], end > 96 ? end - 96 : 0);
-               crcx = (uint32_t)htons(crc) << 17;
-               if (frame->can_dlc < 4)
-                       bitmap[2] |= crcx >> (frame->can_dlc*8);
-               if (frame->can_dlc = 3)
-                       bitmap[3] |= crcx << 8;
-               if (frame->can_dlc >= 4)
-                       bitmap[3] |= crcx >> (frame->can_dlc*8);
        } else {
+               // bit 31                              0
+               // [0] |.............sIIIIIIIIIIIRE0DLC4| s = SOF, I = ID (11 bits), R = RTR, E = IDE, DLC4 = DLC
+               // [1] |00000000111111112222222233333333| Data bytes, MSB first
+               // [2] |44444444555555556666666677777777|
                bitmap[0] =
                        (frame->can_id << 7) |
                        (!!(frame->can_id & CAN_RTR_FLAG)) << 6 |
                        0 << 4                                  |
                        frame->can_dlc & 0xf;
-               bitmap[1] = htonl(((uint32_t*)frame->data)[0]);
-               bitmap[2] = htonl(((uint32_t*)frame->data)[1]);
+               memcpy(&bitmap[1], &frame->data, frame->can_dlc);
                start = 13;
                end = 32 + 8*frame->can_dlc;
        }
-       /* TODO: Calc stuff bits in CRC */
+        crc = calc_bitmap_crc(bitmap, start, end);
+        write_crc_to_bitmap(crc, bitmap, frame);
+        end += 15;
+
        while (start < end) {
                mask = basemask >> (start & 0x1f);
                while (1) {
@@ -269,9 +300,10 @@ unsigned calc_stuff_bits(struct can_frame *frame) {
                ones = !ones;
        }
        //printf ("STUFFED %d BITS\n", stuffed);
+       return stuffed;
 }
 
-unsigned calc_frame_txtime(struct can_frame *frame) {
+unsigned calc_frame_txtime_us(struct can_frame *frame) {
        return calc_stuff_bits(frame) +
                1 +             /* SOF */
                11 +            /* ID A */
@@ -298,6 +330,9 @@ void msg_info_print(FILE *f, struct msg_info *mi)
        static long num = 0;
        char sent[64], received[64];
 
+       if (!f)
+               return;
+
        sprint_canframe(sent, &mi->sent, true);
        sprint_canframe(received, &mi->received, true);
 
@@ -310,7 +345,7 @@ void msg_info_print(FILE *f, struct msg_info *mi)
                        num, S(mi->ts_sent), sent, S(mi->ts_rx_final_kern), S(mi->ts_rx_final), received,
                        DIFF(mi->ts_sent, mi->ts_rx_final_kern),
                        DIFF(mi->ts_sent, mi->ts_rx_final),
-                       calc_frame_txtime(&mi->received));
+                       mi->tx_time_us);
                break;
        case 3:
                fprintf(f, "%ld: %s %s -> %s (%s) -> %s (%s) %s = %s (%s), %s (%s) %d\n",
@@ -321,7 +356,7 @@ void msg_info_print(FILE *f, struct msg_info *mi)
                        DIFF(mi->ts_sent, mi->ts_rx_onwire),
                        DIFF(mi->ts_rx_onwire_kern, mi->ts_rx_final_kern),
                        DIFF(mi->ts_rx_onwire, mi->ts_rx_final),
-                       calc_frame_txtime(&mi->received));
+                       mi->tx_time_us);
                break;
        }
 #undef S
@@ -363,7 +398,7 @@ void dbg_print_timespec(char *msg, struct timespec *tv)
        printf("%s sec=%ld nsec=%ld\n", msg, tv->tv_sec, tv->tv_nsec);
 }
 
-static inline unsigned get_msg_latency_us(struct msg_info *mi)
+static inline void calc_msg_latencies(struct msg_info *mi)
 {
        struct timespec diff;
        switch (num_interfaces) {
@@ -380,9 +415,10 @@ static inline unsigned get_msg_latency_us(struct msg_info *mi)
                        timespec_subtract(&diff, &mi->ts_rx_final_kern, &mi->ts_sent);
                break;
        default:
-               return 0;
+               return;
        }
-       return diff.tv_sec * 1000000 + diff.tv_nsec/1000;
+       mi->lat_measured_us = diff.tv_sec * 1000000 + diff.tv_nsec/1000;
+       mi->tx_time_us = calc_frame_txtime_us(&mi->received);
 }
 
 void set_sched_policy_and_prio(int policy, int rtprio)
@@ -646,7 +682,10 @@ void process_final_rx(int s)
        mi->ts_rx_final = ts_user;
        mi->received = frame;
 
-       histogram_add(&histogram, get_msg_latency_us(mi));
+       calc_msg_latencies(mi);
+
+       histogram_add(&histogram, mi->lat_measured_us);
+       histogram_add(&histogram_gw, mi->lat_measured_us - mi->tx_time_us);
 
        ret = write(completion_pipe[1], &mi, sizeof(mi));
        if (ret == -1)
@@ -829,12 +868,19 @@ int parse_options(int argc, const char *argv[])
        }
 
        if (opt.name) {
-               char *f = talloc_asprintf(local, "%s-hist.txt", opt.name);
+               char *f = talloc_asprintf(local, "%s-hist-raw.txt", opt.name);
                opt.f_hist = fopen(f, "w");
                if (!opt.f_hist)
                        error(1, errno, "fopen: %s", f);
        }
 
+       if (opt.name) {
+               char *f = talloc_asprintf(local, "%s-hist.txt", opt.name);
+               opt.f_hist_gw = fopen(f, "w");
+               if (!opt.f_hist_gw)
+                       error(1, errno, "fopen: %s", f);
+       }
+
        if (opt.name) {
                char *f = talloc_asprintf(local, "%s-stat.txt", opt.name);
                opt.f_stat = fopen(f, "w");
@@ -875,6 +921,7 @@ int main(int argc, const char *argv[])
                msg_infos[i].id = -1;
 
        histogram_init(&histogram, 5000000, 1);
+       histogram_init(&histogram_gw, 5000000, 1);
 
        ret = pipe(completion_pipe);
        if (ret == -1)
@@ -884,6 +931,13 @@ int main(int argc, const char *argv[])
                error(1, errno, "pipe fcntl");
 
        init_ftrace();
+       if (getenv("LATESTER_CONTROL_HACKBENCH")) {
+               char cmd[1000];
+               sprintf(cmd, "ssh -x -a -S $HOME/.ssh/cangw-connection root@192.168.2.3 'kill -CONT -%s'",
+                       getenv("LATESTER_CONTROL_HACKBENCH"));
+               printf("Running: %s\n", cmd);
+               system(cmd);
+       }
 
        pthread_create(&thread, 0, measure_thread, NULL);
 
@@ -933,40 +987,54 @@ int main(int argc, const char *argv[])
 
        pthread_join(thread, NULL);
 
+       if (getenv("LATESTER_CONTROL_HACKBENCH")) {
+               char cmd[1000];
+               sprintf(cmd, "ssh -x -a -S $HOME/.ssh/cangw-connection root@192.168.2.3 'kill -STOP -%s'",
+                       getenv("LATESTER_CONTROL_HACKBENCH"));
+               printf("Running: %s\n", cmd);
+               system(cmd);
+       }
+
        close(completion_pipe[0]);
        close(completion_pipe[1]);
 
        histogram_fprint(&histogram, opt.f_hist);
-       fclose(opt.f_hist);
-       fclose(opt.f_msgs);
-
-
-       fprintf(opt.f_stat, "cmdline='");
-       for (i=0; i<argc; i++)
-               fprintf(opt.f_stat, "%s%s", argv[i], i < argc-1 ? " " : "");
-       fprintf(opt.f_stat, "'\n");
-
-       timespec_subtract(&diff, &stats.tac, &stats.tic);
-       fprintf(opt.f_stat, "duration=%s # seconds\n", tstamp_str(NULL, &diff));
+       histogram_fprint(&histogram_gw, opt.f_hist_gw);
+       if (opt.f_hist)
+               fclose(opt.f_hist);
+       if (opt.f_hist_gw)
+               fclose(opt.f_hist_gw);
+       if (opt.f_msgs)
+               fclose(opt.f_msgs);
+
+       if (opt.f_stat) {
+               fprintf(opt.f_stat, "cmdline='");
+               for (i=0; i<argc; i++)
+                       fprintf(opt.f_stat, "%s%s", argv[i], i < argc-1 ? " " : "");
+               fprintf(opt.f_stat, "'\n");
+
+               timespec_subtract(&diff, &stats.tac, &stats.tic);
+               fprintf(opt.f_stat, "duration=%s # seconds\n", tstamp_str(NULL, &diff));
        
-       fprintf(opt.f_stat, "sent=%d\n", count);
-       fprintf(opt.f_stat, "overrun=%d\n", stats.overrun);
-       if (stats.overrun && !opt.quiet)
-               printf("overrun=%d\n", stats.overrun);
-       fprintf(opt.f_stat, "enobufs=%d\n", stats.enobufs);
-       if (stats.enobufs && !opt.quiet)
-               printf("enobufs=%d\n", stats.enobufs);
-       fprintf(opt.f_stat, "lost=%d\n", stats.lost);
-       if (stats.lost && !opt.quiet)
-               printf("lost=%d\n", stats.lost);
-       fprintf(opt.f_stat, "timeouts=%d\n", stats.timeouts);
-       if (stats.timeouts && !opt.quiet)
-               printf("timeouts=%d\n", stats.timeouts);
-       fprintf(opt.f_stat, "invalid_frame=%d\n", stats.timeouts);
-       if (stats.timeouts && !opt.quiet)
-               printf("invalid_frame=%d\n", stats.timeouts);
-
-       fclose(opt.f_stat);
+               fprintf(opt.f_stat, "sent=%d\n", count);
+               fprintf(opt.f_stat, "overrun=%d\n", stats.overrun);
+               if (stats.overrun && !opt.quiet)
+                       printf("overrun=%d\n", stats.overrun);
+               fprintf(opt.f_stat, "enobufs=%d\n", stats.enobufs);
+               if (stats.enobufs && !opt.quiet)
+                       printf("enobufs=%d\n", stats.enobufs);
+               fprintf(opt.f_stat, "lost=%d\n", stats.lost);
+               if (stats.lost && !opt.quiet)
+                       printf("lost=%d\n", stats.lost);
+               fprintf(opt.f_stat, "timeouts=%d\n", stats.timeouts);
+               if (stats.timeouts && !opt.quiet)
+                       printf("timeouts=%d\n", stats.timeouts);
+               fprintf(opt.f_stat, "invalid_frame=%d\n", stats.timeouts);
+               if (stats.timeouts && !opt.quiet)
+                       printf("invalid_frame=%d\n", stats.timeouts);
+
+               fclose(opt.f_stat);
+       }
 
        return 0;
 }