tstamp->tv_sec, tstamp->tv_nsec/1000);
}
+/* Functions and types for CRC checks.
+ *
+ * Generated on Wed Sep 21 22:30:11 2011,
+ * by pycrc v0.7.8, http://www.tty1.net/pycrc/
+ * using the configuration:
+ * Width = 15
+ * Poly = 0x4599
+ * XorIn = 0x0000
+ * ReflectIn = False
+ * XorOut = 0x0000
+ * ReflectOut = False
+ * Algorithm = table-driven
+ *****************************************************************************/
+typedef uint16_t crc_t;
+
+static const crc_t crc_table[2] = {
+ 0x0000, 0x4599
+};
+
+crc_t crc_update(crc_t crc, uint32_t data, size_t bit_len)
+{
+ unsigned int tbl_idx;
+/* crc_t bc = crc; */
+/* uint32_t bd = data; */
+/* size_t bl = bit_len; */
+
+ while (bit_len--) {
+ tbl_idx = (crc >> 14) ^ (data >> 31);
+ crc = crc_table[tbl_idx & 0x01] ^ (crc << 1);
+ data <<= 1;
+ }
+ crc = crc & 0x7fff;
+/* printf("crc: 0x%04x -> 0x%04x data: 0x%08x len: %d\n", */
+/* bc, crc, bd, bl); */
+ return crc;
+}
+
unsigned calc_stuff_bits(struct can_frame *frame) {
uint32_t bitmap[4];
unsigned start = 0, end;
- uint32_t mask, ones = 0, basemask = 0xf0000000;
+ uint32_t mask, ones = 0, basemask = 0xf8000000;
unsigned stuffed = 0;
memset(bitmap, 0, sizeof(bitmap));
+ crc_t crc = 0;
+ uint32_t crcx;
if (frame->can_id & CAN_EFF_FLAG) {
bitmap[0] =
- (frame->can_id >> 25);
+ ((frame->can_id & CAN_EFF_MASK) >> 25);
bitmap[1] =
- (frame->can_id >> 18) << 25 |
- 3 << 23 |
- (frame->can_id & 0x3ffff) << 7 |
- (!!(frame->can_id & CAN_RTR_FLAG)) << 6 |
- 0 << 4 |
+ ((frame->can_id & CAN_EFF_MASK) >> 18) << 25 |
+ 3 << 23 |
+ ((frame->can_id & CAN_EFF_MASK) & 0x3ffff) << 7 |
+ (!!(frame->can_id & CAN_RTR_FLAG)) << 6 |
+ 0 << 4 |
frame->can_dlc & 0xf;
bitmap[2] = htonl(((uint32_t*)frame->data)[0]);
bitmap[3] = htonl(((uint32_t*)frame->data)[1]);
- start = 28;
+ start = 27;
end = 64 + 8*frame->can_dlc;
+ crc = crc_update(crc, bitmap[0] << start, 32-start);
+ crc = crc_update(crc, bitmap[1], 32);
+ crc = crc_update(crc, bitmap[2], end - 64 > 32 ? 32 : end - 64);
+ crc = crc_update(crc, bitmap[3], end > 96 ? end - 96 : 0);
+ crcx = (uint32_t)htons(crc) << 17;
+ if (frame->can_dlc < 4)
+ bitmap[2] |= crcx >> (frame->can_dlc*8);
+ if (frame->can_dlc = 3)
+ bitmap[3] |= crcx << 8;
+ if (frame->can_dlc >= 4)
+ bitmap[3] |= crcx >> (frame->can_dlc*8);
} else {
bitmap[0] =
(frame->can_id << 7) |
frame->can_dlc & 0xf;
bitmap[1] = htonl(((uint32_t*)frame->data)[0]);
bitmap[2] = htonl(((uint32_t*)frame->data)[1]);
- start = 14;
+ start = 13;
end = 32 + 8*frame->can_dlc;
}
/* TODO: Calc stuff bits in CRC */