From 1820dae3d465929d6096cc0bcf52c7c377783b47 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Fri, 12 May 2023 11:06:26 +0200
Subject: [PATCH] horrible

---
 linux/payload/PlocMpsocHandler.cpp | 30 +++++++++++++++++++++++-------
 linux/payload/plocMpscoDefs.h      |  3 ---
 2 files changed, 23 insertions(+), 10 deletions(-)

diff --git a/linux/payload/PlocMpsocHandler.cpp b/linux/payload/PlocMpsocHandler.cpp
index cd405fa6..61b82f7f 100644
--- a/linux/payload/PlocMpsocHandler.cpp
+++ b/linux/payload/PlocMpsocHandler.cpp
@@ -209,8 +209,11 @@ void PlocMPSoCHandler::doShutDown() {
 
 
 ReturnValue_t PlocMPSoCHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) {
-  *id = mpsoc::TC_GET_HK_REPORT;
-  return buildCommandFromCommand(*id, nullptr, 0);
+  if(getPendingCommand() == DeviceHandlerIF::NO_COMMAND_ID) {
+    *id = mpsoc::TC_GET_HK_REPORT;
+    return buildCommandFromCommand(*id, nullptr, 0);
+  }
+  return NOTHING_TO_SEND;
 }
 
 ReturnValue_t PlocMPSoCHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) {
@@ -256,6 +259,7 @@ ReturnValue_t PlocMPSoCHandler::buildCommandFromCommand(DeviceCommandId_t device
       break;
     }
     case (mpsoc::TC_GET_HK_REPORT): {
+      sif::debug << "getting HK report" << std::endl;
       result = prepareTcGetHkReport();
       break;
     }
@@ -326,13 +330,14 @@ void PlocMPSoCHandler::fillCommandAndReplyMap() {
   this->insertInReplyMap(mpsoc::ACK_REPORT, 3, nullptr, mpsoc::SIZE_ACK_REPORT);
   this->insertInReplyMap(mpsoc::EXE_REPORT, 3, nullptr, mpsoc::SIZE_EXE_REPORT);
   this->insertInReplyMap(mpsoc::TM_MEMORY_READ_REPORT, 2, nullptr, mpsoc::SIZE_TM_MEM_READ_REPORT);
-  this->insertInReplyMap(mpsoc::TM_GET_HK_REPORT, 2, nullptr, 0);
+  this->insertInReplyMap(mpsoc::TM_GET_HK_REPORT, 5, nullptr, mpsoc::SIZE_TM_HK_REPORT);
   this->insertInReplyMap(mpsoc::TM_CAM_CMD_RPT, 2, nullptr, mpsoc::SP_MAX_SIZE);
 }
 
 ReturnValue_t PlocMPSoCHandler::scanForReply(const uint8_t* start, size_t remainingSize,
                                              DeviceCommandId_t* foundId, size_t* foundLen) {
   ReturnValue_t result = returnvalue::OK;
+  sif::debug << "remaining size: " << remainingSize << std::endl;
 
   SpacePacketReader spacePacket;
   spacePacket.setReadOnlyData(start, remainingSize);
@@ -347,6 +352,7 @@ ReturnValue_t PlocMPSoCHandler::scanForReply(const uint8_t* start, size_t remain
 
   switch (apid) {
     case (mpsoc::apid::ACK_SUCCESS):
+        sif::debug << "recv ack success" << std::endl;
       *foundLen = mpsoc::SIZE_ACK_REPORT;
       *foundId = mpsoc::ACK_REPORT;
       break;
@@ -364,12 +370,14 @@ ReturnValue_t PlocMPSoCHandler::scanForReply(const uint8_t* start, size_t remain
       *foundId = mpsoc::TM_CAM_CMD_RPT;
       break;
     case (mpsoc::apid::TM_HK_GET_REPORT): {
+        sif::debug << "recv hk report" << std::endl;
       *foundLen = spacePacket.getFullPacketLen();
       foundPacketLen = *foundLen;
       *foundId = mpsoc::TM_GET_HK_REPORT;
       break;
     }
     case (mpsoc::apid::EXE_SUCCESS):
+        sif::debug << "recv exe success" << std::endl;
       *foundLen = mpsoc::SIZE_EXE_REPORT;
       *foundId = mpsoc::EXE_REPORT;
       break;
@@ -427,7 +435,9 @@ ReturnValue_t PlocMPSoCHandler::interpretDeviceReply(DeviceCommandId_t id, const
   return result;
 }
 
-void PlocMPSoCHandler::setNormalDatapoolEntriesInvalid() {}
+void PlocMPSoCHandler::setNormalDatapoolEntriesInvalid() {
+  hkReport.setValidity(false, true);
+}
 
 uint32_t PlocMPSoCHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 5000; }
 
@@ -572,12 +582,12 @@ ReturnValue_t PlocMPSoCHandler::prepareTcDownlinkPwrOff() {
 
 ReturnValue_t PlocMPSoCHandler::prepareTcGetHkReport() {
   ReturnValue_t result = returnvalue::OK;
-  mpsoc::TcGetHkReport tcDownlinkPwrOff(spParams, sequenceCount);
-  result = tcDownlinkPwrOff.buildPacket();
+  mpsoc::TcGetHkReport tcGetHkReport(spParams, sequenceCount);
+  result = tcGetHkReport.buildPacket();
   if (result != returnvalue::OK) {
     return result;
   }
-  finishTcPrep(tcDownlinkPwrOff.getFullPacketLen());
+  finishTcPrep(tcGetHkReport.getFullPacketLen());
   return returnvalue::OK;
 }
 
@@ -1105,12 +1115,17 @@ void PlocMPSoCHandler::setNextReplyId() {
     case mpsoc::TC_MEM_READ:
       nextReplyId = mpsoc::TM_MEMORY_READ_REPORT;
       break;
+    case mpsoc::TC_GET_HK_REPORT: {
+      nextReplyId = mpsoc::TM_GET_HK_REPORT;
+      break;
+    }
     default:
       /* If no telemetry is expected the next reply is always the execution report */
       nextReplyId = mpsoc::EXE_REPORT;
       break;
   }
 }
+
 size_t PlocMPSoCHandler::getNextReplyLength(DeviceCommandId_t commandId) {
   size_t replyLen = 0;
 
@@ -1136,6 +1151,7 @@ size_t PlocMPSoCHandler::getNextReplyLength(DeviceCommandId_t commandId) {
         replyLen = mpsoc::SP_MAX_SIZE;
         break;
       default: {
+        sif::debug << "reply length " << iter->second.replyLen << std::endl;
         replyLen = iter->second.replyLen;
         break;
       }
diff --git a/linux/payload/plocMpscoDefs.h b/linux/payload/plocMpscoDefs.h
index 1ef72bf5..20d70d66 100644
--- a/linux/payload/plocMpscoDefs.h
+++ b/linux/payload/plocMpscoDefs.h
@@ -608,9 +608,6 @@ class TcDownlinkPwrOn : public TcBase {
   }
 };
 
-/**
- * @brief   Class to build replay stop space packet.
- */
 class TcGetHkReport : public TcBase {
  public:
   TcGetHkReport(ploc::SpTcParams params, uint16_t sequenceCount)