X-Git-Url: http://rtime.felk.cvut.cz/gitweb/can-benchmark.git/blobdiff_plain/7765317e73f5c4e9b0e9f2f28a56091efe5e4930..0050e301dae44fac57f8b30a22f07ec870618c2d:/latester/canframelen.c diff --git a/latester/canframelen.c b/latester/canframelen.c index 5c9003e..0859d48 100644 --- a/latester/canframelen.c +++ b/latester/canframelen.c @@ -4,35 +4,7 @@ #include #include #include -#include /* FIXME: remove me */ - -static void sprint_canframe(char *buf , struct can_frame *cf, int sep) { - /* documentation see lib.h */ - - int i,offset; - int dlc = (cf->can_dlc > 8)? 8 : cf->can_dlc; - - if (cf->can_id & CAN_ERR_FLAG) { - sprintf(buf, "%08X#", cf->can_id & (CAN_ERR_MASK|CAN_ERR_FLAG)); - offset = 9; - } else if (cf->can_id & CAN_EFF_FLAG) { - sprintf(buf, "%08X#", cf->can_id & CAN_EFF_MASK); - offset = 9; - } else { - sprintf(buf, "%03X#", cf->can_id & CAN_SFF_MASK); - offset = 4; - } - - if (cf->can_id & CAN_RTR_FLAG) /* there are no ERR frames with RTR */ - sprintf(buf+offset, "R"); - else - for (i = 0; i < dlc; i++) { - sprintf(buf+offset, "%02X", cf->data[i]); - offset += 2; - if (sep && (i+1 < dlc)) - sprintf(buf+offset++, "."); - } -} +#include /** * Functions and types for CRC checks. @@ -133,26 +105,13 @@ crc_t calc_bitmap_crc(uint8_t *bitmap, unsigned start, unsigned end) return crc; } -void print_bitmap(const char *prefix, uint8_t *bitmap, unsigned start, unsigned end) +unsigned calc_frame_length(struct can_frame *frame) { - int i; - printf("%s", prefix); - for (i = start; i < end; i++) - printf("%c", (bitmap[i/8] << (i % 8)) & 0x80 ? '1' : '0'); - if (*prefix) - printf("\n"); -} - -unsigned calc_stuff_bits(struct can_frame *frame) { uint8_t bitmap[16]; unsigned start = 0, end; - char received[64]; - sprint_canframe(received, frame, true); - printf("Frame: %s\n", received); - + /* Prepare bitmap */ memset(bitmap, 0, sizeof(bitmap)); - if (frame->can_id & CAN_EFF_FLAG) { // bit 7 0 7 0 7 0 7 0 // bitmap[0-3] |.sBBBBBB BBBBBSIE EEEEEEEE EEEEEEEE| s = SOF, B = Base ID (11 bits), S = SRR, I = IDE, E = Extended ID (18 bits) @@ -168,7 +127,7 @@ unsigned calc_stuff_bits(struct can_frame *frame) { bitmap[4] = (frame->can_id & 0x1) << 7 | (!!(frame->can_id & CAN_RTR_FLAG)) << 6 | 0 << 4 | /* r1, r0 */ - frame->can_dlc & 0xf; + (frame->can_dlc & 0xf); memcpy(&bitmap[5], &frame->data, frame->can_dlc); start = 1; end = 40 + 8*frame->can_dlc; @@ -179,74 +138,43 @@ unsigned calc_stuff_bits(struct can_frame *frame) { // bitmap[8-11] |55555555 66666666 77777777 ........| Data bytes bitmap[0] = (frame->can_id & CAN_SFF_MASK) >> 9; bitmap[1] = (frame->can_id >> 1) & 0xff; - bitmap[2] = (frame->can_id << 7) & 0xff | + bitmap[2] = ((frame->can_id << 7) & 0xff) | (!!(frame->can_id & CAN_RTR_FLAG)) << 6 | 0 << 4 | /* IDE, r0 */ - frame->can_dlc & 0xf; + (frame->can_dlc & 0xf); memcpy(&bitmap[3], &frame->data, frame->can_dlc); start = 5; end = 24 + 8*frame->can_dlc; } + /* Calc and append CRC */ crc_t crc = calc_bitmap_crc(bitmap, start, end); uint16_t crc_be = htons(crc << 1); assert(end%8 == 0); memcpy(bitmap + end/8, &crc_be, 2); - print_bitmap("Frame: ", bitmap, start, end); end += 15; - print_bitmap("FrameCRC: ", bitmap, start, end); - 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) */ } - break; } - ones = !ones; } - //printf ("STUFFED %d BITS\n", stuffed); - return stuffed; -} - -unsigned calc_frame_length(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 */ + return end - start + stuffed + 3 + /* CRC del, ACK, ACK del */ 7; /* EOF */ }