[BLE] Add support for decrypting PVVX, BTHome v2 and Victron BLE frames

docs/use/webui.md

@@ -30,6 +30,10 @@ Ability to change the mqtt settings, if the change is unsuccessful it will rever
 
 Ability to change the display of sensor to Metric or Imperial, and disable the WebUI Authentication
 
+## Bluetooth Low Energy - BLE
+
+Ability to add the default AES BLE decryption key, and multiple per-device `macaddress:aeskey` allowing for devices that cannot have their AES key changed.
+
 ## Logging
 
 Ability to temporarily change the logging level.

main/TheengsCommon.h

@@ -63,6 +63,11 @@ extern char mqtt_topic[];
 extern char gateway_name[];
 extern unsigned long lastDiscovery; // Time of the last discovery to trigger automaticaly to off after DiscoveryAutoOffTimer
 
+#if BLEDecryptor
+extern char ble_aes[];
+extern StaticJsonDocument<JSON_BLE_AES_CUSTOM_KEYS> ble_aes_keys;
+#endif
+
 extern bool enqueueJsonObject(const StaticJsonDocument<JSON_MSG_BUFFER>& jsonDoc, int timeout);
 extern bool enqueueJsonObject(const StaticJsonDocument<JSON_MSG_BUFFER>& jsonDoc);
 extern void buildTopicFromId(JsonObject& Jsondata, const char* origin);

main/User_config.h

@@ -652,6 +652,18 @@ bool isAduplicateSignal(uint64_t);
 void storeSignalValue(uint64_t);
 #endif
 
+#ifdef ZgatewayBT
+#  ifndef BLEDecryptor
+#    define BLEDecryptor true //true if decrypt encrypted PVVX or BTHome v2 service data
+#  endif
+#  ifndef JSON_BLE_AES_CUSTOM_KEYS
+#    define JSON_BLE_AES_CUSTOM_KEYS 256 // 42 byte BLE Custom Key * 6 rounded up to 256.
+#  endif
+#  ifndef BLE_AES
+#    define BLE_AES "00112233445566778899001122334455"
+#  endif
+#endif
+
 #define convertTemp_CtoF(c) ((c * 1.8) + 32)
 #define convertTemp_FtoC(f) ((f - 32) * 5 / 9)
 

main/config_WebContent.h

@@ -62,7 +62,11 @@
 #else
 #  define configure_6
 #endif
-#define configure_7
+#ifdef BLEDecryptor
+#  define configure_7 "<p><form action='bl' method='post'><button>Configure BLE</button></form></p>"
+#else
+#  define configure_7
+#endif
 #define configure_8
 
 /*------------------- ----------------------*/
@@ -219,6 +223,15 @@ const char config_lora_body[] = body_header
     "</form>"
     "</fieldset>" body_footer_config_menu;
 
+#ifdef BLEDecryptor
+const char config_ble_body[] = body_header "<fieldset class=\"set1\"><legend><span><b>Configure BLE</b></span></legend><form method='post' action='bl'><p><b>BLE AES Default Key (32 char hex)</b></p><input id='bk' name='bk' minlength='32' maxlength='32' placeholder=" BLE_AES " value='%s' oninput='bkv()'><p id='bke' style='color:red;'></p><hr><p><b>BLE Key Pairs</b></p><p>MacAddress:AESKey with space separator</p><p><textarea id='kp' name='kp' placeholder='A4C138012345:00112233445566778899001122334455' rows='3' cols='46' oninput='kpv()'>%s</textarea></p><p id='kpe' style='color:red;'></p><br><button id='s' name='save' type='submit' class='button bgrn'>Save</button></form></fieldset>" body_footer_config_menu;
+
+// Client side javascript validation of the Default AES Key is hex, and the custom keys are in the correct format. This pushes the theengs-bridge-v11 way of the file size, so reudcing it now and leaving it commented out
+//const char ble_script[] = "function bkv(){let e=document.getElementById('bk'),t=document.getElementById('bke'),l=e.value.trim();0===l.length||/^[A-Fa-f0-9]{32}$/.test(l)?(t.textContent='',e.style.color=''):(t.textContent='Invalid key',e.style.color='red')}function kpv(){let e=document.getElementById('kp'),t=document.getElementById('kpe'),l=e.value.split(/ /),n=/^[A-Fa-f0-9]{12}:[A-Fa-f0-9]{32}$/,o=l.map((e,t)=>({line:e,index:t})).filter(e=>!n.test(e.line));if(0===e.value.trim().length||0===o.length)t.textContent='',e.style.color='';else{let i=o.map(e=>e.index+1).join(', ');t.textContent=`Invalid format on line(s): ${i}`,e.style.color='red'}}";
+
+const char ble_script[] = "";
+#endif
+
 const char footer[] = "<div style='text-align:right;font-size:11px;'><hr/><a href='https://community.openmqttgateway.com' target='_blank' style='color:#aaa;'>%s</a></div></div></body></html>";
 
 // Source file - https://github.com/1technophile/OpenMQTTGateway/blob/54decb4b65c7894b926ac3a89de0c6b2a3021506/docs/.vuepress/public/favicon-16x16.png

main/gatewayBT.cpp

@@ -57,6 +57,10 @@ BTConfig_s BTConfig;
 
 #  if BLEDecoder
 #    include <decoder.h>
+#    if BLEDecryptor
+#      include "mbedtls/ccm.h"
+#      include "mbedtls/aes.h"
+#    endif
 TheengsDecoder decoder;
 #  endif
 
@@ -1154,6 +1158,27 @@ void launchBTDiscovery(bool overrideDiscovery) {
 void launchBTDiscovery(bool overrideDiscovery) {}
 #  endif
 
+#  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 len = hex.length();
+  if (len % 2 || len / 2 > maxLen) return -1;
+  for (int i = 0, j = 0; i < len; i += 2, j++) {
+    out[j] = (uint8_t) strtol(hex.substring(i, i + 2).c_str(), nullptr, 16);
+  }
+  return len / 2;
+}
+// Reverse bytes
+void reverseBytes(uint8_t *data, size_t length) {
+    size_t i;
+    for (i = 0; i < length / 2; i++) {
+        uint8_t temp = data[i];
+        data[i] = data[length - 1 - i];
+        data[length - 1 - i] = temp;
+    }
+}
+#  endif
+
 #  if BLEDecoder
 void process_bledata(JsonObject& BLEdata) {
   yield(); // Necessary to let the loop run in case of connectivity issues
@@ -1166,6 +1191,213 @@ void process_bledata(JsonObject& BLEdata) {
   int model_id = BTConfig.extDecoderEnable ? -1 : decoder.decodeBLEJson(BLEdata);
   int mac_type = BLEdata["mac_type"].as<int>();
 
+# if BLEDecryptor
+  if (BLEdata["encr"] && (BLEdata["encr"].as<int>() >0 && BLEdata["encr"].as<int>() <=2)) {
+    // Decrypting Encrypted BLE Data PVVX, BTHome or Victron
+    Log.trace(F("[BLEDecryptor] Decrypt ENCR:%d ModelID:%s Payload:%s" CR), BLEdata["encr"].as<int>(), BLEdata["model_id"].as<const char*>(), BLEdata["cipher"].as<const char*>());
+
+    // MAC address
+    String macWOdots = BLEdata["id"].as<String>();   // Mac Address without dots
+    macWOdots.replace(":", "");
+    unsigned char macAddress[6];
+    int maclen = hexToBytes(macWOdots, macAddress, 6);
+    if (maclen != 6) {
+      Log.error(F("[BLEDecryptor] Invalid MAC Address length %d" CR), maclen);
+      return;
+    }
+
+    // AES decryption key
+    unsigned char bleaeskey[16];
+    int bleaeskeylength = 0;
+    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 {
+      Log.trace(F("[BLEDecryptor] Default AES key" CR));
+      bleaeskeylength = hexToBytes(ble_aes, bleaeskey, 16);
+    }
+    // Check AES Key
+    if (bleaeskeylength != 16) {
+      Log.error(F("[BLEDecryptor] Invalid key length %d" CR), bleaeskeylength);
+      return;
+    }
+
+    // Build nonce and aad
+    uint8_t nonce[16];
+    int noncelength = 0;
+    unsigned char aad[1];
+    int aadLength;
+
+    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);
+      int maclen = hexToBytes(macWOdots, macAddress, 6);
+
+      unsigned char servicedata[16];
+      int servicedatalen = hexToBytes(BLEdata["servicedata"].as<String>(), servicedata, 16);
+      nonce[6] = servicedatalen + 3;            // 1 byte : length of (service data + type and UUID)
+      nonce[7] = 0x16;                          // 1 byte : "16" -> AD type for "Service Data - 16-bit UUID"
+      nonce[8] = 0x1A;                          // 2 bytes: "1a18" -> UUID 181a in little-endian
+      nonce[9] = 0x18;                          //
+      unsigned char ctr[1];                     // 1 byte : counter
+      int ctrlen = hexToBytes(BLEdata["ctr"].as<String>(), ctr, 1);
+      if (ctrlen != 1) {
+        Log.error(F("[BLEDecryptor] Invalid counter length %d" CR), ctrlen);
+        return;
+      }
+      nonce[10] = ctr[0];
+      aad[0] = 0x11;
+      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
+      noncelength = 13;                                 // 13 bytes
+      memcpy(nonce, macAddress, 6);
+      nonce[6] = 0xD2;                                  // UUID
+      nonce[7] = 0xFC;
+      nonce[8] = 0x41;                                  // BTHome Device Data encrypted payload byte
+      unsigned char ctr[4];                             // Counter
+      int ctrlen = hexToBytes(BLEdata["ctr"].as<String>(), ctr, 4);
+      if (ctrlen != 4) {
+        Log.error(F("[BLEDecryptor] Invalid counter length %d" CR), ctrlen);
+        return;
+      }
+      memcpy(&nonce[9], ctr, 4);
+      aad[0] = 0x00;
+      aadLength = 0;
+      Log.trace(F("[BLEDecryptor] BTHomeV2 nonce %s" CR), NimBLEUtils::dataToHexString(nonce, noncelength).c_str());
+
+    } 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);
+      if (ivlen != 2) {
+        Log.error(F("[BLEDecryptor] Invalid iv length %d" CR), ivlen);
+        return;
+      }
+      memcpy(nonce, iv, 2);
+      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>());
+    unsigned char ciphertext[cipherlen];
+    int ciphertextlen = hexToBytes(BLEdata["cipher"].as<String>(), ciphertext, cipherlen);
+    unsigned char decrypted[ciphertextlen];      // Decrypted payload
+
+    // Decrypt ciphertext
+    if (BLEdata["encr"].as<int>() == 1 || BLEdata["encr"].as<int>() == 2) {
+      // Decrypt PVVX and BTHome V2 ciphertext using AES CCM
+      mbedtls_ccm_context ctx;
+      mbedtls_ccm_init(&ctx);
+      if (mbedtls_ccm_setkey(&ctx, MBEDTLS_CIPHER_ID_AES, bleaeskey, 128) != 0) {
+          Log.error(F("[BLEDecryptor] Failed to set AES key to mbedtls" CR));
+          return;
+      }    
+
+      // Message Integrity Check (MIC)
+      unsigned char mic[4];
+      int miclen = hexToBytes(BLEdata["mic"].as<String>(), mic, 4);
+      if (miclen != 4) {
+        Log.error(F("[BLEDecryptor] Invalid MIC length %d" CR), miclen);
+        return;
+      }
+
+      int ret = mbedtls_ccm_auth_decrypt(
+          &ctx,                   // AES Key
+          ciphertextlen,          // length of ciphertext
+          nonce, noncelength,     // Nonce
+          aad, aadLength,         // AAD
+          ciphertext,             // input ciphertext
+          decrypted,              // output plaintext
+          mic, sizeof(mic)        // Message Integrity Check
+      );
+      mbedtls_ccm_free(&ctx);
+
+      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));
+          return;
+      } else {
+          Log.error(F("[BLEDecryptor] Decryption failed with error: %X" CR), ret);
+          return;
+      }
+
+      // Build new servicedata
+      if (BLEdata["encr"].as<int>() == 1){            // PVVX
+        BLEdata["servicedata"] = NimBLEUtils::dataToHexString(decrypted, ciphertextlen);
+      } else if (BLEdata["encr"].as<int>() == 2) {    // BTHomeV2
+        // Build new servicedata
+        uint8_t newservicedata[3 + ciphertextlen];
+        newservicedata[0] = 0x40;      // Decrypted BTHomeV2 Packet Type
+        newservicedata[1] = 0x00;      // Packet counter which the PVVX BTHome non-encrypted has but the encrypted does not
+        newservicedata[2] = 0x00;      // **TODO Convert the ctr to the packet counter or just stick with 0?
+        memcpy(&newservicedata[3], decrypted, ciphertextlen);
+        BLEdata["servicedata"] = NimBLEUtils::dataToHexString(newservicedata, ciphertextlen + 3);
+      } else {
+        return;
+      }
+      Log.trace(F("[BLEDecryptor] Decrypted servicedata %s" CR), BLEdata["servicedata"].as<const char*>());
+
+    } else if (BLEdata["encr"].as<int>() == 3) {
+      // Decrypt Victron Energy encrypted advertisements.
+      size_t nc_off = 0;
+      uint8_t stream_block[16] = {0};
+
+      mbedtls_aes_context ctx;
+      mbedtls_aes_init(&ctx);
+      mbedtls_aes_setkey_enc(&ctx, bleaeskey, 128);
+      int ret = mbedtls_aes_crypt_ctr(
+        &ctx,                   // AES Key
+        ciphertextlen,          // length of ciphertext
+        &nc_off,
+        nonce,                  // 16 byte nonce with 2 bytes iv
+        stream_block,
+        ciphertext,             // input ciphertext
+        decrypted               // output plaintext
+      );
+      mbedtls_aes_free(&ctx);
+
+      if (ret == 0) {
+        Log.notice(F("[BLEDecryptor] Victron Decryption successful" CR));
+      } else if (ret == MBEDTLS_ERR_CCM_AUTH_FAILED) {
+          Log.error(F("[BLEDecryptor] Victron Authentication failed." CR));
+          return;
+      } else {
+          Log.error(F("[BLEDecryptor] Victron decryption failed with error: %X" CR), ret);
+          return;
+      }
+
+      // Build new manufacturerdata
+      unsigned char manufacturerdata[10 + ciphertextlen];
+      int manufacturerdatalen = hexToBytes(BLEdata["manufacturerdata"].as<String>(), manufacturerdata, 10);
+      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
+      BLEdata["manufacturerdata"] = NimBLEUtils::dataToHexString(manufacturerdata, 10 + ciphertextlen);  // Rebuild manufacturerdata
+      Log.trace(F("[BLEDecryptor] Victron decrypted manufacturerdata %s" CR), BLEdata["manufacturerdata"].as<const char*>());
+    }
+
+    // Print before and after decoder post decryption
+    // serializeJsonPretty(BLEdata, Serial);
+    model_id = BTConfig.extDecoderEnable ? -1 : decoder.decodeBLEJson(BLEdata);
+    // serializeJsonPretty(BLEdata, Serial);
+    Log.trace(F("[BLEDecryptor] Decrypted model_id %d" CR), model_id);    
+
+    // Remove the cipher fields from BLEdata
+    BLEdata.remove("encr");
+    BLEdata.remove("cipher");
+    BLEdata.remove("ctr");
+    BLEdata.remove("mic");
+
+  }
+#  endif
+
   // Convert prmacs to RMACS until or if OMG gets Identity MAC/IRK decoding
   if (BLEdata["prmac"]) {
     BLEdata.remove("prmac");

main/main.cpp

@@ -94,6 +94,12 @@ bool ethConnected = false;
 char mqtt_topic[parameters_size + 1] = Base_Topic;
 char gateway_name[parameters_size + 1] = Gateway_Name;
 unsigned long lastDiscovery = 0;
+
+#ifdef BLEDecryptor
+  char ble_aes[parameters_size] = BLE_AES;
+  StaticJsonDocument<JSON_BLE_AES_CUSTOM_KEYS> ble_aes_keys;
+#endif
+
 #if !MQTT_BROKER_MODE
 ss_cnt_parameters cnt_parameters_array[cnt_parameters_array_size] = CNT_PARAMS_ARR;
 #endif
@@ -2022,6 +2028,10 @@ void saveConfig() {
 #  endif
   json["gateway_name"] = gateway_name;
   json["ota_pass"] = ota_pass;
+#  ifdef BLEDecryptor
+  json["ble_aes"] = ble_aes;
+  json["ble_aes_keys"] = ble_aes_keys;
+#  endif
 
   File configFile = SPIFFS.open("/config.json", "w");
   if (!configFile) {
@@ -2169,6 +2179,16 @@ bool loadConfigFromFlash() {
           }
 #  endif
         }
+#  ifdef BLEDecryptor
+        if (json.containsKey("ble_aes")) {
+          strcpy(ble_aes, json["ble_aes"]);
+          Log.trace(F("loaded default BLE AES key %s" CR), ble_aes);
+        }        
+        if (json.containsKey("ble_aes_keys")) {
+          ble_aes_keys = json["ble_aes_keys"];
+          Log.trace(F("loaded %d custom BLE AES keys" CR), ble_aes_keys.size());
+        }        
+#  endif
         result = true;
       } else {
         Log.warning(F("failed to load json config" CR));
@@ -3441,6 +3461,25 @@ void XtoSYS(const char* topicOri, JsonObject& SYSdata) { // json object decoding
       mqttSetupPending = true; // trigger reconnect in loop using the new topic/name
     }
 
+#ifdef BLEDecryptor
+    if (SYSdata.containsKey("ble_aes") || SYSdata.containsKey("ble_aes_keys")) {
+      if (SYSdata.containsKey("ble_aes")) {
+        strncpy(ble_aes, SYSdata["ble_aes"], parameters_size);
+      }
+      if (SYSdata.containsKey("ble_aes_keys")) {
+        Log.warning(F("Contains ble_aes_keys" CR));
+        ble_aes_keys = SYSdata["ble_aes_keys"];
+      } else {
+        // If no keys are passed clear the object.
+        StaticJsonDocument<JSON_BLE_AES_CUSTOM_KEYS> jsonBLEBuffer;
+        ble_aes_keys = jsonBLEBuffer.to<JsonObject>();
+      }
+#  ifndef ESPWifiManualSetup
+      saveConfig();
+#  endif
+    }
+#endif
+
 #if !MQTT_BROKER_MODE
 #  ifdef MQTTsetMQTT