#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++, ".");
- }
-}
/**
* 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)
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);
- /* Count stuffed bits (bit stuffing is only employed for fields from SOF to CRC) */
- printf("Stuffed: ");
+ /* Count stuffed bits */
uint8_t mask = 0x1f, lookfor = 0;
- unsigned stuffed = 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 (start < end) {
- unsigned bits = (bitmap[start/8] << 8 | bitmap[start/8+1]) >> (16 - 5 - start%8);
+ 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 */
- print_bitmap("", bitmap, start, start+clz[change]);
- start += clz[change];
+ i += 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");
+ i += (mask == 0x1f) ? 5 : 4;
+ if (i <= end) {
stuffed++;
mask = 0x1e; /* Next look for 4 bits (5th bit is the suffed one) */
}
}
}
- printf ("\nstuffed %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 */
}