implement frame header parising for wifi interface

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;
+
       }
+      
     }
   }
 

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];