From 803b42b5e7fa465244f76a735445ced8a6ad9fa0 Mon Sep 17 00:00:00 2001
From: Bryan Mayland <bmayland@capnbry.net>
Date: Sun, 14 Jul 2024 10:13:56 -0400
Subject: [PATCH] Refactor to resolve web flashing failure after optimization

---
 src/lib/Handset/CRSFHandset.cpp | 121 ++++++++++++++++----------------
 src/lib/Handset/CRSFHandset.h   |   2 +-
 2 files changed, 60 insertions(+), 63 deletions(-)

diff --git a/src/lib/Handset/CRSFHandset.cpp b/src/lib/Handset/CRSFHandset.cpp
index b0a3091521..1ef39d0e01 100644
--- a/src/lib/Handset/CRSFHandset.cpp
+++ b/src/lib/Handset/CRSFHandset.cpp
@@ -72,7 +72,9 @@ void CRSFHandset::Begin()
     UARTinverted = halfDuplex; // on a full UART we will start uninverted checking first
     CRSFHandset::Port.begin(UARTrequestedBaud, SERIAL_8N1,
                      GPIO_PIN_RCSIGNAL_RX, GPIO_PIN_RCSIGNAL_TX,
-                     false, 500);
+                     false, 0);
+    // Arduino defaults every esp32 stream to a 1000ms timeout which is just baffling
+    CRSFHandset::Port.setTimeout(0);
     if (halfDuplex)
     {
         duplex_set_RX();
@@ -398,6 +400,25 @@ bool CRSFHandset::ProcessPacket()
     return packetReceived;
 }
 
+void CRSFHandset::alignBufferToSync(uint8_t startIdx)
+{
+    uint8_t *SerialInBuffer = inBuffer.asUint8_t;
+
+    for (unsigned int i=startIdx ; i<SerialInPacketPtr ; i++)
+    {
+        // If we find a header byte then move that and trailing bytes to the head of the buffer and let's go!
+        if (SerialInBuffer[i] == CRSF_ADDRESS_CRSF_TRANSMITTER || SerialInBuffer[i] == CRSF_SYNC_BYTE)
+        {
+            SerialInPacketPtr -= i;
+            memmove(SerialInBuffer, &SerialInBuffer[i], SerialInPacketPtr);
+            return;
+        }
+    }
+
+    // If no header found then discard this entire buffer
+    SerialInPacketPtr = 0;
+}
+
 void CRSFHandset::handleInput()
 {
     uint8_t *SerialInBuffer = inBuffer.asUint8_t;
@@ -434,76 +455,49 @@ void CRSFHandset::handleInput()
         flush_port_input();
     }
 
+    // Add new data, and then discard bytes until we start with header byte
     auto toRead = std::min(CRSFHandset::Port.available(), CRSF_MAX_PACKET_LEN - SerialInPacketPtr);
-    SerialInPacketPtr += (int)CRSFHandset::Port.readBytes(&SerialInBuffer[SerialInPacketPtr], toRead);
+    SerialInPacketPtr += CRSFHandset::Port.readBytes(&SerialInBuffer[SerialInPacketPtr], toRead);
+    alignBufferToSync(0);
+
+    // Make sure we have at least a packet header and a length byte
+    if (SerialInPacketPtr < 3)
+        return;
 
-    // discard bytes until we start with header byte
-    if (!CRSFframeActive)
+    // Sanity check: A total packet must be at least [sync][len][type][crc] (if no payload) and at most CRSF_MAX_PACKET_LEN
+    const uint32_t totalLen = SerialInBuffer[1] + 2;
+    if (totalLen < 4 || totalLen > CRSF_MAX_PACKET_LEN)
     {
-        for (int i=0 ; i<SerialInPacketPtr ; i++)
-        {
-            // If we find a header byte then move that and trailing bytes to the head of the buffer and let's go!
-            if (SerialInBuffer[i] == CRSF_ADDRESS_CRSF_TRANSMITTER || SerialInBuffer[i] == CRSF_SYNC_BYTE)
-            {
-                SerialInPacketPtr -= i;
-                memmove(SerialInBuffer, SerialInBuffer + i, SerialInPacketPtr);
-                CRSFframeActive = true;
-                break;
-            }
-        }
-        // If no header found then discard this entire buffer
-        if (!CRSFframeActive)
-        {
-            SerialInPacketPtr = 0;
-        }
+        // Start looking for another packet after this start byte
+        alignBufferToSync(1);
+        return;
     }
 
-    // We have a packet header and at least a length byte as well, so we see if we have a pull packet for processing
-    if (CRSFframeActive && SerialInPacketPtr>2)
+    // Only proceed one there are enough bytes in the buffer for the entire packet
+    if (SerialInPacketPtr < totalLen)
+        return;
+
+    uint8_t CalculatedCRC = crsf_crc.calc(&SerialInBuffer[2], totalLen - 3);
+    if (CalculatedCRC == SerialInBuffer[totalLen - 1])
     {
-        // Sanity check: If the length byte is pushing over the max packet size skip this header byte and start scanning again
-        if ((SerialInBuffer[1] + 2) > CRSF_MAX_PACKET_LEN)
-        {
-            for (int i=0 ; i<SerialInPacketPtr ; i++)
-            {
-                if (SerialInBuffer[i] == CRSF_ADDRESS_CRSF_TRANSMITTER || SerialInBuffer[i] == CRSF_SYNC_BYTE)
-                {
-                    SerialInPacketPtr -= i;
-                    memmove(SerialInBuffer, SerialInBuffer + i, SerialInPacketPtr);
-                    CRSFframeActive = true;
-                    return;
-                }
-            }
-            SerialInPacketPtr = 0;
-            CRSFframeActive = false;
-        }
-        // If we have at least the number of bytes for a full packet then validate and process it
-        else if (SerialInPacketPtr >= (SerialInBuffer[1] + 2))
+        GoodPktsCount++;
+        if (ProcessPacket())
         {
-            uint8_t CalculatedCRC = crsf_crc.calc(SerialInBuffer + 2, SerialInBuffer[1] - 1);
-
-            if (CalculatedCRC == SerialInBuffer[SerialInBuffer[1] + 2 - 1])
-            {
-                GoodPktsCount++;
-                if (ProcessPacket())
-                {
-                    handleOutput(SerialInBuffer[1] + 2);
-                    if (RCdataCallback)
-                    {
-                        RCdataCallback();
-                    }
-                }
-            }
-            else
+            handleOutput(totalLen);
+            if (RCdataCallback)
             {
-                DBGLN("UART CRC failure");
-                BadPktsCount++;
+                RCdataCallback();
             }
-            SerialInPacketPtr -= (SerialInBuffer[1] + 2);
-            memmove(SerialInBuffer, SerialInBuffer + (SerialInBuffer[1] + 2), SerialInPacketPtr);
-            CRSFframeActive = false;
         }
     }
+    else
+    {
+        DBGLN("UART CRC failure");
+        BadPktsCount++;
+    }
+
+    SerialInPacketPtr -= totalLen;
+    memmove(SerialInBuffer, &SerialInBuffer[totalLen], SerialInPacketPtr);
 }
 
 void CRSFHandset::handleOutput(int receivedBytes)
@@ -765,9 +759,12 @@ bool CRSFHandset::UARTwdt()
     bool retval = false;
 #if !defined(DEBUG_TX_FREERUN)
     uint32_t now = millis();
-    if (now >= (UARTwdtLastChecked + UARTwdtInterval))
+    if (now - UARTwdtLastChecked > UARTwdtInterval)
     {
-        if (BadPktsCount >= GoodPktsCount || !controllerConnected)
+        // If no packets or more bad than good packets, rate cycle/autobaud the UART but
+        // do not adjust the parameters while in wifi mode. If a firmware is being
+        // uploaded, it will cause tons of serial errors during the flash writes
+        if ((connectionState != wifiUpdate) && (BadPktsCount >= GoodPktsCount || !controllerConnected))
         {
             DBGLN("Too many bad UART RX packets!");
 
diff --git a/src/lib/Handset/CRSFHandset.h b/src/lib/Handset/CRSFHandset.h
index 461c57863d..aa84515d04 100644
--- a/src/lib/Handset/CRSFHandset.h
+++ b/src/lib/Handset/CRSFHandset.h
@@ -62,7 +62,6 @@ class CRSFHandset final : public Handset
 
     /// UART Handling ///
     uint8_t SerialInPacketPtr = 0; // index where we are reading/writing
-    bool CRSFframeActive = false;  // since we get a copy of the serial data use this flag to know when to ignore it
     static bool halfDuplex;
     bool transmitting = false;
     uint32_t GoodPktsCount = 0;
@@ -84,6 +83,7 @@ class CRSFHandset final : public Handset
     void duplex_set_TX() const;
     void RcPacketToChannelsData();
     bool processInternalCrsfPackage(uint8_t *package);
+    void alignBufferToSync(uint8_t startIdx);
     bool ProcessPacket();
     bool UARTwdt();
     uint32_t autobaud();