diff --git a/src/helpers/esp32/SerialWifiInterface.cpp b/src/helpers/esp32/SerialWifiInterface.cpp index 2df9980a..9470c827 100644 --- a/src/helpers/esp32/SerialWifiInterface.cpp +++ b/src/helpers/esp32/SerialWifiInterface.cpp @@ -54,6 +54,9 @@ size_t SerialWifiInterface::checkRecvFrame(uint8_t dest[]) { // switch active connection to new client client = newClient; + + // forget received frame header + received_frame_header = NULL; } @@ -86,13 +89,83 @@ 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(received_frame_header == NULL){ + + // 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 + uint8_t frame_header[3]; + client.readBytes(frame_header, frame_header_length); + + // parse frame header + uint8_t frame_type = frame_header[0]; + uint16_t frame_length; + memcpy(&frame_length, &frame_header[1], 2); + + // we have received a frame header + received_frame_header = new FrameHeader(); + received_frame_header->type = frame_type; + received_frame_header->length = frame_length; + + } + } + + // check if we have received a frame header + if(received_frame_header){ + + // 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; + } + received_frame_header = NULL; + 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; + } + received_frame_header = NULL; + return 0; + } + + // read frame data + uint8_t frame_data[MAX_FRAME_SIZE]; + client.readBytes(frame_data, frame_length); + + // copy frame to provided buffer + memcpy(dest, frame_data, frame_length); + + // ready for next frame + received_frame_header = NULL; + return frame_length; + + } + } } diff --git a/src/helpers/esp32/SerialWifiInterface.h b/src/helpers/esp32/SerialWifiInterface.h index 2b6c6edd..c7139b40 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 = NULL; + #define FRAME_QUEUE_SIZE 4 int recv_queue_len; Frame recv_queue[FRAME_QUEUE_SIZE];