Working version of bit stuffing calculator

diff --git a/latester/canframelen.c b/latester/canframelen.c
index 5c9003e8beec12db19d76b0458e3a1daec404cc0..ce4699b42cdc857d68b44b3f6f9f605c854c449d 100644 (file)
--- a/latester/canframelen.c
+++ b/latester/canframelen.c
@@ -196,37 +196,31 @@ unsigned calc_stuff_bits(struct can_frame *frame) {
        end += 15;
        print_bitmap("FrameCRC: ", bitmap, start, end);
 
-       uint8_t mask, ones = 0, basemask = 0xf8;
+       /* Count stuffed bits (bit stuffing is only employed for fields from SOF to CRC) */
+       printf("Stuffed:  ");
+       uint8_t mask = 0x1f, lookfor = 0;
        unsigned stuffed = 0;
+       const int8_t clz[32] = /* count of leading zeros in 5 bit numbers */
+               { 5, 4, 3, 3, 2, 2, 2, 2, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
        while (start < end) {
-               start++;        /* FIXME: This loop is currently broken!!! */
-               mask = basemask >> (start%32);
-               while (1) {
-                       ones = ones ? mask : 0;
-                       uint32_t chunk = (bitmap[start/32] & 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);
-                               }
+               unsigned bits = (bitmap[start/8] << 8 | bitmap[start/8+1]) >> (16 - 5 - start%8);
+               lookfor = lookfor ? 0 : mask; /* We alternate between looking for a series of zeros or ones */
+               unsigned change = (bits & mask) ^ lookfor; /* 1 indicates a change */
+               if (change) { /* No bit was stuffed here */
+                       print_bitmap("", bitmap, start, start+clz[change]);
+                       start += clz[change];
+                       mask = 0x1f; /* Next look for 5 same bits */
+               } else {
+                       start += (mask == 0x1f) ? 5 : 4;
+                       if (start <= end) {
+                               print_bitmap("", bitmap, start-((mask == 0x1f) ? 5 : 4), start);
+                               printf("X");
+                               stuffed++;
+                               mask = 0x1e; /* Next look for 4 bits (5th bit is the suffed one) */
                        }
-                       break;
                }
-               ones = !ones;
        }
-       //printf ("STUFFED %d BITS\n", stuffed);
+       printf ("\nstuffed %d bits\n", stuffed);
        return stuffed;
 }
 

diff --git a/latester/test.c b/latester/test.c
index 214565fed5d3f7d87d96c35ed67e477cd54e9268..ec3f0e47fe00d1fe03f2792e343a3f445a512554 100644 (file)
--- a/latester/test.c
+++ b/latester/test.c
@@ -3,8 +3,7 @@
 
 int main(int argc, char *argv[])
 {
-       struct can_frame cf = { .can_id = 0x0f111121 | CAN_EFF_FLAG, .can_dlc = 1, .data = { 0x22 } };
+       struct can_frame cf = { .can_id = 0x123, .can_dlc = 2, .data = { 0x00, 0xff } };
        printf("Length: %u\n", calc_frame_length(&cf));
-
        return 0;
 }