diff --git a/src/helpers/esp32/SerialWifiInterface.cpp b/src/helpers/esp32/SerialWifiInterface.cpp index 2df9980a..462e3ecc 100644 --- a/src/helpers/esp32/SerialWifiInterface.cpp +++ b/src/helpers/esp32/SerialWifiInterface.cpp @@ -43,6 +43,15 @@ bool SerialWifiInterface::isWriteBusy() const { return false; } +bool SerialWifiInterface::hasReceivedFrameHeader() { + return received_frame_header.type != 0 && received_frame_header.length != 0; +} + +void SerialWifiInterface::resetReceivedFrameHeader() { + received_frame_header.type = 0; + received_frame_header.length = 0; +} + size_t SerialWifiInterface::checkRecvFrame(uint8_t dest[]) { // check if new client connected auto newClient = server.available(); @@ -54,6 +63,9 @@ size_t SerialWifiInterface::checkRecvFrame(uint8_t dest[]) { // switch active connection to new client client = newClient; + + // forget received frame header + resetReceivedFrameHeader(); } @@ -86,13 +98,69 @@ size_t SerialWifiInterface::checkRecvFrame(uint8_t dest[]) { send_queue[i] = send_queue[i + 1]; } } else { - int len = client.available(); - if (len > 0) { - uint8_t buf[MAX_FRAME_SIZE + 4]; - client.readBytes(buf, len); - memcpy(dest, buf+3, len-3); // remove header (don't even check ... problems are on the other dir) - return len-3; + + // check if we are waiting for a frame header + if(!hasReceivedFrameHeader()){ + + // make sure we have received enough bytes for a frame header + // 3 bytes frame header = (1 byte frame type) + (2 bytes frame length as unsigned 16-bit little endian) + int frame_header_length = 3; + if(client.available() >= frame_header_length){ + + // read frame header + client.readBytes(&received_frame_header.type, 1); + client.readBytes((uint8_t*)&received_frame_header.length, 2); + + } + } + + // check if we have received a frame header + if(hasReceivedFrameHeader()){ + + // make sure we have received enough bytes for the required frame length + int available = client.available(); + int frame_type = received_frame_header.type; + int frame_length = received_frame_header.length; + if(frame_length > available){ + WIFI_DEBUG_PRINTLN("Waiting for %d more bytes", frame_length - available); + return 0; + } + + // skip frames that are larger than MAX_FRAME_SIZE + if(frame_length > MAX_FRAME_SIZE){ + WIFI_DEBUG_PRINTLN("Skipping frame: length=%d is larger than MAX_FRAME_SIZE=%d", frame_length, MAX_FRAME_SIZE); + while(frame_length > 0){ + uint8_t skip[1]; + int skipped = client.read(skip, 1); + frame_length -= skipped; + } + resetReceivedFrameHeader(); + return 0; + } + + // skip frames that are not expected type + // '<' is 0x3c which indicates a frame sent from app to radio + if(frame_type != '<'){ + WIFI_DEBUG_PRINTLN("Skipping frame: type=0x%x is unexpected", frame_type); + while(frame_length > 0){ + uint8_t skip[1]; + int skipped = client.read(skip, 1); + frame_length -= skipped; + } + resetReceivedFrameHeader(); + return 0; + } + + // read frame data to provided buffer + client.readBytes(dest, frame_length); + + // ready for next frame + resetReceivedFrameHeader(); + return frame_length; + + } + } } diff --git a/src/helpers/esp32/SerialWifiInterface.h b/src/helpers/esp32/SerialWifiInterface.h index 2b6c6edd..19291497 100644 --- a/src/helpers/esp32/SerialWifiInterface.h +++ b/src/helpers/esp32/SerialWifiInterface.h @@ -12,11 +12,18 @@ class SerialWifiInterface : public BaseSerialInterface { WiFiServer server; WiFiClient client; + struct FrameHeader { + uint8_t type; + uint16_t length; + }; + struct Frame { uint8_t len; uint8_t buf[MAX_FRAME_SIZE]; }; + FrameHeader received_frame_header; + #define FRAME_QUEUE_SIZE 4 int recv_queue_len; Frame recv_queue[FRAME_QUEUE_SIZE]; @@ -33,6 +40,8 @@ public: _isEnabled = false; _last_write = 0; send_queue_len = recv_queue_len = 0; + received_frame_header.type = 0; + received_frame_header.length = 0; } void begin(int port); @@ -47,6 +56,9 @@ public: size_t writeFrame(const uint8_t src[], size_t len) override; size_t checkRecvFrame(uint8_t dest[]) override; + + bool hasReceivedFrameHeader(); + void resetReceivedFrameHeader(); }; #if WIFI_DEBUG_LOGGING && ARDUINO