From 537449e6af5895bd9aa107ddd416f1b3fbb216d9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Jo=C3=A3o=20Br=C3=A1zio?= Date: Sun, 7 Sep 2025 22:00:06 +0100 Subject: [PATCH] Refactor ESPNowBridge packet handling to use 2-byte magic header and improve packet size validation --- src/helpers/bridges/ESPNowBridge.cpp | 88 ++++++++++++++++++++-------- src/helpers/bridges/ESPNowBridge.h | 33 +++++++---- src/helpers/bridges/RS232Bridge.h | 6 +- 3 files changed, 89 insertions(+), 38 deletions(-) diff --git a/src/helpers/bridges/ESPNowBridge.cpp b/src/helpers/bridges/ESPNowBridge.cpp index a470d521..aed63a6b 100644 --- a/src/helpers/bridges/ESPNowBridge.cpp +++ b/src/helpers/bridges/ESPNowBridge.cpp @@ -10,7 +10,7 @@ ESPNowBridge *ESPNowBridge::_instance = nullptr; // Static callback wrappers -void ESPNowBridge::recv_cb(const uint8_t *mac, const uint8_t *data, int len) { +void ESPNowBridge::recv_cb(const uint8_t *mac, const uint8_t *data, int32_t len) { if (_instance) { _instance->onDataRecv(mac, data, len); } @@ -78,33 +78,44 @@ void ESPNowBridge::xorCrypt(uint8_t *data, size_t len) { } } -void ESPNowBridge::onDataRecv(const uint8_t *mac, const uint8_t *data, int len) { - // Ignore packets that are too small - if (len < 3) { +void ESPNowBridge::onDataRecv(const uint8_t *mac, const uint8_t *data, int32_t len) { + // Ignore packets that are too small to contain header + checksum + if (len < (MAGIC_HEADER_SIZE + CHECKSUM_SIZE)) { #if MESH_PACKET_LOGGING Serial.printf("%s: ESPNOW BRIDGE: RX packet too small, len=%d\n", getLogDateTime(), len); #endif return; } - // Check packet header magic - if (data[0] != ESPNOW_HEADER_MAGIC) { + // Validate total packet size + if (len > MAX_ESPNOW_PACKET_SIZE) { #if MESH_PACKET_LOGGING - Serial.printf("%s: ESPNOW BRIDGE: RX invalid magic 0x%02X\n", getLogDateTime(), data[0]); + Serial.printf("%s: ESPNOW BRIDGE: RX packet too large, len=%d\n", getLogDateTime(), len); +#endif + return; + } + + // Check packet header magic + uint16_t received_magic = (data[0] << 8) | data[1]; + if (received_magic != ESPNOW_HEADER_MAGIC) { +#if MESH_PACKET_LOGGING + Serial.printf("%s: ESPNOW BRIDGE: RX invalid magic 0x%04X\n", getLogDateTime(), received_magic); #endif return; } // Make a copy we can decrypt uint8_t decrypted[MAX_ESPNOW_PACKET_SIZE]; - memcpy(decrypted, data + 1, len - 1); // Skip magic byte + const size_t encryptedDataLen = len - MAGIC_HEADER_SIZE; + memcpy(decrypted, data + MAGIC_HEADER_SIZE, encryptedDataLen); - // Try to decrypt - xorCrypt(decrypted, len - 1); + // Try to decrypt (checksum + payload) + xorCrypt(decrypted, encryptedDataLen); // Validate checksum uint16_t received_checksum = (decrypted[0] << 8) | decrypted[1]; - uint16_t calculated_checksum = fletcher16(decrypted + 2, len - 3); + const size_t payloadLen = encryptedDataLen - CHECKSUM_SIZE; + uint16_t calculated_checksum = fletcher16(decrypted + CHECKSUM_SIZE, payloadLen); if (received_checksum != calculated_checksum) { // Failed to decrypt - likely from a different network @@ -116,14 +127,14 @@ void ESPNowBridge::onDataRecv(const uint8_t *mac, const uint8_t *data, int len) } #if MESH_PACKET_LOGGING - Serial.printf("%s: ESPNOW BRIDGE: RX, len=%d\n", getLogDateTime(), len - 3); + Serial.printf("%s: ESPNOW BRIDGE: RX, payload_len=%d\n", getLogDateTime(), payloadLen); #endif // Create mesh packet mesh::Packet *pkt = _instance->_mgr->allocNew(); if (!pkt) return; - if (pkt->readFrom(decrypted + 2, len - 3)) { + if (pkt->readFrom(decrypted + CHECKSUM_SIZE, payloadLen)) { _instance->onPacketReceived(pkt); } else { _instance->_mgr->free(pkt); @@ -144,27 +155,56 @@ void ESPNowBridge::onPacketReceived(mesh::Packet *packet) { void ESPNowBridge::onPacketTransmitted(mesh::Packet *packet) { if (!_seen_packets.hasSeen(packet)) { + + // First validate the packet pointer + if (!packet) { +#if MESH_PACKET_LOGGING + Serial.printf("%s: ESPNOW BRIDGE: TX invalid packet pointer\n", getLogDateTime()); +#endif + return; + } + + // Create a temporary buffer just for size calculation and reuse for actual writing + uint8_t sizingBuffer[MAX_PAYLOAD_SIZE]; + uint16_t meshPacketLen = packet->writeTo(sizingBuffer); + + // Check if packet fits within our maximum payload size + if (meshPacketLen > MAX_PAYLOAD_SIZE) { +#if MESH_PACKET_LOGGING + Serial.printf("%s: ESPNOW BRIDGE: TX packet too large (payload=%d, max=%d)\n", getLogDateTime(), + meshPacketLen, MAX_PAYLOAD_SIZE); +#endif + return; + } + uint8_t buffer[MAX_ESPNOW_PACKET_SIZE]; - buffer[0] = ESPNOW_HEADER_MAGIC; - // Write packet to buffer starting after magic byte and checksum - uint16_t len = packet->writeTo(buffer + 3); + // Write magic header (2 bytes) + buffer[0] = (ESPNOW_HEADER_MAGIC >> 8) & 0xFF; + buffer[1] = ESPNOW_HEADER_MAGIC & 0xFF; - // Calculate and add checksum - uint16_t checksum = fletcher16(buffer + 3, len); - buffer[1] = (checksum >> 8) & 0xFF; - buffer[2] = checksum & 0xFF; + // Write packet payload starting after magic header and checksum + const size_t packetOffset = MAGIC_HEADER_SIZE + CHECKSUM_SIZE; + memcpy(buffer + packetOffset, sizingBuffer, meshPacketLen); - // Encrypt payload (not including magic byte) - xorCrypt(buffer + 1, len + 2); + // Calculate and add checksum (only of the payload) + uint16_t checksum = fletcher16(buffer + packetOffset, meshPacketLen); + buffer[2] = (checksum >> 8) & 0xFF; // High byte + buffer[3] = checksum & 0xFF; // Low byte + + // Encrypt payload and checksum (not including magic header) + xorCrypt(buffer + MAGIC_HEADER_SIZE, meshPacketLen + CHECKSUM_SIZE); + + // Total packet size: magic header + checksum + payload + const size_t totalPacketSize = MAGIC_HEADER_SIZE + CHECKSUM_SIZE + meshPacketLen; // Broadcast using ESP-NOW uint8_t broadcastAddress[] = { 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF }; - esp_err_t result = esp_now_send(broadcastAddress, buffer, len + 3); + esp_err_t result = esp_now_send(broadcastAddress, buffer, totalPacketSize); #if MESH_PACKET_LOGGING if (result == ESP_OK) { - Serial.printf("%s: ESPNOW BRIDGE: TX, len=%d\n", getLogDateTime(), len); + Serial.printf("%s: ESPNOW BRIDGE: TX, len=%d\n", getLogDateTime(), meshPacketLen); } else { Serial.printf("%s: ESPNOW BRIDGE: TX FAILED!\n", getLogDateTime()); } diff --git a/src/helpers/bridges/ESPNowBridge.h b/src/helpers/bridges/ESPNowBridge.h index 7d2dbb0b..5c771471 100644 --- a/src/helpers/bridges/ESPNowBridge.h +++ b/src/helpers/bridges/ESPNowBridge.h @@ -25,9 +25,9 @@ * - Maximum packet size of 250 bytes (ESP-NOW limitation) * * Packet Structure: - * [1 byte] Magic Header (0xAB) - Used to identify ESPNowBridge packets + * [2 bytes] Magic Header - Used to identify ESPNowBridge packets * [2 bytes] Fletcher-16 checksum of encrypted payload (calculated over payload only) - * [n bytes] Encrypted payload containing the mesh packet + * [246 bytes max] Encrypted payload containing the mesh packet * * The Fletcher-16 checksum is used to validate packet integrity and detect * corrupted or tampered packets. It's calculated over the encrypted payload @@ -47,7 +47,7 @@ class ESPNowBridge : public AbstractBridge { private: static ESPNowBridge *_instance; - static void recv_cb(const uint8_t *mac, const uint8_t *data, int len); + static void recv_cb(const uint8_t *mac, const uint8_t *data, int32_t len); static void send_cb(const uint8_t *mac, esp_now_send_status_t status); /** Packet manager for allocating and queuing mesh packets */ @@ -60,18 +60,29 @@ private: SimpleMeshTables _seen_packets; /** - * Maximum ESP-NOW packet size (250 bytes) - * This is a hardware limitation of ESP-NOW protocol: - * - ESP-NOW header: 20 bytes - * - Max payload: 250 bytes - * Source: ESP-NOW API documentation + * ESP-NOW Protocol Structure: + * - ESP-NOW header: 20 bytes (handled by ESP-NOW protocol) + * - ESP-NOW payload: 250 bytes maximum + * Total ESP-NOW packet: 270 bytes + * + * Our Bridge Packet Structure (must fit in ESP-NOW payload): + * - Magic header: 2 bytes + * - Checksum: 2 bytes + * - Available payload: 246 bytes */ static const size_t MAX_ESPNOW_PACKET_SIZE = 250; /** - * Magic byte to identify ESPNowBridge packets (0xAB) + * Size constants for packet parsing */ - static const uint8_t ESPNOW_HEADER_MAGIC = 0xAB; + static const size_t MAGIC_HEADER_SIZE = 2; + static const size_t CHECKSUM_SIZE = 2; + static const size_t MAX_PAYLOAD_SIZE = MAX_ESPNOW_PACKET_SIZE - (MAGIC_HEADER_SIZE + CHECKSUM_SIZE); + + /** + * Magic bytes to identify ESPNowBridge packets + */ + static const uint16_t ESPNOW_HEADER_MAGIC = 0xC03E; /** Buffer for receiving ESP-NOW packets */ uint8_t _rx_buffer[MAX_ESPNOW_PACKET_SIZE]; @@ -106,7 +117,7 @@ private: * @param data Received data * @param len Length of received data */ - void onDataRecv(const uint8_t *mac, const uint8_t *data, int len); + void onDataRecv(const uint8_t *mac, const uint8_t *data, int32_t len); /** * ESP-NOW send callback diff --git a/src/helpers/bridges/RS232Bridge.h b/src/helpers/bridges/RS232Bridge.h index 2adeb503..49c781cb 100644 --- a/src/helpers/bridges/RS232Bridge.h +++ b/src/helpers/bridges/RS232Bridge.h @@ -21,7 +21,7 @@ * - Baud rate fixed at 115200 * * Packet Structure: - * [2 bytes] Magic Header (0xCAFE) - Used to identify start of packet + * [2 bytes] Magic Header - Used to identify start of packet * [2 bytes] Fletcher-16 checksum - Data integrity check * [1 byte] Payload length * [n bytes] Packet payload @@ -87,8 +87,8 @@ private: /** Helper function to get formatted timestamp for logging */ const char* getLogDateTime(); - /** Magic number to identify start of RS232 packets (0xCAFE) */ - static constexpr uint16_t SERIAL_PKT_MAGIC = 0xCAFE; + /** Magic number to identify start of RS232 packets */ + static constexpr uint16_t SERIAL_PKT_MAGIC = 0xC03E; /** * @brief The total overhead of the serial protocol in bytes.