#include <stdbool.h>
#include <string.h>
#include <assert.h>
-#include <stdio.h> /* 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 <arpa/inet.h>
/**
* Functions and types for CRC checks.
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)
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;
// 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 */
}
projects / can-benchmark.git / blobdiff