From f28e4d1cd7bdf0a6eda31fe5a0a314d4386ccfaa Mon Sep 17 00:00:00 2001 From: eddy Date: Tue, 4 Aug 2026 22:11:25 +0200 Subject: [PATCH] feat(server): add broker characteristics for client ota registry --- src/main.cpp | 228 ++++++++++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 227 insertions(+), 1 deletion(-) diff --git a/src/main.cpp b/src/main.cpp index f795bb5..a6f3bcf 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -46,6 +46,13 @@ CRGB leds[NUM_LEDS]; #define VERSION_UUID "beb5483e-36e1-4688-b7f5-ea07361f26db" #define OTA_UUID "beb5483e-36e1-4688-b7f5-ea07361f26da" #define ECDH_UUID "beb5483e-36e1-4688-b7f5-ea07361f26dc" +#define CLIENT_REGISTRY_UUID "beb5483e-36e1-4688-b7f5-ea07361f26dd" +#define CLIENT_CONTROL_UUID "beb5483e-36e1-4688-b7f5-ea07361f26de" + +#define BROKER_CMD_UPDATE_AVAILABLE 0x10 +#define BROKER_CMD_CLIENT_READY 0x20 +#define BROKER_PLAIN_SIZE 20 +#define BROKER_PKT_SIZE (4 + 8 + BROKER_PLAIN_SIZE + 16) #define OTA_CHUNK_SIZE 500 #define OTA_CMD_START 0x10 @@ -80,6 +87,20 @@ struct ClientState { }; ClientState clients[MAX_CLIENTS]; +#define BROKER_STATUS_IDLE 0x00 +#define BROKER_STATUS_READY 0x01 +#define BROKER_STATUS_UPDATING 0x02 + +struct RemoteClientInfo { + uint8_t uuid[ESP_UUID_LEN]; + uint16_t version; + uint8_t status; + bool active; +}; +RemoteClientInfo remoteClients[MAX_CLIENTS]; +NimBLECharacteristic *pRegistryChar = nullptr; +NimBLECharacteristic *pControlChar = nullptr; + uint8_t psk[PSK_LEN]; uint8_t deviceUuid[ESP_UUID_LEN]; bool pskProvisioned = false; @@ -152,6 +173,10 @@ class MyServerCallbacks : public NimBLEServerCallbacks { clients[i].ecdhReady = false; memset(clients[i].aesKey, 0, AES_KEY_LEN); clients[i].ecdh.free(); + remoteClients[i].active = false; + remoteClients[i].version = 0; + remoteClients[i].status = BROKER_STATUS_IDLE; + memset(remoteClients[i].uuid, 0, ESP_UUID_LEN); connHandles[i] = 0xFFFF; connectedCount--; ESP_LOGI(TAG, "BLE: disconnected (slot=%d, clients=%d/%d, mac=%s)", @@ -365,6 +390,193 @@ class OtaCharacteristicCallbacks : public NimBLECharacteristicCallbacks { } }; +static void sendBrokerNotify(int clientIdx, uint8_t cmd, const uint8_t *data, size_t dataLen) { + if (clientIdx < 0 || clientIdx >= MAX_CLIENTS || !connActive[clientIdx]) return; + + uint8_t plainPayload[BROKER_PLAIN_SIZE]; + memset(plainPayload, 0, BROKER_PLAIN_SIZE); + plainPayload[0] = cmd; + memcpy(plainPayload + 1, data, dataLen); + + uint8_t pkt[BROKER_PKT_SIZE]; + memset(pkt, 0, BROKER_PKT_SIZE); + uint32_t s = 0; + pkt[0] = (s >> 24) & 0xFF; + pkt[1] = (s >> 16) & 0xFF; + pkt[2] = (s >> 8) & 0xFF; + pkt[3] = s & 0xFF; + + for (int i = 0; i < 8; i++) pkt[4 + i] = (uint8_t)(esp_random() & 0xFF); + + uint8_t nonce[12]; + memcpy(nonce, pkt, 4); + memcpy(nonce + 4, pkt + 4, 8); + + mbedtls_gcm_context gcm; + mbedtls_gcm_init(&gcm); + int ret = mbedtls_gcm_setkey(&gcm, MBEDTLS_CIPHER_ID_AES, clients[clientIdx].aesKey, AES_KEY_LEN * 8); + if (ret != 0) { + ESP_LOGE(TAG, "BROKER: send notify setkey failed"); + mbedtls_gcm_free(&gcm); + return; + } + + mbedtls_gcm_crypt_and_tag(&gcm, MBEDTLS_GCM_ENCRYPT, + BROKER_PLAIN_SIZE, nonce, 12, NULL, 0, + plainPayload, pkt + 12, 16, pkt + 12 + BROKER_PLAIN_SIZE); + mbedtls_gcm_free(&gcm); + + pControlChar->setValue(pkt, BROKER_PKT_SIZE); + pControlChar->notify(connHandles[clientIdx], true); +} + +class RegistryCharacteristicCallbacks : public NimBLECharacteristicCallbacks { + void onWrite(NimBLECharacteristic *pChar, NimBLEConnInfo &connInfo) override { + if (otaInProgress) return; + + std::string value = pChar->getValue(); + if (value.empty()) return; + const uint8_t *data = (const uint8_t *)value.data(); + size_t len = value.length(); + + if (len != BROKER_PKT_SIZE) { + ESP_LOGW(TAG, "REGISTRY: bad packet length %d (expected %d)", (int)len, BROKER_PKT_SIZE); + return; + } + + int idx = findClientIdx(connInfo.getConnHandle()); + if (idx < 0 || !clients[idx].ecdhReady) { + ESP_LOGW(TAG, "REGISTRY: write rejected — no ECDH (handle=%d)", connInfo.getConnHandle()); + return; + } + + uint8_t nonce[12]; + memcpy(nonce, data, 4); + memcpy(nonce + 4, data + 4, 8); + + uint8_t plainPayload[BROKER_PLAIN_SIZE]; + uint8_t ciphertext[BROKER_PLAIN_SIZE]; + memcpy(ciphertext, data + 12, BROKER_PLAIN_SIZE); + + mbedtls_gcm_context gcm; + mbedtls_gcm_init(&gcm); + int ret = mbedtls_gcm_setkey(&gcm, MBEDTLS_CIPHER_ID_AES, clients[idx].aesKey, AES_KEY_LEN * 8); + if (ret != 0) { + ESP_LOGE(TAG, "REGISTRY: setkey failed (ret=%d)", ret); + mbedtls_gcm_free(&gcm); + return; + } + + ret = mbedtls_gcm_auth_decrypt(&gcm, + BROKER_PLAIN_SIZE, nonce, 12, NULL, 0, + data + 12 + BROKER_PLAIN_SIZE, 16, ciphertext, plainPayload); + mbedtls_gcm_free(&gcm); + + if (ret != 0) { + ESP_LOGE(TAG, "REGISTRY: AES-GCM decrypt failed (ret=%d)", ret); + return; + } + + uint8_t cmd = plainPayload[0]; + if (cmd != 0x00) { + ESP_LOGW(TAG, "REGISTRY: unexpected cmd 0x%02X (expected 0x00)", cmd); + return; + } + + memcpy(remoteClients[idx].uuid, plainPayload + 1, ESP_UUID_LEN); + remoteClients[idx].version = ((uint16_t)plainPayload[1 + ESP_UUID_LEN] << 8) | + (uint16_t)plainPayload[1 + ESP_UUID_LEN + 1]; + remoteClients[idx].status = BROKER_STATUS_IDLE; + remoteClients[idx].active = true; + + ESP_LOGI(TAG, "REGISTRY: client registered (slot=%d, uuid=%02X%02X%02X%02X, ver=%d)", + idx, + remoteClients[idx].uuid[0], remoteClients[idx].uuid[1], + remoteClients[idx].uuid[2], remoteClients[idx].uuid[3], + remoteClients[idx].version); + } +}; + +class ControlCharacteristicCallbacks : public NimBLECharacteristicCallbacks { + void onWrite(NimBLECharacteristic *pChar, NimBLEConnInfo &connInfo) override { + if (otaInProgress) return; + + std::string value = pChar->getValue(); + if (value.empty()) return; + const uint8_t *data = (const uint8_t *)value.data(); + size_t len = value.length(); + + if (len != BROKER_PKT_SIZE) { + ESP_LOGW(TAG, "BROKER: bad packet length %d (expected %d)", (int)len, BROKER_PKT_SIZE); + return; + } + + int idx = findClientIdx(connInfo.getConnHandle()); + if (idx < 0 || !clients[idx].ecdhReady) { + ESP_LOGW(TAG, "BROKER: write rejected — no ECDH (handle=%d)", connInfo.getConnHandle()); + return; + } + + uint8_t nonce[12]; + memcpy(nonce, data, 4); + memcpy(nonce + 4, data + 4, 8); + + uint8_t plainPayload[BROKER_PLAIN_SIZE]; + uint8_t ciphertext[BROKER_PLAIN_SIZE]; + memcpy(ciphertext, data + 12, BROKER_PLAIN_SIZE); + + mbedtls_gcm_context gcm; + mbedtls_gcm_init(&gcm); + int ret = mbedtls_gcm_setkey(&gcm, MBEDTLS_CIPHER_ID_AES, clients[idx].aesKey, AES_KEY_LEN * 8); + if (ret != 0) { + ESP_LOGE(TAG, "BROKER: setkey failed (ret=%d)", ret); + mbedtls_gcm_free(&gcm); + return; + } + + ret = mbedtls_gcm_auth_decrypt(&gcm, + BROKER_PLAIN_SIZE, nonce, 12, NULL, 0, + data + 12 + BROKER_PLAIN_SIZE, 16, ciphertext, plainPayload); + mbedtls_gcm_free(&gcm); + + if (ret != 0) { + ESP_LOGE(TAG, "BROKER: AES-GCM decrypt failed (ret=%d)", ret); + return; + } + + uint8_t cmd = plainPayload[0]; + ESP_LOGI(TAG, "BROKER: received cmd 0x%02X from slot %d", cmd, idx); + + if (cmd == BROKER_CMD_UPDATE_AVAILABLE) { + uint8_t targetUuid[ESP_UUID_LEN]; + memcpy(targetUuid, plainPayload + 1, ESP_UUID_LEN); + + for (int i = 0; i < MAX_CLIENTS; i++) { + if (remoteClients[i].active && + memcmp(remoteClients[i].uuid, targetUuid, ESP_UUID_LEN) == 0) { + ESP_LOGI(TAG, "BROKER: forwarding update notification to client slot %d", i); + sendBrokerNotify(i, BROKER_CMD_UPDATE_AVAILABLE, targetUuid, ESP_UUID_LEN); + break; + } + } + } else if (cmd == BROKER_CMD_CLIENT_READY) { + uint8_t clientUuid[ESP_UUID_LEN]; + memcpy(clientUuid, plainPayload + 1, ESP_UUID_LEN); + + ESP_LOGI(TAG, "BROKER: client ready for OTA"); + for (int i = 0; i < MAX_CLIENTS; i++) { + if (remoteClients[i].active && + memcmp(remoteClients[i].uuid, clientUuid, ESP_UUID_LEN) == 0) { + remoteClients[i].status = BROKER_STATUS_READY; + break; + } + } + } else { + ESP_LOGW(TAG, "BROKER: unknown cmd 0x%02X", cmd); + } + } +}; + class EcdhCharacteristicCallbacks : public NimBLECharacteristicCallbacks { void onWrite(NimBLECharacteristic *pChar, NimBLEConnInfo &connInfo) override { if (otaInProgress) return; @@ -500,6 +712,10 @@ void setup() { for (int i = 0; i < MAX_CLIENTS; i++) { clients[i].ecdhReady = false; memset(clients[i].aesKey, 0, AES_KEY_LEN); + remoteClients[i].active = false; + remoteClients[i].version = 0; + remoteClients[i].status = BROKER_STATUS_IDLE; + memset(remoteClients[i].uuid, 0, ESP_UUID_LEN); } pinMode(TOUCH1_PIN, INPUT_PULLUP); @@ -549,7 +765,17 @@ void setup() { ); pOtaChar->setCallbacks(new OtaCharacteristicCallbacks()); - pService->start(); + pRegistryChar = pService->createCharacteristic( + CLIENT_REGISTRY_UUID, + NIMBLE_PROPERTY::READ | NIMBLE_PROPERTY::WRITE_NR | NIMBLE_PROPERTY::NOTIFY + ); + pRegistryChar->setCallbacks(new RegistryCharacteristicCallbacks()); + + pControlChar = pService->createCharacteristic( + CLIENT_CONTROL_UUID, + NIMBLE_PROPERTY::WRITE_NR | NIMBLE_PROPERTY::NOTIFY + ); + pControlChar->setCallbacks(new ControlCharacteristicCallbacks()); NimBLEAdvertising *pAdvertising = NimBLEDevice::getAdvertising(); pAdvertising->setName("SylDa_Server");