Verify response CRC and dynamic CRC calculations for commands

Author: sandeepmistry

src/utility/ECC508.cpp

@@ -36,16 +36,9 @@ int ECC508Class::random(byte data[], size_t length)
   }
 
   while (length) {
-    _wire->beginTransmission(_address);
-    _wire->write(0x03);
-    _wire->write(0x07);
-    _wire->write(0x1b);
-    _wire->write(0x00);
-    _wire->write(0x00);
-    _wire->write(0x00);
-    _wire->write(0x24);
-    _wire->write(0xcd);
-    _wire->endTransmission();
+    if (!sendCommand(0x1b, 0x00, 0x0000)) {
+      return 0;
+    }
 
     delay(23);
 
@@ -117,18 +110,11 @@ int ECC508Class::version()
     return 0;
   }
 
-  _wire->beginTransmission(_address);
-  _wire->write(0x03);
-  _wire->write(0x07);
-  _wire->write(0x30);
-  _wire->write(0x00);
-  _wire->write(0x00);
-  _wire->write(0x00);
-  _wire->write(0x03);
-  _wire->write(0x5d);
-  _wire->endTransmission();
+  if (!sendCommand(0x30, 0x00, 0x0000)) {
+    return 0;
+  }
 
-  delay(2);
+  delay(1);
 
   if (!receiveResponse(&version, sizeof(version))) {
     return 0;
@@ -140,6 +126,26 @@ int ECC508Class::version()
   return version;
 }
 
+int ECC508Class::sendCommand(uint8_t opcode, uint8_t param1, uint16_t param2)
+{
+  byte command[8]; // 1 for type, 1 for length, 1 for opcode, 1 for param1, 2 for param2, 2 for crc
+
+  command[0] = 0x03;
+  command[1] = sizeof(command) - 1;
+  command[2] = opcode;
+  command[3] = param1;
+  memcpy(&command[4], &param2, sizeof(param2));
+
+  uint16_t crc = crc16(&command[1], sizeof(command) - 3);
+  memcpy(&command[6], &crc, sizeof(crc));
+
+  if (_wire->sendTo(_address, command, sizeof(command)) != 0) {
+    return 0;
+  }
+
+  return 1;
+}
+
 int ECC508Class::receiveResponse(void* response, size_t length)
 {
   int retries = 20;
@@ -148,15 +154,49 @@ int ECC508Class::receiveResponse(void* response, size_t length)
 
   while (_wire->requestFrom(_address, responseBuffer, responseSize) != responseSize && retries--);
 
+  // make sure length matches
   if (responseBuffer[0] != responseSize) {
     return 0;
   }
 
-  // TODO: verify CRC
+  // verify CRC
+  uint16_t responseCrc = responseBuffer[length + 1] | (responseBuffer[length + 2] << 8);
+  if (responseCrc != crc16(responseBuffer, responseSize - 2)) {
+    return 0;
+  }
 
   memcpy(response, &responseBuffer[1], length);
 
   return 1;
 }
 
+uint16_t ECC508Class::crc16(byte data[], size_t length)
+{
+  if (data == NULL || length == 0) {
+    return 0;
+  }
+
+  uint16_t crc = 0;
+
+  while (length) {
+    byte b = *data;
+
+    for (uint8_t shift = 0x01; shift > 0x00; shift <<= 1) {
+      uint8_t dataBit = (b & shift) ? 1 : 0;
+      uint8_t crcBit = crc >> 15;
+
+      crc <<= 1;
+      
+      if (dataBit != crcBit) {
+        crc ^= 0x8005;
+      }
+    }
+
+    length--;
+    data++;
+  }
+
+  return crc;
+}
+
 ECC508Class ECC508(Wire, 0x60);

src/utility/ECC508.h

@@ -21,7 +21,9 @@ class ECC508Class
 
   int version();
 
+  int sendCommand(uint8_t opcode, uint8_t param1, uint16_t param2);
   int receiveResponse(void* response, size_t length);
+  uint16_t crc16(byte data[], size_t length);
 
 private:
   TwoWire* _wire;