From 0c389177e35db549ab11ae4b260c72e79da53038 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Tue, 4 Oct 2022 18:38:53 +0200
Subject: [PATCH] refactor DLE parsing and move semaphore release up top

---
 fsfw                             |  2 +-
 linux/devices/ScexUartReader.cpp | 26 +++++++++++++++++++++-----
 2 files changed, 22 insertions(+), 6 deletions(-)

diff --git a/fsfw b/fsfw
index c47bed07..cfc00d02 160000
--- a/fsfw
+++ b/fsfw
@@ -1 +1 @@
-Subproject commit c47bed07606548fd2510caa45bdd9fc867732065
+Subproject commit cfc00d02607d22ec306d0540f9f2329774df1cdd
diff --git a/linux/devices/ScexUartReader.cpp b/linux/devices/ScexUartReader.cpp
index 44c397e8..93e24d08 100644
--- a/linux/devices/ScexUartReader.cpp
+++ b/linux/devices/ScexUartReader.cpp
@@ -35,6 +35,7 @@ ReturnValue_t ScexUartReader::performOperation(uint8_t operationCode) {
   while (true) {
     semaphore->acquire();
     int bytesRead = 0;
+    // debugMode = true;
     while (true) {
       bytesRead = read(serialPort, reinterpret_cast<void *>(recBuf.data()),
                        static_cast<unsigned int>(recBuf.size()));
@@ -44,6 +45,21 @@ ReturnValue_t ScexUartReader::performOperation(uint8_t operationCode) {
           state = States::IDLE;
           break;
         }
+        size_t bytesRead = 0;
+        ReturnValue_t result = returnvalue::OK;
+        while (result != DleParser::NO_PACKET_FOUND) {
+          result = dleParser.parseRingBuf(bytesRead);
+          if (result == returnvalue::OK) {
+            // Packet found, advance read pointer.
+            // currentBytesRead = 0;
+            dleParser.confirmBytesRead(bytesRead);
+          } else if (result == DleParser::POSSIBLE_PACKET_LOSS) {
+            // Markers found at wrong place
+            // which might be a hint for a possibly lost packet.
+            // currentBytesRead = 0;
+            dleParser.confirmBytesRead(bytesRead);
+          }
+        }
         TaskFactory::delayTask(400);
       } else if (bytesRead < 0) {
         sif::warning << "ScexUartReader::performOperation: read call failed with error [" << errno
@@ -124,8 +140,12 @@ ReturnValue_t ScexUartReader::sendMessage(CookieIF *cookie, const uint8_t *sendD
   }
   state = States::RUNNING;
   lock->unlockMutex();
+  ReturnValue_t result = semaphore->release();
+  if (result != OK) {
+    std::cout << "ScexUartReader::sendMessage: Releasing semaphore failed" << std::endl;
+  }
   size_t encodedLen = 0;
-  ReturnValue_t result =
+  result =
       dleEncoder.encode(sendData, sendLen, cmdbuf.data(), cmdbuf.size(), &encodedLen, true);
   if (result != OK) {
     sif::warning << "ScexUartReader::sendMessage: Encoding failed" << std::endl;
@@ -137,10 +157,6 @@ ReturnValue_t ScexUartReader::sendMessage(CookieIF *cookie, const uint8_t *sendD
                  << std::endl;
     return FAILED;
   }
-  result = semaphore->release();
-  if (result != OK) {
-    std::cout << "ScexUartReader::sendMessage: Releasing semaphore failed" << std::endl;
-  }
   return OK;
 }