[BT] Victron decryption adjustments (#2228)

main/gatewayBT.cpp

@@ -58,8 +58,8 @@ BTConfig_s BTConfig;
 #  if BLEDecoder
 #    include <decoder.h>
 #    if BLEDecryptor
-#      include "mbedtls/ccm.h"
 #      include "mbedtls/aes.h"
+#      include "mbedtls/ccm.h"
 #    endif
 TheengsDecoder decoder;
 #  endif
@@ -1171,16 +1171,17 @@ void launchBTDiscovery(bool overrideDiscovery) {}
 
 #  if BLEDecryptor
 // ** TODO - Hex string to bytes, there is probably a function for this already just need to find it
-int hexToBytes(String hex, uint8_t *out, size_t maxLen) {
+int hexToBytes(String hex, uint8_t* out, size_t maxLen) {
   int len = hex.length();
-  if (len % 2 || len / 2 > maxLen) return -1;
+  int bytesToWrite = min(len / 2, (int)maxLen);
-  for (int i = 0, j = 0; i < len; i += 2, j++) {
+  if (len % 2) return -1; // Odd length is invalid
-    out[j] = (uint8_t) strtol(hex.substring(i, i + 2).c_str(), nullptr, 16);
+  for (int i = 0, j = 0; j < bytesToWrite; i += 2, j++) {
+    out[j] = (uint8_t)strtol(hex.substring(i, i + 2).c_str(), nullptr, 16);
   }
-  return len / 2;
+  return bytesToWrite;
 }
 // Reverse bytes
-void reverseBytes(uint8_t *data, size_t length) {
+void reverseBytes(uint8_t* data, size_t length) {
   size_t i;
   for (i = 0; i < length / 2; i++) {
     uint8_t temp = data[i];
@@ -1220,7 +1221,7 @@ void process_bledata(JsonObject& BLEdata) {
     // AES decryption key
     unsigned char bleaeskey[16];
     int bleaeskeylength = 0;
-    if (ble_aes_keys.containsKey(macWOdots)){
+    if (ble_aes_keys.containsKey(macWOdots)) {
       Log.trace(F("[BLEDecryptor] Custom AES key %s" CR), ble_aes_keys[macWOdots].as<const char*>());
       bleaeskeylength = hexToBytes(ble_aes_keys[macWOdots], bleaeskey, 16);
     } else {
@@ -1239,7 +1240,7 @@ void process_bledata(JsonObject& BLEdata) {
     unsigned char aad[1];
     int aadLength;
 
-    if (BLEdata["encr"].as<int>() == 1){        // PVVX Encrypted
+    if (BLEdata["encr"].as<int>() == 1) { // PVVX Encrypted
       noncelength = 11; // 11 bytes
       reverseBytes(macAddress, 6); // 6 bytes: device address in reverse
       memcpy(nonce, macAddress, 6);
@@ -1262,7 +1263,7 @@ void process_bledata(JsonObject& BLEdata) {
       aadLength = 1;
       Log.trace(F("[BLEDecryptor] PVVX nonce %s" CR), NimBLEUtils::dataToHexString(nonce, noncelength).c_str());
 
-    } else if (BLEdata["encr"].as<int>() == 2){         // BTHome V2 Encrypted
+    } else if (BLEdata["encr"].as<int>() == 2) { // BTHome V2 Encrypted
       noncelength = 13; // 13 bytes
       memcpy(nonce, macAddress, 6);
       nonce[6] = 0xD2; // UUID
@@ -1279,7 +1280,7 @@ void process_bledata(JsonObject& BLEdata) {
       aadLength = 0;
       Log.trace(F("[BLEDecryptor] BTHomeV2 nonce %s" CR), NimBLEUtils::dataToHexString(nonce, noncelength).c_str());
 
-    } else if (BLEdata["encr"].as<int>() == 3){
+    } else if (BLEdata["encr"].as<int>() == 3) {
       nonce[16] = {0}; // Victron has a 16 byte zero padded nonce with IV bytes 6,7
       unsigned char iv[2];
       int ivlen = hexToBytes(BLEdata["ctr"].as<String>(), iv, 2);
@@ -1288,16 +1289,18 @@ void process_bledata(JsonObject& BLEdata) {
         return;
       }
       memcpy(nonce, iv, 2);
+      memset(nonce + 2, 0, 14); // 14 bytes: zero padding
       Log.trace(F("[BLEDecryptor] Victron nonce %s" CR), NimBLEUtils::dataToHexString(nonce, 16).c_str());
     } else {
       return; // No match
     }
 
     // Ciphertext to bytes
-    int cipherlen = sizeof(BLEdata["cipher"].as<String>());
+    String cipherHex = BLEdata["cipher"].as<String>();
+    int cipherlen = cipherHex.length() / 2; // Number of bytes in ciphertext
     unsigned char ciphertext[cipherlen];
     int ciphertextlen = hexToBytes(BLEdata["cipher"].as<String>(), ciphertext, cipherlen);
-    unsigned char decrypted[ciphertextlen];      // Decrypted payload
+    unsigned char decrypted[cipherlen];
 
     // Decrypt ciphertext
     if (BLEdata["encr"].as<int>() == 1 || BLEdata["encr"].as<int>() == 2) {
@@ -1331,7 +1334,11 @@ void process_bledata(JsonObject& BLEdata) {
       if (ret == 0) {
         Log.notice(F("[BLEDecryptor] Decryption successful" CR));
       } else if (ret == MBEDTLS_ERR_CCM_AUTH_FAILED) {
-          Log.error(F("[BLEDecryptor] Authentication failed." CR));
+        if (ble_aes_keys.containsKey(macWOdots)) {
+          Log.error(F("[BLEDecryptor] Decryption failed for %s with key %s" CR), macWOdots.c_str(), ble_aes_keys[macWOdots].as<const char*>());
+        } else {
+          Log.error(F("[BLEDecryptor] Decryption failed for %s with default key" CR), macWOdots.c_str());
+        }
         return;
       } else {
         Log.error(F("[BLEDecryptor] Decryption failed with error: %X" CR), ret);
@@ -1339,7 +1346,7 @@ void process_bledata(JsonObject& BLEdata) {
       }
 
       // Build new servicedata
-      if (BLEdata["encr"].as<int>() == 1){            // PVVX
+      if (BLEdata["encr"].as<int>() == 1) { // PVVX
         BLEdata["servicedata"] = NimBLEUtils::dataToHexString(decrypted, ciphertextlen);
       } else if (BLEdata["encr"].as<int>() == 2) { // BTHomeV2
         // Build new servicedata
@@ -1389,7 +1396,7 @@ void process_bledata(JsonObject& BLEdata) {
       manufacturerdata[2] = 0x11; // Replace byte 2 with "11" indicate decrypted data
       manufacturerdata[7] = 0xff; // Replace byte 7 with "ff" to indicate decrypted data
       manufacturerdata[8] = 0xff; // Replace byte 8 with "ff" to indicate decrypted data
-      memcpy(&manufacturerdata[8], decrypted, ciphertextlen);  // Append the decrypted payload to the manufacturer data
+      memcpy(&manufacturerdata[10], decrypted, ciphertextlen); // Append the decrypted payload to the manufacturer data
       BLEdata["manufacturerdata"] = NimBLEUtils::dataToHexString(manufacturerdata, 10 + ciphertextlen); // Rebuild manufacturerdata
       Log.trace(F("[BLEDecryptor] Victron decrypted manufacturerdata %s" CR), BLEdata["manufacturerdata"].as<const char*>());
     }
@@ -1405,7 +1412,6 @@ void process_bledata(JsonObject& BLEdata) {
     BLEdata.remove("cipher");
     BLEdata.remove("ctr");
     BLEdata.remove("mic");
-
   }
 #    endif