From 159da20ef2a485f6f1924a55a9e0fd9aa77c6328 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Mon, 20 Mar 2023 14:31:10 +0100
Subject: [PATCH] some tweaks for busy handling

---
 linux/ipcore/PapbVcInterface.cpp         | 25 ++++++++++------
 mission/tmtc/LiveTmTask.cpp              |  3 ++
 mission/tmtc/TmStoreTaskBase.cpp         | 37 +++++++++++++-----------
 mission/tmtc/VirtualChannelWithQueue.cpp |  5 +++-
 4 files changed, 43 insertions(+), 27 deletions(-)

diff --git a/linux/ipcore/PapbVcInterface.cpp b/linux/ipcore/PapbVcInterface.cpp
index 51afc15d..2f875d04 100644
--- a/linux/ipcore/PapbVcInterface.cpp
+++ b/linux/ipcore/PapbVcInterface.cpp
@@ -44,16 +44,23 @@ void PapbVcInterface::endPacketTransfer() { *vcBaseReg = CONFIG_END; }
 ReturnValue_t PapbVcInterface::pollPapbBusySignal() const {
   gpio::Levels papbBusyState = gpio::Levels::LOW;
   ReturnValue_t result = returnvalue::OK;
+  uint32_t busyIdx = 0;
 
-  /** Check if PAPB interface is ready to receive data */
-  result = gpioComIF->readGpio(papbBusyId, papbBusyState);
-  if (result != returnvalue::OK) {
-    sif::warning << "PapbVcInterface::pollPapbBusySignal: Failed to read papb busy signal"
-                 << std::endl;
-    return returnvalue::FAILED;
-  }
-  if (papbBusyState == gpio::Levels::LOW) {
-    return PAPB_BUSY;
+  while (true) {
+    /** Check if PAPB interface is ready to receive data */
+    result = gpioComIF->readGpio(papbBusyId, papbBusyState);
+    if (result != returnvalue::OK) {
+      sif::warning << "PapbVcInterface::pollPapbBusySignal: Failed to read papb busy signal"
+                   << std::endl;
+      return returnvalue::FAILED;
+    }
+    if (papbBusyState == gpio::Levels::HIGH) {
+      return returnvalue::OK;
+    }
+    if (busyIdx == 100) {
+      return PAPB_BUSY;
+    }
+    busyIdx++;
   }
 
   return returnvalue::OK;
diff --git a/mission/tmtc/LiveTmTask.cpp b/mission/tmtc/LiveTmTask.cpp
index 53a9f04a..7e6d3de6 100644
--- a/mission/tmtc/LiveTmTask.cpp
+++ b/mission/tmtc/LiveTmTask.cpp
@@ -11,6 +11,9 @@ ReturnValue_t LiveTmTask::performOperation(uint8_t opCode) {
   while (true) {
     // The funnel tasks are scheduled here directly as well.
     ReturnValue_t result = channel.sendNextTm();
+    if (result == DirectTmSinkIF::IS_BUSY) {
+      sif::error << "Lost live TM, PAPB busy" << std::endl;
+    }
     if (result == MessageQueueIF::EMPTY) {
       if (tmFunnelCd.hasTimedOut()) {
         pusFunnel.performOperation(0);
diff --git a/mission/tmtc/TmStoreTaskBase.cpp b/mission/tmtc/TmStoreTaskBase.cpp
index d8b6bdcb..8328c65e 100644
--- a/mission/tmtc/TmStoreTaskBase.cpp
+++ b/mission/tmtc/TmStoreTaskBase.cpp
@@ -26,24 +26,27 @@ bool TmStoreTaskBase::handleOneStore(PersistentTmStoreWithTmQueue& store,
   if (store.getState() == PersistentTmStore::State::DUMPING) {
     size_t dumpedLen = 0;
     bool fileHasSwapped;
-    if (not channel.isBusy()) {
-      tmSinkBusyCd.resetTimer();
-      result = store.dumpNextPacket(channel, dumpedLen, fileHasSwapped);
-      if ((result == PersistentTmStore::DUMP_DONE or result == returnvalue::OK) and dumpedLen > 0) {
-        dumpContext.dumpedBytes += dumpedLen;
-        dumpContext.numberOfDumpedPackets += 1;
-      }
-      if (result == PersistentTmStore::DUMP_DONE) {
-        uint32_t startTime;
-        uint32_t endTime;
-        store.getStartAndEndTimeCurrentOrLastDump(startTime, endTime);
-        triggerEvent(dumpContext.eventIfDone, dumpContext.numberOfDumpedPackets,
-                     dumpContext.dumpedBytes);
-        dumpsPerformed = true;
-      } else if (result == returnvalue::OK) {
-        dumpsPerformed = true;
-      }
+    // if (not channel.isBusy()) {
+    tmSinkBusyCd.resetTimer();
+    result = store.dumpNextPacket(channel, dumpedLen, fileHasSwapped);
+    if (result == DirectTmSinkIF::IS_BUSY) {
+      sif::warning << "PersistentTmStore: PAPB was too busy for dump" << std::endl;
     }
+    if ((result == PersistentTmStore::DUMP_DONE or result == returnvalue::OK) and dumpedLen > 0) {
+      dumpContext.dumpedBytes += dumpedLen;
+      dumpContext.numberOfDumpedPackets += 1;
+    }
+    if (result == PersistentTmStore::DUMP_DONE) {
+      uint32_t startTime;
+      uint32_t endTime;
+      store.getStartAndEndTimeCurrentOrLastDump(startTime, endTime);
+      triggerEvent(dumpContext.eventIfDone, dumpContext.numberOfDumpedPackets,
+                   dumpContext.dumpedBytes);
+      dumpsPerformed = true;
+    } else if (result == returnvalue::OK) {
+      dumpsPerformed = true;
+    }
+    //}
     if (cancelDumpCd.hasTimedOut() or tmSinkBusyCd.hasTimedOut()) {
       triggerEvent(persTmStore::DUMP_WAS_CANCELLED, store.getObjectId());
       store.cancelDump();
diff --git a/mission/tmtc/VirtualChannelWithQueue.cpp b/mission/tmtc/VirtualChannelWithQueue.cpp
index 884dbf89..ef93687e 100644
--- a/mission/tmtc/VirtualChannelWithQueue.cpp
+++ b/mission/tmtc/VirtualChannelWithQueue.cpp
@@ -36,7 +36,10 @@ ReturnValue_t VirtualChannelWithQueue::sendNextTm() {
     return result;
   }
 
-  write(data, size);
+  ReturnValue_t result = write(data, size);
+  if (result != returnvalue::OK) {
+    return result;
+  }
   tmStore.deleteData(storeId);
   if (result != returnvalue::OK) {
     return result;