- uint8_t mask, ones = 0, basemask = 0xf8;
- unsigned stuffed = 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);
- }
+ /* Count stuffed bits */
+ uint8_t mask = 0x1f, lookfor = 0;
+ unsigned i = start, 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 (i < end) {
+ unsigned bits = (bitmap[i/8] << 8 | bitmap[i/8+1]) >> (16 - 5 - i%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 */
+ i += clz[change];
+ mask = 0x1f; /* Next look for 5 same bits */
+ } else {
+ i += (mask == 0x1f) ? 5 : 4;
+ if (i <= end) {
+ stuffed++;
+ mask = 0x1e; /* Next look for 4 bits (5th bit is the suffed one) */