From 714e2d07e530f7a06c10c8a7bf7fade2617e47ef Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Fri, 12 May 2023 17:13:51 +0200
Subject: [PATCH] and were at DHB debugging again

---
 linux/payload/PlocMpsocHandler.cpp | 14 +++++++++-----
 1 file changed, 9 insertions(+), 5 deletions(-)

diff --git a/linux/payload/PlocMpsocHandler.cpp b/linux/payload/PlocMpsocHandler.cpp
index 86589d21..484ce47c 100644
--- a/linux/payload/PlocMpsocHandler.cpp
+++ b/linux/payload/PlocMpsocHandler.cpp
@@ -225,9 +225,9 @@ void PlocMPSoCHandler::doShutDown() {
 }
 
 ReturnValue_t PlocMPSoCHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) {
-  if (not normalCmdPending) {
+  if (not commandIsExecuting(mpsoc::TC_GET_HK_REPORT)) {
     *id = mpsoc::TC_GET_HK_REPORT;
-    normalCmdPending = true;
+    // normalCmdPending = true;
     return buildCommandFromCommand(*id, nullptr, 0);
   }
   return NOTHING_TO_SEND;
@@ -424,6 +424,7 @@ ReturnValue_t PlocMPSoCHandler::scanForReply(const uint8_t* start, size_t remain
     triggerEvent(MPSOC_HANDLER_SEQUENCE_COUNT_MISMATCH, sequenceCount, recvSeqCnt);
     sequenceCount = recvSeqCnt;
   }
+  sif::debug << "sequence count: " << sequenceCount.get() << std::endl;
   // This sequence count ping pong does not make any sense but it is how the MPSoC expects it.
   sequenceCount++;
   return result;
@@ -804,19 +805,22 @@ ReturnValue_t PlocMPSoCHandler::handleExecutionReport(const uint8_t* data) {
 
   uint16_t apid = (*(data) << 8 | *(data + 1)) & APID_MASK;
 
-  auto cmdDoneHandler = [&]() {
+  auto cmdDoneHandler = [&](bool success) {
     if (normalCmdPending) {
       normalCmdPending = false;
     }
     auto commandIter = deviceCommandMap.find(getPendingCommand());
     if (commandIter != deviceCommandMap.end()) {
       commandIter->second.isExecuting = false;
+      if (commandIter->second.sendReplyTo != MessageQueueIF::NO_QUEUE) {
+        actionHelper.finish(success, commandIter->second.sendReplyTo, getPendingCommand(), result);
+      }
     }
     disableAllReplies();
   };
   switch (apid) {
     case (mpsoc::apid::EXE_SUCCESS): {
-      cmdDoneHandler();
+      cmdDoneHandler(true);
       break;
     }
     case (mpsoc::apid::EXE_FAILURE): {
@@ -833,7 +837,7 @@ ReturnValue_t PlocMPSoCHandler::handleExecutionReport(const uint8_t* data) {
       printStatus(data);
       sendFailureReport(mpsoc::EXE_REPORT, MPSoCReturnValuesIF::RECEIVED_EXE_FAILURE);
       result = IGNORE_REPLY_DATA;
-      cmdDoneHandler();
+      cmdDoneHandler(false);
       break;
     }
     default: {