From 6cc9e721dbd2334e35ddb8a9e4d547eea94c47dd Mon Sep 17 00:00:00 2001
From: Michal Sojka <sojka@os.inf.tu-dresden.de>
Date: Wed, 21 Sep 2011 23:19:05 +0200
Subject: [PATCH] latester: Add partial support for CRC calculation

(not much tested)
---
 latester/latester.c | 68 +++++++++++++++++++++++++++++++++++++++------
 1 file changed, 59 insertions(+), 9 deletions(-)

diff --git a/latester/latester.c b/latester/latester.c
index f58086a..d09b1ab 100644
--- 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 */
-- 
2.39.2