Merge pull request #1308 from liamcottle/fix/wifi-interface-frames

Fix: WiFi Interface Frame Parsing

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

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