victron-esp32-ble-mqtt/src/victron_ble.cpp

157 lines
5.3 KiB
C++

#include "victron_ble.h"
#include <Arduino.h>
#include <string.h>
#include <stdio.h>
#include <mbedtls/aes.h>
// ---------------------------------------------------------------------------
// Victron advertisement manufacturer data layout (after 2-byte company ID):
//
// Byte 0-1: prefix (uint16 LE)
// Byte 2-3: model_id (uint16 LE)
// Byte 4: readout_type
// Byte 5-6: IV / nonce (uint16 LE)
// Byte 7: key check byte (must equal encKey[0])
// Byte 8+: AES-128-CTR encrypted payload
//
// AES-128-CTR: 128-bit little-endian counter, initial value = nonce (uint16)
// Matches Python: Counter.new(128, initial_value=iv, little_endian=True)
// ---------------------------------------------------------------------------
static bool aes128ctrDecrypt(const uint8_t key[16], uint16_t nonce,
const uint8_t* in, uint8_t* out, size_t len) {
// 16-byte little-endian IV — nonce in bytes 0-1, rest zero
uint8_t iv[16] = {0};
iv[0] = nonce & 0xFF;
iv[1] = (nonce >> 8) & 0xFF;
mbedtls_aes_context ctx;
mbedtls_aes_init(&ctx);
if (mbedtls_aes_setkey_enc(&ctx, key, 128) != 0) {
mbedtls_aes_free(&ctx);
return false;
}
size_t nc_off = 0;
uint8_t stream_block[16] = {0};
int ret = mbedtls_aes_crypt_ctr(&ctx, len, &nc_off, iv, stream_block, in, out);
mbedtls_aes_free(&ctx);
return ret == 0;
}
// ---------------------------------------------------------------------------
// BitReader — mirrors Python BitReader exactly, reads fields LSB first
// ---------------------------------------------------------------------------
struct BitReader {
const uint8_t* data;
size_t len;
size_t bitPos;
BitReader(const uint8_t* d, size_t l) : data(d), len(l), bitPos(0) {}
uint32_t readUnsigned(int bits) {
uint32_t result = 0;
for (int i = 0; i < bits; i++) {
size_t byteIdx = bitPos / 8;
size_t bitIdx = bitPos % 8;
if (byteIdx < len) {
result |= (uint32_t)((data[byteIdx] >> bitIdx) & 1) << i;
}
bitPos++;
}
return result;
}
int32_t readSigned(int bits) {
uint32_t raw = readUnsigned(bits);
if (raw & (1u << (bits - 1))) {
raw |= ~((1u << bits) - 1);
}
return (int32_t)raw;
}
};
// ---------------------------------------------------------------------------
// SmartSolar MPPT decrypted payload (matches Python solar_charger.py exactly)
//
// 8 bits: charge_state
// 8 bits: charger_error
// 16 bits: battery_voltage (signed, /100 → V)
// 16 bits: battery_current (signed, /10 → A)
// 16 bits: yield_today (unsigned, x10 → Wh)
// 16 bits: solar_power (unsigned, 1W/bit)
// 9 bits: external_device_load (unsigned, /10 → A, not published)
// ---------------------------------------------------------------------------
MpptData parseVictronMppt(const uint8_t* encKey, const uint8_t* data, size_t len) {
MpptData result = {};
result.valid = false;
// Need at least 9 bytes: 5 header + 2 nonce + 1 key check + 1 encrypted
if (len < 9) return result;
uint16_t nonce = (uint16_t)data[5] | ((uint16_t)data[6] << 8);
uint8_t keyCheck = data[7];
const uint8_t* encrypted = data + 8;
size_t encLen = len - 8;
// Key check: first byte must equal encKey[0]
if (keyCheck != encKey[0]) {
Serial.printf("[BLE] Key check failed: got 0x%02X expected 0x%02X\n",
keyCheck, encKey[0]);
return result;
}
uint8_t decrypted[32] = {0};
if (encLen > sizeof(decrypted)) {
Serial.printf("[BLE] Encrypted payload too long (%u bytes)\n",
(unsigned)encLen);
return result;
}
if (!aes128ctrDecrypt(encKey, nonce, encrypted, decrypted, encLen)) {
Serial.println("[BLE] AES decryption failed");
return result;
}
BitReader reader(decrypted, encLen);
uint32_t chargeState = reader.readUnsigned(8);
uint32_t chargerError = reader.readUnsigned(8);
int32_t batV_raw = reader.readSigned(16);
int32_t batI_raw = reader.readSigned(16);
uint32_t yieldToday = reader.readUnsigned(16);
uint32_t solarPower = reader.readUnsigned(16);
uint32_t extLoad = reader.readUnsigned(9); // 9 bits, 0.1A/bit
result.externalLoad = (extLoad != 0x1FF) ? extLoad / 10.0f : 0.0f;
result.chargeState = (uint8_t)chargeState;
result.errorCode = (uint8_t)chargerError;
result.batteryVoltage = (batV_raw != 0x7FFF) ? batV_raw / 100.0f : 0.0f;
result.batteryCurrent = (batI_raw != 0x7FFF) ? batI_raw / 10.0f : 0.0f;
result.yieldToday = (yieldToday != 0xFFFF) ? yieldToday * 10.0f : 0.0f;
result.pvPower = (solarPower != 0xFFFF) ? (float)solarPower : 0.0f;
result.pvVoltage = 0.0f;
result.valid = true;
return result;
}
const char* chargeStateName(uint8_t state) {
switch (state) {
case 0: return "off";
case 2: return "fault";
case 3: return "bulk";
case 4: return "absorption";
case 5: return "float";
case 6: return "storage";
case 7: return "equalize";
case 245: return "starting";
case 246: return "repeated_absorption";
case 247: return "recondition";
case 248: return "battery_safe";
case 249: return "active";
case 252: return "external_control";
case 255: return "not_available";
default: return "unknown";
}
}