Comment debug printf()s

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

diff --git a/latester/latester.c b/latester/latester.c
index 61b2b433f45afe5c20bba5bc5830ff82a99ca83a..f58086afba0e76f16e1b4288025aa8f897c7a6be 100644 (file)
--- a/latester/latester.c
+++ b/latester/latester.c
@@ -1,6 +1,6 @@
 /**************************************************************************/
 /* CAN latency tester                                                     */
-/* Copyright (C) 2010 Michal Sojka, DCE FEE CTU Prague                    */
+/* Copyright (C) 2010, 2011 Michal Sojka, DCE FEE CTU Prague                    */
 /* License: GPLv2                                                        */
 /**************************************************************************/
 
@@ -158,6 +158,89 @@ static inline char *tstamp_str(const void *ctx, struct timespec *tstamp)
                               tstamp->tv_sec, tstamp->tv_nsec/1000);
 }
 
+unsigned calc_stuff_bits(struct can_frame *frame) {
+       uint32_t bitmap[4];
+       unsigned start = 0, end;
+       uint32_t mask, ones = 0, basemask = 0xf0000000;
+       unsigned stuffed = 0;
+       memset(bitmap, 0, sizeof(bitmap));
+
+       if (frame->can_id & CAN_EFF_FLAG) {
+               bitmap[0] =
+                       (frame->can_id >> 25);
+               bitmap[1] =
+                       (frame->can_id >> 18) << 25             |
+                       3 << 23                                 |
+                       (frame->can_id & 0x3ffff) << 7          |
+                       (!!(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]);
+               start = 28;
+               end = 64 + 8*frame->can_dlc;
+       } else {
+               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]);
+               start = 14;
+               end = 32 + 8*frame->can_dlc;
+       }
+       /* TODO: Calc stuff bits in CRC */
+       while (start < end) {
+               mask = basemask >> (start & 0x1f);
+               while (1) {
+                       ones = ones ? mask : 0;
+                       uint32_t chunk = (bitmap[start >> 5] & mask) ^ ones;
+                       //printf("start=%d  bitmap=0x%08x  mask=0x%08x  ones=0x%08x  chunk=0x%08x\n", start, bitmap[start >> 5], mask, ones, chunk);
+                       if (chunk) {
+                               unsigned change = __builtin_clz(chunk);
+                               start = start & ~0x1f | change;
+                               basemask = 0xf8000000;
+                       } else {
+                               unsigned oldstart = start;
+                               start += __builtin_popcount(mask);
+                               mask = (oldstart & 0x1f) ? basemask << (-oldstart & 0x1f) : 0;
+                               //printf("oldstart=%d  shl=%d  mask=0x%08x\n", oldstart, -oldstart & 0x1f, mask);
+                               if (mask && start < end)
+                                       continue;
+                               if (start <= end && !mask) {
+                                       stuffed++;
+                                       basemask = 0xf0000000;
+                                       //printf("stuffed %d\n", !ones);
+                               }
+                       }
+                       break;
+               }
+               ones = !ones;
+       }
+       //printf ("STUFFED %d BITS\n", stuffed);
+}
+
+unsigned calc_frame_txtime(struct can_frame *frame) {
+       return calc_stuff_bits(frame) +
+               1 +             /* SOF */
+               11 +            /* ID A */
+               ((frame->can_id & CAN_EFF_FLAG) ?
+                1 +            /* SRR */
+                1 +            /* IDE */
+                18 +           /* ID B */
+                1 +            /* RTR */
+                2              /* r1, r0 */
+                :
+                1 +            /* rtr */
+                2) +           /* ide, r0 */
+               4 +             /* dlc */
+               8*frame->can_dlc +
+               15 +            /* CRC */
+               3 +             /* CRC del, ACK, ACK del */
+               7;              /* EOF */
+}
+
 void msg_info_print(FILE *f, struct msg_info *mi)
 {
        struct timespec diff;
@@ -173,20 +256,22 @@ void msg_info_print(FILE *f, struct msg_info *mi)
 
        switch (num_interfaces) {
        case 2:
-               fprintf(f, "%ld: %s %s -> %s (%s) %s = %s (%s)\n",
+               fprintf(f, "%ld: %s %s -> %s (%s) %s = %s (%s) %d\n",
                        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));
+                       DIFF(mi->ts_sent, mi->ts_rx_final_kern),
+                       DIFF(mi->ts_sent, mi->ts_rx_final),
+                       calc_frame_txtime(&mi->received));
                break;
        case 3:
-               fprintf(f, "%ld: %s %s -> %s (%s) -> %s (%s) %s = %s (%s), %s (%s)\n",
+               fprintf(f, "%ld: %s %s -> %s (%s) -> %s (%s) %s = %s (%s), %s (%s) %d\n",
                        num, S(mi->ts_sent), sent,
                        S(mi->ts_rx_onwire_kern), S(mi->ts_rx_onwire),
                        S(mi->ts_rx_final_kern), S(mi->ts_rx_final), received,
                        DIFF(mi->ts_sent, mi->ts_rx_onwire_kern),
                        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));
+                       DIFF(mi->ts_rx_onwire, mi->ts_rx_final),
+                       calc_frame_txtime(&mi->received));
                break;
        }
 #undef S