Add core OP support for CRC validation, with extra support for Volkswagen MQB

selfdrive/can/common.h

@@ -7,11 +7,13 @@
 
 #define ARRAYSIZE(x) (sizeof(x)/sizeof(x[0]))
 
-
 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 pedal_checksum(unsigned int address, uint64_t d, int l);
 
+void init_crc_lookup_tables();
+unsigned int volkswagen_crc(unsigned int address, uint64_t d, int l);
+
 struct SignalPackValue {
   const char* name;
   double value;
@@ -44,6 +46,8 @@ enum SignalType {
   TOYOTA_CHECKSUM,
   PEDAL_CHECKSUM,
   PEDAL_COUNTER,
+  VOLKSWAGEN_CRC,
+  VOLKSWAGEN_COUNTER,
 };
 
 struct Signal {

selfdrive/can/dbc_template.cc

@@ -31,6 +31,10 @@ const Signal sigs_{{address}}[] = {
       .type = SignalType::PEDAL_CHECKSUM,
       {% elif address in [512, 513] and sig.name == "COUNTER_PEDAL" %}
       .type = SignalType::PEDAL_COUNTER,
+      {% elif checksum_type == "volkswagen" and sig.name == "CRC" %}
+      .type = SignalType::VOLKSWAGEN_CRC,
+      {% elif checksum_type == "volkswagen" and sig.name == "COUNTER" %}
+      .type = SignalType::VOLKSWAGEN_COUNTER,
       {% else %}
       .type = SignalType::DEFAULT,
       {% endif %}

selfdrive/can/libdbc_py.py

@@ -41,6 +41,8 @@
   TOYOTA_CHECKSUM,
   PEDAL_CHECKSUM,
   PEDAL_COUNTER,
+  VOLKSWAGEN_CRC,
+  VOLKSWAGEN_COUNTER,
 } SignalType;
 
 typedef struct {

selfdrive/can/packer.cc

@@ -51,6 +51,8 @@ namespace {
           signal_lookup[std::make_pair(msg->address, std::string(sig->name))] = *sig;
         }
       }
+
+      init_crc_lookup_tables();
     }
 
     uint64_t pack(uint32_t address, const std::vector<SignalPackValue> &signals, int counter) {
@@ -82,28 +84,42 @@ namespace {
         }
         auto sig = sig_it->second;
 
-        if (sig.type != SignalType::HONDA_COUNTER){
+        if ((sig.type != SignalType::HONDA_COUNTER) && (sig.type != SignalType::VOLKSWAGEN_COUNTER)) {
           WARN("COUNTER signal type not valid\n");
         }
 
         ret = set_value(ret, sig, counter);
       }
 
-      auto sig_it = signal_lookup.find(std::make_pair(address, "CHECKSUM"));
-      if (sig_it != signal_lookup.end()) {
-        auto sig = sig_it->second;
-        if (sig.type == SignalType::HONDA_CHECKSUM){
+      auto sig_it_checksum = signal_lookup.find(std::make_pair(address, "CHECKSUM"));
+      if (sig_it_checksum != signal_lookup.end()) {
+        auto sig = sig_it_checksum->second;
+        if (sig.type == SignalType::HONDA_CHECKSUM) {
           unsigned int chksm = honda_checksum(address, ret, message_lookup[address].size);
           ret = set_value(ret, sig, chksm);
         }
-        else if (sig.type == SignalType::TOYOTA_CHECKSUM){
+        else if (sig.type == SignalType::TOYOTA_CHECKSUM) {
           unsigned int chksm = toyota_checksum(address, ret, message_lookup[address].size);
           ret = set_value(ret, sig, chksm);
         } else {
           //WARN("CHECKSUM signal type not valid\n");
         }
       }
 
+      auto sig_it_crc = signal_lookup.find(std::make_pair(address, "CRC"));
+      if (sig_it_crc != signal_lookup.end()) {
+        auto sig = sig_it_crc->second;
+        if (sig.type == SignalType::VOLKSWAGEN_CRC) {
+          // FIXME: Hackish fix for an endianness issue. The message is in reverse byte order
+          // until later in the pack process. Checksums can be run backwards, CRCs not so much.
+          // The correct fix is unclear but this works for the moment.
+          unsigned int chksm = volkswagen_crc(address, ReverseBytes(ret), message_lookup[address].size);
+          ret = set_value(ret, sig, chksm);
+        } else {
+          // WARN("CRC signal type not valid\n");
+        }
+      }
+
       return ret;
     }
 

selfdrive/can/packer_impl.pyx

@@ -18,7 +18,9 @@ ctypedef enum SignalType:
   HONDA_COUNTER,
   TOYOTA_CHECKSUM,
   PEDAL_CHECKSUM,
-  PEDAL_COUNTER
+  PEDAL_COUNTER,
+  VOLKSWAGEN_CRC,
+  VOLKSWAGEN_COUNTER
 
 cdef struct Signal:
   const char* name

selfdrive/can/parser.cc

@@ -24,9 +24,11 @@
 // #define DEBUG printf
 #define INFO printf
 
-
 #define MAX_BAD_COUNTER 5
 
+// Static lookup table for fast computation of CRC8 poly 0x2F, aka 8H2F/AUTOSAR
+uint8_t crc8_lut_8h2f[256];
+
 unsigned int honda_checksum(unsigned int address, uint64_t d, int l) {
   d >>= ((8-l)*8); // remove padding
   d >>= 4; // remove checksum
@@ -75,6 +77,95 @@ unsigned int pedal_checksum(unsigned int address, uint64_t d, int l) {
   return crc;
 }
 
+void gen_crc_lookup_table(uint8_t poly, uint8_t crc_lut[])
+{
+  uint8_t crc;
+  int i, j;
+
+  for (i = 0; i < 256; i++) {
+    crc = i;
+    for (j = 0; j < 8; j++) {
+      if ((crc & 0x80) != 0)
+        crc = (uint8_t)((crc << 1) ^ poly);
+      else
+        crc <<= 1;
+    }
+    crc_lut[i] = crc;
+  }
+}
+
+void init_crc_lookup_tables()
+{
+  // At init time, set up static lookup tables for fast CRC computation.
+
+  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)
+{
+  // 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 *dat = (uint8_t *)&d;
+  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 ^= dat[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 = dat[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];
+      break;
+    case 0x9F:  // EPS_01 Electric Power Steering
+      crc ^= (uint8_t[]){0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5}[counter];
+      break;
+    case 0xAD:  // Getriebe_11 Automatic Gearbox
+      crc ^= (uint8_t[]){0x3F,0x69,0x39,0xDC,0x94,0xF9,0x14,0x64,0xD8,0x6A,0x34,0xCE,0xA2,0x55,0xB5,0x2C}[counter];
+      break;
+    case 0xFD:  // ESP_21 Electronic Stability Program
+      crc ^= (uint8_t[]){0xB4,0xEF,0xF8,0x49,0x1E,0xE5,0xC2,0xC0,0x97,0x19,0x3C,0xC9,0xF1,0x98,0xD6,0x61}[counter];
+      break;
+    case 0x106: // ESP_05 Electronic Stability Program
+      crc ^= (uint8_t[]){0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07}[counter];
+      break;
+    case 0x117: // ACC_10 Automatic Cruise Control
+      crc ^= (uint8_t[]){0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC}[counter];
+      break;
+    case 0x122: // ACC_06 Automatic Cruise Control
+      crc ^= (uint8_t[]){0x37,0x7D,0xF3,0xA9,0x18,0x46,0x6D,0x4D,0x3D,0x71,0x92,0x9C,0xE5,0x32,0x10,0xB9}[counter];
+      break;
+    case 0x126: // HCA_01 Heading Control Assist
+      crc ^= (uint8_t[]){0xDA,0xDA,0xDA,0xDA,0xDA,0xDA,0xDA,0xDA,0xDA,0xDA,0xDA,0xDA,0xDA,0xDA,0xDA,0xDA}[counter];
+      break;
+    case 0x12B: // GRA_ACC_01 Steering wheel controls for ACC
+      crc ^= (uint8_t[]){0x6A,0x38,0xB4,0x27,0x22,0xEF,0xE1,0xBB,0xF8,0x80,0x84,0x49,0xC7,0x9E,0x1E,0x2B}[counter];
+      break;
+    case 0x30C: // ACC_02 Automatic Cruise Control
+      crc ^= (uint8_t[]){0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F}[counter];
+      break;
+    case 0x3C0: // Klemmen_Status_01 ignition and starting status
+      crc ^= (uint8_t[]){0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3}[counter];
+      break;
+    case 0x65D: // ESP_20 Electronic Stability Program
+      crc ^= (uint8_t[]){0xAC,0xB3,0xAB,0xEB,0x7A,0xE1,0x3B,0xF7,0x73,0xBA,0x7C,0x9E,0x06,0x5F,0x02,0xD9}[counter];
+      break;
+    default:    // As-yet undefined CAN message, CRC check expected to fail
+      INFO("Attempt to CRC check undefined Volkswagen message 0x%02X\n", address);
+      crc ^= (uint8_t[]){0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}[counter];
+      break;
+  }
+  crc = crc8_lut_8h2f[crc];
+
+  return crc ^ 0xFF; // Return after standard final XOR for CRC8 8H2F/AUTOSAR
+}
+
 namespace {
 
 uint64_t read_u64_be(const uint8_t* v) {
@@ -133,7 +224,7 @@ struct MessageState {
 
       if (sig.type == SignalType::HONDA_CHECKSUM) {
         if (honda_checksum(address, dat, size) != tmp) {
-          INFO("%X CHECKSUM FAIL\n", address);
+          INFO("0x%X CHECKSUM FAIL\n", address);
           return false;
         }
       } else if (sig.type == SignalType::HONDA_COUNTER) {
@@ -142,12 +233,21 @@ struct MessageState {
         }
       } else if (sig.type == SignalType::TOYOTA_CHECKSUM) {
         if (toyota_checksum(address, dat, size) != tmp) {
-          INFO("%X CHECKSUM FAIL\n", address);
+          INFO("0x%X CHECKSUM FAIL\n", address);
+          return false;
+        }
+      } else if (sig.type == SignalType::VOLKSWAGEN_CRC) {
+        if (volkswagen_crc(address, dat, size) != tmp) {
+          INFO("0x%X CRC FAIL\n", address);
+          return false;
+        }
+      } else if (sig.type == SignalType::VOLKSWAGEN_COUNTER) {
+        if (!update_counter_generic(tmp, sig.b2)) {
           return false;
         }
       } else if (sig.type == SignalType::PEDAL_CHECKSUM) {
         if (pedal_checksum(address, dat, size) != tmp) {
-          INFO("%X PEDAL CHECKSUM FAIL\n", address);
+          INFO("0x%X PEDAL CHECKSUM FAIL\n", address);
           return false;
         }
       } else if (sig.type == SignalType::PEDAL_COUNTER) {
@@ -171,7 +271,7 @@ struct MessageState {
     if (((old_counter+1) & ((1 << cnt_size) -1)) != v) {
       counter_fail += 1;
       if (counter_fail > 1) {
-        INFO("%X COUNTER FAIL %d -- %d vs %d\n", address, counter_fail, old_counter, (int)v);
+        INFO("0x%X COUNTER FAIL %d -- %d vs %d\n", address, counter_fail, old_counter, (int)v);
       }
       if (counter_fail >= MAX_BAD_COUNTER) {
         return false;
@@ -223,7 +323,9 @@ class CANParser {
     }
 
     dbc = dbc_lookup(dbc_name);
-    assert(dbc); 
+    assert(dbc);
+    init_crc_lookup_tables();
+
     for (const auto& op : options) {
       MessageState state = {
         .address = op.address,

selfdrive/can/parser_pyx.pxd

@@ -12,7 +12,9 @@ ctypedef enum SignalType:
   HONDA_COUNTER,
   TOYOTA_CHECKSUM,
   PEDAL_CHECKSUM,
-  PEDAL_COUNTER
+  PEDAL_COUNTER,
+  VOLKSWAGEN_CRC,
+  VOLKSWAGEN_COUNTER
 
 cdef struct Signal:
   const char* name

selfdrive/can/process_dbc.py

@@ -38,48 +38,64 @@ def main():
     if dbc_mtime < out_mtime and template_mtime < out_mtime and this_file_mtime < out_mtime:
       continue #skip output is newer than template and dbc
 
-    msgs = [(address, msg_name, msg_size, sorted(msg_sigs, key=lambda s: s.name not in ("COUNTER", "CHECKSUM"))) # process counter and checksums first
+    msgs = [(address, msg_name, msg_size, sorted(msg_sigs, key=lambda s: s.name not in ("COUNTER", "CHECKSUM", "CRC"))) # process counter and checksums first
             for address, ((msg_name, msg_size), msg_sigs) in sorted(can_dbc.msgs.items()) if msg_sigs]
 
     def_vals = {a: set(b) for a,b in can_dbc.def_vals.items()} #remove duplicates
     def_vals = [(address, sig) for address, sig in sorted(def_vals.items())]
 
     if can_dbc.name.startswith("honda") or can_dbc.name.startswith("acura"):
-      checksum_type = "honda"
+      car_type = "honda"
       checksum_size = 4
+      counter_size = 2
     elif can_dbc.name.startswith("toyota") or can_dbc.name.startswith("lexus"):
-      checksum_type = "toyota"
+      car_type = "toyota"
       checksum_size = 8
+      counter_size = None
+    elif can_dbc.name.startswith("vw") or can_dbc.name.startswith("volkswagen") or can_dbc.name.startswith("audi") or can_dbc.name.startswith ("seat") or can_dbc.name.startswith("skoda"):
+      car_type = "volkswagen"
+      checksum_size = 8
+      counter_size = 4
     else:
-      checksum_type = None
+      car_type = None
+      checksum_size = None
+      counter_size = None
 
     for address, msg_name, msg_size, sigs in msgs:
       for sig in sigs:
-        if checksum_type is not None and sig.name == "CHECKSUM":
-          if sig.size != checksum_size:
-            sys.exit("CHECKSUM is not %d bits longs %s" % (checksum_size, msg_name))
-          if checksum_type == "honda" and sig.start_bit % 8 != 3:
-            sys.exit("CHECKSUM starts at wrong bit %s" % msg_name)
-          if checksum_type == "toyota" and sig.start_bit % 8 != 7:
-            sys.exit("CHECKSUM starts at wrong bit %s" % msg_name)
-        if checksum_type == "honda" and sig.name == "COUNTER":
-          if sig.size != 2:
-            sys.exit("COUNTER is not 2 bits longs %s" % msg_name)
-          if sig.start_bit % 8 != 5:
-            sys.exit("COUNTER starts at wrong bit %s" % msg_name)
+        if car_type is not None:
+          if sig.name == "CHECKSUM":
+            if sig.size != checksum_size:
+              sys.exit("CHECKSUM is not %d bits long %s" % (checksum_size, msg_name))
+            if car_type == "honda" and sig.start_bit % 8 != 3:
+              sys.exit("CHECKSUM starts at wrong bit %s" % msg_name)
+            if car_type == "toyota" and sig.start_bit % 8 != 7:
+              sys.exit("CHECKSUM starts at wrong bit %s" % msg_name)
+          elif sig.name == "CRC":
+            if sig.size != checksum_size:
+              sys.exit("CRC is not %d bits long %s" % (checksum_size, msg_name))
+            if car_type == "volkswagen" and sig.start_bit % 8 != 0:
+              sys.exit("CRC starts at wrong bit %s" % msg_name)
+          elif sig.name == "COUNTER" and car_type in ["honda", "volkswagen"]:
+            if sig.size != counter_size:
+              sys.exit("COUNTER is not %d bits long %s" % (counter_size, msg_name))
+            if car_type == "honda" and sig.start_bit % 8 != 5:
+              sys.exit("COUNTER starts at wrong bit %s" % msg_name)
+            if car_type == "volkswagen" and sig.start_bit % 8 != 0:
+              sys.exit("COUNTER starts at wrong bit %s" % msg_name)
         if address in [0x200, 0x201]:
           if sig.name == "COUNTER_PEDAL" and sig.size != 4:
-            sys.exit("PEDAL COUNTER is not 4 bits longs %s" % msg_name)
+            sys.exit("PEDAL COUNTER is not 4 bits long %s" % msg_name)
           if sig.name == "CHECKSUM_PEDAL" and sig.size != 8:
-            sys.exit("PEDAL CHECKSUM is not 8 bits longs %s" % msg_name)
+            sys.exit("PEDAL CHECKSUM is not 8 bits long %s" % msg_name)
 
     # Fail on duplicate message names
     c = Counter([msg_name for address, msg_name, msg_size, sigs in msgs])
     for name, count in c.items():
       if count > 1:
         sys.exit("Duplicate message name in DBC file %s" % name)
 
-    parser_code = template.render(dbc=can_dbc, checksum_type=checksum_type, msgs=msgs, def_vals=def_vals, len=len)
+    parser_code = template.render(dbc=can_dbc, checksum_type=car_type, msgs=msgs, def_vals=def_vals, len=len)
 
 
     with open(out_fn, "w") as out_f: