CAN FD support in packer + parser (commaai#568)

* can fd support in packer + parser

* don't need these anymore

* fix pedal crc

* fix dynamic parsing

* cleanup

* packer test

can/SConscript

@@ -11,6 +11,7 @@ for x in sorted(os.listdir('../')):
     in_fn = [os.path.join('../', x), 'dbc_template.cc']
     out_fn = os.path.join('dbc_out', x.replace(".dbc", ".cc"))
     dbc = env.Command(out_fn, in_fn, compile_dbc)
+    env.Depends(dbc, ["dbc.py", "process_dbc.py"])
     dbcs.append(dbc)
 
 libdbc = env.SharedLibrary('libdbc', ["dbc.cc", "parser.cc", "packer.cc", "common.cc"]+dbcs, LIBS=["capnp", "kj"])

can/common.cc

@@ -1,50 +1,44 @@
 #include "common.h"
 
-unsigned int honda_checksum(unsigned int address, uint64_t d, int l) {
-  d >>= ((8-l)*8); // remove padding
-  d >>= 4; // remove checksum
-
+unsigned int honda_checksum(uint32_t address, const std::vector<uint8_t> &d) {
   int s = 0;
-  bool extended = address > 0x7FF; // extended can
   while (address) { s += (address & 0xF); address >>= 4; }
-  while (d) { s += (d & 0xF); d >>= 4; }
+  for (int i = 0; i < d.size(); i++) {
+    uint8_t x = d[i];
+    if (i == d.size()-1) x >>= 4; // remove checksum
+    s += (x & 0xF) + (x >> 4);
+  }
   s = 8-s;
-  if (extended) s += 3;
-  s &= 0xF;
+  if (address > 0x7FF) s += 3;  // extended can
 
-  return s;
+  return s & 0xF;
 }
 
-unsigned int toyota_checksum(unsigned int address, uint64_t d, int l) {
-  d >>= ((8-l)*8); // remove padding
-  d >>= 8; // remove checksum
-
-  unsigned int s = l;
+unsigned int toyota_checksum(uint32_t address, const std::vector<uint8_t> &d) {
+  unsigned int s = d.size();
   while (address) { s += address & 0xFF; address >>= 8; }
-  while (d) { s += d & 0xFF; d >>= 8; }
+  for (int i = 0; i < d.size() - 1; i++) { s += d[i]; }
 
   return s & 0xFF;
 }
 
-unsigned int subaru_checksum(unsigned int address, uint64_t d, int l) {
-  d >>= ((8-l)*8); // remove padding
-
+unsigned int subaru_checksum(uint32_t address, const std::vector<uint8_t> &d) {
   unsigned int s = 0;
   while (address) { s += address & 0xFF; address >>= 8; }
-  l -= 1; // checksum is first byte
-  while (l) { s += d & 0xFF; d >>= 8; l -= 1; }
+
+  // skip checksum in first byte
+  for (int i = 1; i < d.size(); i++) { s += d[i]; };
 
   return s & 0xFF;
 }
 
-unsigned int chrysler_checksum(unsigned int address, uint64_t d, int l) {
-  /* This function does not want the checksum byte in the input data.
-  jeep chrysler canbus checksum from http://illmatics.com/Remote%20Car%20Hacking.pdf */
+unsigned int chrysler_checksum(uint32_t address, const std::vector<uint8_t> &d) {
+  /* jeep chrysler canbus checksum from http://illmatics.com/Remote%20Car%20Hacking.pdf */
   uint8_t checksum = 0xFF;
-  for (int j = 0; j < (l - 1); j++) {
+  for (int j = 0; j < (d.size() - 1); j++) {
     uint8_t shift = 0x80;
-    uint8_t curr = (d >> 8*j) & 0xFF;
-    for (int i=0; i<8; i++) {
+    uint8_t curr = d[j];
+    for (int i = 0; i < 8; i++) {
       uint8_t bit_sum = curr & shift;
       uint8_t temp_chk = checksum & 0x80U;
       if (bit_sum != 0U) {
@@ -94,22 +88,22 @@ void init_crc_lookup_tables() {
   gen_crc_lookup_table(0x2F, crc8_lut_8h2f);    // CRC-8 8H2F/AUTOSAR for Volkswagen
 }
 
-unsigned int volkswagen_crc(unsigned int address, uint64_t d, int l) {
+unsigned int volkswagen_crc(uint32_t address, const std::vector<uint8_t> &d) {
   // Volkswagen uses standard CRC8 8H2F/AUTOSAR, but they compute it with
   // a magic variable padding byte tacked onto the end of the payload.
   // https://www.autosar.org/fileadmin/user_upload/standards/classic/4-3/AUTOSAR_SWS_CRCLibrary.pdf
 
   uint8_t crc = 0xFF; // Standard init value for CRC8 8H2F/AUTOSAR
 
   // CRC the payload first, skipping over the first byte where the CRC lives.
-  for (int i = 1; i < l; i++) {
-    crc ^= (d >> (i*8)) & 0xFF;
+  for (int i = 1; i < d.size(); i++) {
+    crc ^= d[i];
     crc = crc8_lut_8h2f[crc];
   }
 
   // Look up and apply the magic final CRC padding byte, which permutes by CAN
   // address, and additionally (for SOME addresses) by the message counter.
-  uint8_t counter = ((d >> 8) & 0xFF) & 0x0F;
+  uint8_t counter = d[1] & 0x0F;
   switch(address) {
     case 0x86:  // LWI_01 Steering Angle
       crc ^= (uint8_t[]){0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86}[counter];
@@ -175,47 +169,20 @@ unsigned int volkswagen_crc(unsigned int address, uint64_t d, int l) {
   return crc ^ 0xFF; // Return after standard final XOR for CRC8 8H2F/AUTOSAR
 }
 
-unsigned int pedal_checksum(uint64_t d, int l) {
+unsigned int pedal_checksum(const std::vector<uint8_t> &d) {
   uint8_t crc = 0xFF;
   uint8_t poly = 0xD5; // standard crc8
 
-  d >>= ((8-l)*8); // remove padding
-  d >>= 8; // remove checksum
-
-  int i, j;
-  for (i = 0; i < l - 1; i++) {
-    crc ^= (d >> (i*8)) & 0xFF;
-    for (j = 0; j < 8; j++) {
+  // skip checksum byte
+  for (int i = d.size()-2; i >= 0; i--) {
+    crc ^= d[i];
+    for (int j = 0; j < 8; j++) {
       if ((crc & 0x80) != 0) {
         crc = (uint8_t)((crc << 1) ^ poly);
-      }
-      else {
+      } else {
         crc <<= 1;
       }
     }
   }
   return crc;
 }
-
-
-uint64_t read_u64_be(const uint8_t* v) {
-  return (((uint64_t)v[0] << 56)
-          | ((uint64_t)v[1] << 48)
-          | ((uint64_t)v[2] << 40)
-          | ((uint64_t)v[3] << 32)
-          | ((uint64_t)v[4] << 24)
-          | ((uint64_t)v[5] << 16)
-          | ((uint64_t)v[6] << 8)
-          | (uint64_t)v[7]);
-}
-
-uint64_t read_u64_le(const uint8_t* v) {
-  return ((uint64_t)v[0]
-          | ((uint64_t)v[1] << 8)
-          | ((uint64_t)v[2] << 16)
-          | ((uint64_t)v[3] << 24)
-          | ((uint64_t)v[4] << 32)
-          | ((uint64_t)v[5] << 40)
-          | ((uint64_t)v[6] << 48)
-          | ((uint64_t)v[7] << 56));
-}

can/common.h

@@ -15,20 +15,18 @@
 #define INFO printf
 #define WARN printf
 #define DEBUG(...)
-// #define DEBUG printf
+//#define DEBUG printf
 
 #define MAX_BAD_COUNTER 5
 
-// Helper functions
-unsigned int honda_checksum(unsigned int address, uint64_t d, int l);
-unsigned int toyota_checksum(unsigned int address, uint64_t d, int l);
-unsigned int subaru_checksum(unsigned int address, uint64_t d, int l);
-unsigned int chrysler_checksum(unsigned int address, uint64_t d, int l);
+// Car specific functions
+unsigned int honda_checksum(uint32_t address, const std::vector<uint8_t> &d);
+unsigned int toyota_checksum(uint32_t address, const std::vector<uint8_t> &d);
+unsigned int subaru_checksum(uint32_t address, const std::vector<uint8_t> &d);
+unsigned int chrysler_checksum(uint32_t address, const std::vector<uint8_t> &d);
 void init_crc_lookup_tables();
-unsigned int volkswagen_crc(unsigned int address, uint64_t d, int l);
-unsigned int pedal_checksum(uint64_t d, int l);
-uint64_t read_u64_be(const uint8_t* v);
-uint64_t read_u64_le(const uint8_t* v);
+unsigned int volkswagen_crc(uint32_t address, const std::vector<uint8_t> &d);
+unsigned int pedal_checksum(const std::vector<uint8_t> &d);
 
 class MessageState {
 public:
@@ -48,7 +46,7 @@ class MessageState {
   bool ignore_checksum = false;
   bool ignore_counter = false;
 
-  bool parse(uint64_t sec, uint8_t * dat);
+  bool parse(uint64_t sec, const std::vector<uint8_t> &dat);
   bool update_counter_generic(int64_t v, int cnt_size);
 };
 
@@ -85,6 +83,6 @@ class CANPacker {
 
 public:
   CANPacker(const std::string& dbc_name);
-  uint64_t pack(uint32_t address, const std::vector<SignalPackValue> &values, int counter);
+  std::vector<uint8_t> pack(uint32_t address, const std::vector<SignalPackValue> &values, int counter);
   Msg* lookup_message(uint32_t address);
 };

can/common.pxd

@@ -1,12 +1,12 @@
 # distutils: language = c++
 # cython: language_level=3
 
-from libc.stdint cimport uint32_t, uint64_t, uint16_t
-from libcpp.vector cimport vector
+from libc.stdint cimport uint8_t, uint16_t, uint32_t, uint64_t
+from libcpp cimport bool
 from libcpp.map cimport map
 from libcpp.string cimport string
+from libcpp.vector cimport vector
 from libcpp.unordered_set cimport unordered_set
-from libcpp cimport bool
 
 
 cdef extern from "common_dbc.h":
@@ -24,7 +24,7 @@ cdef extern from "common_dbc.h":
 
   cdef struct Signal:
     const char* name
-    int b1, b2, bo
+    int start_bit, msb, lsb, size
     bool is_signed
     double factor, offset
     SignalType type
@@ -80,4 +80,4 @@ cdef extern from "common.h":
 
   cdef cppclass CANPacker:
    CANPacker(string)
-   uint64_t pack(uint32_t, vector[SignalPackValue], int counter)
+   vector[uint8_t] pack(uint32_t, vector[SignalPackValue], int counter)

can/common_dbc.h

@@ -44,7 +44,7 @@ enum SignalType {
 
 struct Signal {
   const char* name;
-  int b1, b2, bo;
+  int start_bit, msb, lsb, size;
   bool is_signed;
   double factor, offset;
   bool is_little_endian;

can/dbc.py

@@ -13,7 +13,7 @@ def int_or_float(s):
     return float(s)
 
 
-DBCSignal = namedtuple("DBCSignal", ["name", "start_bit", "size", "is_little_endian", "is_signed",
+DBCSignal = namedtuple("DBCSignal", ["name", "start_bit", "msb", "lsb", "size", "is_little_endian", "is_signed",
                                      "factor", "offset", "tmin", "tmax", "units"])
 
 
@@ -40,8 +40,8 @@ def __init__(self, fn):
     # A dictionary which maps message ids to a list of tuples (signal name, definition value pairs)
     self.def_vals = defaultdict(list)
 
-    # lookup to bit reverse each byte
-    self.bits_index = [(i & ~0b111) + ((-i - 1) & 0b111) for i in range(64)]
+    # used to find big endian LSB from MSB and size
+    be_bits = [(j + i*8) for i in range(64) for j in range(7, -1, -1)]
 
     for l in self.txt:
       l = l.strip()
@@ -83,10 +83,19 @@ def __init__(self, fn):
         tmax = int_or_float(dat.group(go + 9))
         units = dat.group(go + 10)
 
+        if is_little_endian:
+          lsb = start_bit
+          msb = start_bit + signal_size - 1
+        else:
+          lsb = be_bits[be_bits.index(start_bit) + signal_size - 1]
+          msb = start_bit
+
         self.msgs[ids][1].append(
-          DBCSignal(sgname, start_bit, signal_size, is_little_endian,
+          DBCSignal(sgname, start_bit, msb, lsb, signal_size, is_little_endian,
                     is_signed, factor, offset, tmin, tmax, units))
 
+        assert lsb < (64*8) and msb < (64*8), f"Signal out of bounds: {msb=} {lsb=}"
+
       if l.startswith("VAL_ "):
         # new signal value/definition
         dat = val_regexp.match(l)

can/dbc_template.cc

@@ -6,15 +6,11 @@ namespace {
 const Signal sigs_{{address}}[] = {
   {% for sig in sigs %}
     {
-      {% if sig.is_little_endian %}
-        {% set b1 = sig.start_bit %}
-      {% else %}
-        {% set b1 = (sig.start_bit//8)*8  + (-sig.start_bit-1) % 8 %}
-      {% endif %}
       .name = "{{sig.name}}",
-      .b1 = {{b1}},
-      .b2 = {{sig.size}},
-      .bo = {{64 - (b1 + sig.size)}},
+      .start_bit = {{sig.start_bit}},
+      .msb  = {{sig.msb}},
+      .lsb = {{sig.lsb}},
+      .size = {{sig.size}},
       .is_signed = {{"true" if sig.is_signed else "false"}},
       .factor = {{sig.factor}},
       .offset = {{sig.offset}},