FW: Implement proper receive framing; fixes #52

fw/KFDtool/SerialProtocol.c

@@ -12,8 +12,16 @@
 #define ESC 0x63
 #define ESC_PLACEHOLDER 0x64
 
+uint8_t rxIndex;
+uint8_t rxStartFlag;
+uint8_t rxEscapeFlag;
+
 void spConnect(void)
 {
+    rxIndex = 0;
+    rxStartFlag = 0;
+    rxEscapeFlag = 0;
+
     USB_setup(TRUE, TRUE);
 }
 
@@ -32,54 +40,86 @@ void spDisconnect(void)
     USB_disable(); // Disable USB module, disable PLL
 }
 
-uint16_t spRxData(uint8_t* outData)
+uint16_t spRxData(uint8_t *rxBuffer)
 {
-    // TODO implement ring buffer, currently expecting all data to come in one transfer
+    uint8_t inData[1];
-
     uint16_t inDataCount;
-    uint8_t inData[128];
 
+    while (1)
+    {
         inDataCount = cdcReceiveDataInBuffer(inData, sizeof(inData), CDC0_INTFNUM);
 
-    // don't process partial frames
+        // no data to read
-    if (inDataCount < 3 || inData[0] != SOM_EOM || inData[inDataCount - 1] != SOM_EOM)
+        if (inDataCount < 1)
         {
             return 0;
         }
 
-    uint16_t inIndex;
+        // reset if buffer overrun
-    uint16_t outIndex;
+        if (rxIndex == sizeof(rxBuffer))
-    outIndex = 0;
+        {
+            rxIndex = 0;
+            rxStartFlag = 0;
+            rxEscapeFlag = 0;
 
-    for (inIndex = 1; inIndex < inDataCount - 1; inIndex++) // skip SOM and EOM
+            return 0;
-    {
+        }
-        if (inData[inIndex] == ESC)
-        {
-            inIndex++;
 
-            if (inData[inIndex] == SOM_EOM_PLACEHOLDER)
+        // got SOM/EOM
+        if (inData[0] == SOM_EOM)
         {
-                outData[outIndex] = SOM_EOM;
+            // not started, set start flag, clear other flags
-            }
+            if (rxStartFlag == 0)
-            else if (inData[inIndex] == ESC_PLACEHOLDER)
             {
-                outData[outIndex] = ESC;
+                rxIndex = 0;
-            }
+                rxStartFlag = 1;
+                rxEscapeFlag = 0;
+
+                return 0;
             }
+            // started, clear start flag and return message length
             else
             {
-            outData[outIndex] = inData[inIndex];
+                rxStartFlag = 0;
+
+                return rxIndex;
+            }
         }
 
-        outIndex++;
+        // not started, do not continue
+        if (rxStartFlag == 0)
+        {
+            return 0;
         }
 
-    return outIndex;
+        // escape byte
+        if (rxEscapeFlag)
+        {
+            if (inData[0] == SOM_EOM_PLACEHOLDER)
+            {
+                rxBuffer[rxIndex++] = SOM_EOM;
+            }
+            else if (inData[0] == ESC_PLACEHOLDER)
+            {
+                rxBuffer[rxIndex++] = ESC;
+            }
+
+            rxEscapeFlag = 0;
+        }
+        // escape byte, set flag for next byte to be escaped
+        else if (inData[0] == ESC)
+        {
+            rxEscapeFlag = 1;
+        }
+        // normal byte, save as is
+        else
+        {
+            rxBuffer[rxIndex++] = inData[0];
+        }
+    }
 }
 
-uint16_t spFrameData(const uint8_t* inData,
+uint16_t spFrameData(const uint8_t *inData, uint16_t inLength, uint8_t *outData)
-                     uint16_t inLength,
-                     uint8_t* outData)
 {
     uint16_t escCharsNeeded = 0;
     uint16_t i;
@@ -127,8 +167,7 @@ uint16_t spFrameData(const uint8_t* inData,
     return totalCharsNeeded;
 }
 
-void spTxDataBack(const uint8_t* inData,
+void spTxDataBack(const uint8_t *inData, uint16_t inLength)
-                            uint16_t inLength)
 {
     uint16_t outLength;
     uint8_t outData[128];
@@ -138,8 +177,7 @@ void spTxDataBack(const uint8_t* inData,
     cdcSendDataInBackground(outData, outLength, CDC0_INTFNUM, 1000);
 }
 
-void spTxDataWait(const uint8_t* inData,
+void spTxDataWait(const uint8_t *inData, uint16_t inLength)
-                            uint16_t inLength)
 {
     uint16_t outLength;
     uint8_t outData[128];