latester: Add partial support for CRC calculation

[can-benchmark.git] / latester / latester.c

diff --git a/latester/latester.c b/latester/latester.c
index f58086afba0e76f16e1b4288025aa8f897c7a6be..d09b1ab1a3027fedf670a657e4f6ee348306d10a 100644 (file)
--- a/latester/latester.c
+++ b/latester/latester.c
@@ -158,27 +158,77 @@ static inline char *tstamp_str(const void *ctx, struct timespec *tstamp)
                               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) |
@@ -187,7 +237,7 @@ unsigned calc_stuff_bits(struct can_frame *frame) {
                        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 */