[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
@@ -1173,11 +1173,12 @@ void launchBTDiscovery(bool overrideDiscovery) {}
 // ** 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 len = hex.length();
-  if (len % 2 || len / 2 > maxLen) return -1;
-  for (int i = 0, j = 0; i < len; i += 2, j++) {
+  int bytesToWrite = min(len / 2, (int)maxLen);
+  if (len % 2) return -1; // Odd length is invalid
+  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) {
@@ -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);
@@ -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