From d42b52780581fef80ae7161ce4e379b074392ec9 Mon Sep 17 00:00:00 2001
From: Michal Sojka <sojkam1@fel.cvut.cz>
Date: Fri, 10 Jan 2014 12:40:07 +0100
Subject: [PATCH] Working version of bit stuffing calculator

---
 latester/canframelen.c | 46 ++++++++++++++++++------------------------
 latester/test.c        |  3 +--
 2 files changed, 21 insertions(+), 28 deletions(-)

diff --git a/latester/canframelen.c b/latester/canframelen.c
index 5c9003e..ce4699b 100644
--- 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 214565f..ec3f0e4 100644
--- 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;
 }
-- 
2.39.2