From 74af4417db6d331085568b37804afc7af56a17dc Mon Sep 17 00:00:00 2001
From: George Hotz <geohot@gmail.com>
Date: Sun, 10 Jun 2018 23:11:29 -0700
Subject: [PATCH] can crc

---
 board/drivers/canbitbang.h    | 38 ++++++++++++++++++++++++-----------
 tests/gmbitbang/test_packer.c |  3 +++
 2 files changed, 29 insertions(+), 12 deletions(-)

diff --git a/board/drivers/canbitbang.h b/board/drivers/canbitbang.h
index 9732adf597b195..e152412151da9b 100644
--- a/board/drivers/canbitbang.h
+++ b/board/drivers/canbitbang.h
@@ -1,3 +1,4 @@
+#define MAX_BITS_CAN_PACKET (64+44+25)
 
 // returns out_len
 int do_bitstuff(char *out, char *in, int in_len) {
@@ -26,24 +27,35 @@ int do_bitstuff(char *out, char *in, int in_len) {
   return j;
 }
 
+int append_crc(char *in, int in_len) {
+  int crc = 0;
+  for (int i = 0; i < in_len; i++) {
+    crc <<= 1;
+    if (in[i] ^ ((crc>>15)&1)) {
+      crc = crc ^ 0x4599;
+    }
+    crc &= 0x7fff;
+  }
+  for (int i = 14; i >= 0; i--) {
+    in[in_len++] = (crc>>i)&1;
+  }
+  return in_len;
+}
+
 int get_bit_message(char *out) {
-  char test_pkt[] = {
+  char test_pkt[MAX_BITS_CAN_PACKET];
+  char test_pkt_src[] = {
     0, // SOF
     0,0,0,0, // ID10-ID7
-    //1, // bitstuff
     0,0,1,0,1,0,0,   // ID6-ID0
     0, // RTR
     0, // IDE
     0, // reserved
-    //1, // bitstuff
     0,0,0,1, // len
     0,0,0,0,0, // 1st byte 7-3
-    //1, // bitstuff
     0,0,1,     // 1st byte 2-0
-
-    // CRC from real message
-    1,1,1,0,1,1,1,0,1,0,1,0,0,1,1,
   };
+
   char footer[] = {
     1,  // CRC delimiter
 
@@ -54,12 +66,14 @@ int get_bit_message(char *out) {
   };
   #define SPEEED 30
 
-  // testing
-  //char *test_pkt_stuffed = test_pkt;
-  //int len = sizeof(test_pkt);
+  // copy packet
+  for (int i = 0; i < sizeof(test_pkt_src); i++) test_pkt[i] = test_pkt_src[i];
+
+  // append crc
+  int len = append_crc(test_pkt, sizeof(test_pkt_src));
 
   // do bitstuffing
-  int len = do_bitstuff(out, test_pkt, sizeof(test_pkt));
+  len = do_bitstuff(out, test_pkt, len);
 
   // append footer
   for (int i = 0; i < sizeof(footer); i++) {
@@ -85,7 +99,7 @@ void bitbang_gmlan(CAN_FIFOMailBox_TypeDef *to_bang) {
   puth(can_num_lookup[3]);
   puts("\n");
 
-  char pkt_stuffed[64+44+25];
+  char pkt_stuffed[MAX_BITS_CAN_PACKET];
   int len = get_bit_message(pkt_stuffed);
 
   // actual bitbang loop
diff --git a/tests/gmbitbang/test_packer.c b/tests/gmbitbang/test_packer.c
index 0f4386fab278ca..4387c839914140 100644
--- a/tests/gmbitbang/test_packer.c
+++ b/tests/gmbitbang/test_packer.c
@@ -4,10 +4,13 @@
 int main() {
   char out[100];
   int len = get_bit_message(out);
+  printf("T:");
   for (int i = 0; i < len; i++) {
     printf("%d", out[i]);
   }
   printf("\n");
+  printf("R:0000010010100000100010000010011110111010100111111111111111");
+  printf("\n");
   return 0;
 }