rewrite almost done

linux/payload/FreshSupvHandler.cpp

@@ -505,11 +505,27 @@ void FreshSupvHandler::handleTransitionToOff() {
   }
 }
 
-ReturnValue_t FreshSupvHandler::sendCommand(TcBase& tc) {
+ReturnValue_t FreshSupvHandler::sendCommand(TcBase& tc, uint32_t cmdCountdownMs) {
   if (DEBUG_PLOC_SUPV) {
     sif::debug << "PLOC SUPV: SEND PACKET Size " << tc.getFullPacketLen() << " Module APID "
                << (int)tc.getModuleApid() << " Service ID " << (int)tc.getServiceId() << std::endl;
   }
+  ActiveCmdInfo info(cmdCountdownMs);
+  auto activeCmdIter =
+      activeActionCmds.find(buildActiveCmdKey(tc.getModuleApid(), tc.getServiceId()));
+  if (activeCmdIter == activeActionCmds.end()) {
+    activeActionCmds.emplace(buildActiveCmdKey(tc.getModuleApid(), tc.getServiceId()), info);
+
+  } else {
+    if (activeCmdIter->second.isPending) {
+      return HasActionsIF::IS_BUSY;
+    }
+    activeCmdIter->second.isPending = true;
+    activeCmdIter->second.ackRecv = false;
+    activeCmdIter->second.ackExeRecv = false;
+    activeCmdIter->second.cmdCountdown.setTimeout(cmdCountdownMs);
+    activeCmdIter->second.cmdCountdown.resetTimer();
+  }
   return uartManager.sendMessage(comCookie, tc.getFullPacket(), tc.getFullPacketLen());
 }
 
@@ -768,10 +784,9 @@ ReturnValue_t FreshSupvHandler::prepareWipeMramCmd(const uint8_t* commandData, s
 ReturnValue_t FreshSupvHandler::parseTmPackets() {
   uint8_t* receivedData = nullptr;
   size_t receivedSize = 0;
-  ReturnValue_t result;
   while (true) {
-    result = uartManager.readReceivedMessage(comCookie, &receivedData, &receivedSize);
+    ReturnValue_t result = uartManager.readReceivedMessage(comCookie, &receivedData, &receivedSize);
-    if (receivedSize == 0) {
+    if (result != returnvalue::OK or receivedSize == 0) {
       break;
     }
     tmReader.setData(receivedData, receivedSize);
@@ -784,13 +799,13 @@ ReturnValue_t FreshSupvHandler::parseTmPackets() {
         switch (tmReader.getServiceId()) {
           case (static_cast<uint8_t>(supv::tm::TmtcId::ACK)):
           case (static_cast<uint8_t>(supv::tm::TmtcId::NAK)): {
-            // TODO: Handle ACK report.
+            handleAckReport(receivedData);
-            return OK;
+            continue;
           }
           case (static_cast<uint8_t>(supv::tm::TmtcId::EXEC_ACK)):
           case (static_cast<uint8_t>(supv::tm::TmtcId::EXEC_NAK)): {
-            // TODO: Hnadle Exe report.
+            handleExecutionReport(receivedData);
-            return OK;
+            continue;
           }
         }
         break;
@@ -908,13 +923,13 @@ void FreshSupvHandler::handlePacketPrint() {
 }
 
 bool FreshSupvHandler::isCommandAlreadyActive(ActionId_t actionId) const {
-  auto iter = activeActionCmds.find(actionId);
+  // Not the most efficient implementation but who cares. It's not like this map is going
-  if (iter == activeActionCmds.end()) {
+  // to be huge in the nominal case..
-    return false;
+  for (const auto& info : activeActionCmds) {
-  }
+    if (info.second.commandId == actionId and info.second.isPending) {
-  if (iter->second.isPending) {
       return true;
     }
+  }
   return false;
 }
 
@@ -1053,3 +1068,140 @@ ReturnValue_t FreshSupvHandler::eventSubscription() {
   }
   return result;
 }
+
+ReturnValue_t FreshSupvHandler::handleAckReport(const uint8_t* data) {
+  using namespace supv;
+  ReturnValue_t result = returnvalue::OK;
+  if (not tmReader.verifyCrc()) {
+    sif::error << "PlocSupervisorHandler::handleAckReport: CRC failure" << std::endl;
+    triggerEvent(SUPV_CRC_FAILURE_EVENT);
+    return returnvalue::FAILED;
+  }
+  AcknowledgmentReport ack(tmReader);
+  result = ack.parse();
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  auto infoIter =
+      activeActionCmds.find(buildActiveCmdKey(ack.getRefModuleApid(), ack.getRefServiceId()));
+  if (infoIter == activeActionCmds.end()) {
+    triggerEvent(SUPV_ACK_UNKNOWN_COMMAND, ack.getRefModuleApid(), ack.getRefServiceId());
+    return result;
+  }
+  ActiveCmdInfo& info = infoIter->second;
+  if (tmReader.getServiceId() == static_cast<uint8_t>(supv::tm::TmtcId::NAK)) {
+    triggerEvent(SUPV_ACK_FAILURE, info.commandId, static_cast<uint32_t>(ack.getStatusCode()));
+    ack.printStatusInformation();
+    printAckFailureInfo(ack.getStatusCode(), info.commandId);
+    if (info.commandedBy != MessageQueueIF::NO_QUEUE) {
+      actionHelper.finish(false, info.commandedBy, info.commandId, result::RECEIVED_ACK_FAILURE);
+    }
+    info.isPending = false;
+    return returnvalue::OK;
+  }
+  info.ackRecv = true;
+  if (info.ackRecv and info.ackExeRecv) {
+    actionHelper.finish(true, info.commandedBy, info.commandId, returnvalue::OK);
+    info.isPending = false;
+  }
+  return result;
+}
+
+void FreshSupvHandler::printAckFailureInfo(uint16_t statusCode, DeviceCommandId_t commandId) {
+  switch (commandId) {
+    case (supv::SET_TIME_REF): {
+      sif::warning
+          << "PlocSupervisoHandler: Setting time failed. Make sure the OBC has a valid time"
+          << std::endl;
+      break;
+    }
+    default:
+      break;
+  }
+}
+
+uint32_t FreshSupvHandler::buildActiveCmdKey(uint16_t moduleApid, uint8_t serviceId) {
+  return (moduleApid << 16) | serviceId;
+}
+
+ReturnValue_t FreshSupvHandler::handleExecutionReport(const uint8_t* data) {
+  using namespace supv;
+  ReturnValue_t result = returnvalue::OK;
+
+  if (not tmReader.verifyCrc()) {
+    return result::CRC_FAILURE;
+  }
+  ExecutionReport exe(tmReader);
+  result = exe.parse();
+  if (result != OK) {
+    return result;
+  }
+  auto infoIter =
+      activeActionCmds.find(buildActiveCmdKey(exe.getRefModuleApid(), exe.getRefServiceId()));
+  if (infoIter == activeActionCmds.end()) {
+    triggerEvent(SUPV_EXE_ACK_UNKNOWN_COMMAND, exe.getRefModuleApid(), exe.getRefServiceId());
+    return result;
+  }
+  ActiveCmdInfo& info = infoIter->second;
+  if (tmReader.getServiceId() == static_cast<uint8_t>(supv::tm::TmtcId::EXEC_ACK)) {
+    result = handleExecutionSuccessReport(info, exe);
+  } else if (tmReader.getServiceId() == static_cast<uint8_t>(supv::tm::TmtcId::EXEC_NAK)) {
+    handleExecutionFailureReport(info, exe);
+    return returnvalue::OK;
+  }
+  info.ackExeRecv = true;
+  if (info.ackRecv and info.ackExeRecv) {
+    actionHelper.finish(true, info.commandedBy, info.commandId, returnvalue::OK);
+    info.isPending = false;
+  }
+  return result;
+}
+
+ReturnValue_t FreshSupvHandler::handleExecutionSuccessReport(ActiveCmdInfo& info,
+                                                             ExecutionReport& report) {
+  switch (info.commandId) {
+    case supv::READ_GPIO: {
+      // TODO: Fix
+      uint16_t gpioState = report.getStatusCode();
+#if OBSW_DEBUG_PLOC_SUPERVISOR == 1
+      sif::info << "PlocSupervisorHandler: Read GPIO TM, State: " << gpioState << std::endl;
+#endif /* OBSW_DEBUG_PLOC_SUPERVISOR == 1 */
+      uint8_t data[sizeof(gpioState)];
+      size_t size = 0;
+      ReturnValue_t result = SerializeAdapter::serialize(&gpioState, data, &size, sizeof(gpioState),
+                                                         SerializeIF::Endianness::BIG);
+      if (result != returnvalue::OK) {
+        sif::debug << "PlocSupervisorHandler: Failed to deserialize GPIO state" << std::endl;
+      }
+      result = actionHelper.reportData(info.commandedBy, info.commandId, data, sizeof(data));
+      if (result != returnvalue::OK) {
+        sif::warning << "PlocSupervisorHandler: Read GPIO, failed to report data" << std::endl;
+      }
+      break;
+    }
+    case supv::SET_TIME_REF: {
+      // We could only allow proper bootup when the time was set successfully, but
+      // this makes debugging difficult.
+
+      if (startupState == StartupState::WAIT_FOR_TIME_REPLY) {
+        startupState = StartupState::TIME_WAS_SET;
+      }
+      break;
+    }
+    default:
+      break;
+  }
+  return returnvalue::OK;
+}
+
+void FreshSupvHandler::handleExecutionFailureReport(ActiveCmdInfo& info, ExecutionReport& report) {
+  using namespace supv;
+  report.printStatusInformation();
+  if (info.commandId != DeviceHandlerIF::NO_COMMAND_ID) {
+    triggerEvent(SUPV_EXE_FAILURE, info.commandId, static_cast<uint32_t>(report.getStatusCode()));
+  }
+  if (info.commandedBy) {
+    actionHelper.finish(false, info.commandedBy, info.commandId, report.getStatusCode());
+  }
+  info.isPending = false;
+}

linux/payload/FreshSupvHandler.h

@@ -115,6 +115,7 @@ class FreshSupvHandler : public FreshDeviceHandlerBase {
   struct ActiveCmdInfo {
     ActiveCmdInfo(uint32_t cmdCountdownMs) : cmdCountdown(cmdCountdownMs) {}
 
+    DeviceCommandId_t commandId;
     bool isPending = false;
     bool ackRecv = false;
     bool ackExeRecv = false;
@@ -123,8 +124,10 @@ class FreshSupvHandler : public FreshDeviceHandlerBase {
     Countdown cmdCountdown;
   };
 
+  uint32_t buildActiveCmdKey(uint16_t moduleApid, uint8_t serviceId);
+
   // Map for Action commands. For normal commands, a separate static structure will be used.
-  std::map<ActionId_t, ActiveCmdInfo> activeActionCmds;
+  std::map<uint32_t, ActiveCmdInfo> activeActionCmds;
 
   ActiveCmdInfo hkRequestCmdInfo = ActiveCmdInfo(500);
 
@@ -134,7 +137,7 @@ class FreshSupvHandler : public FreshDeviceHandlerBase {
 
   ReturnValue_t parseTmPackets();
 
-  ReturnValue_t sendCommand(TcBase& tc);
+  ReturnValue_t sendCommand(TcBase& tc, uint32_t cmdCountdownMs = 1000);
   ReturnValue_t sendEmptyCmd(uint16_t apid, uint8_t serviceId);
   ReturnValue_t prepareSelBootImageCmd(const uint8_t* commandData);
   ReturnValue_t prepareSetTimeRefCmd();
@@ -162,6 +165,11 @@ class FreshSupvHandler : public FreshDeviceHandlerBase {
   ReturnValue_t eventSubscription();
   void handlePacketPrint();
   bool isCommandAlreadyActive(ActionId_t actionId) const;
+  ReturnValue_t handleAckReport(const uint8_t* data);
+  void printAckFailureInfo(uint16_t statusCode, DeviceCommandId_t commandId);
+  ReturnValue_t handleExecutionReport(const uint8_t* data);
+  ReturnValue_t handleExecutionSuccessReport(ActiveCmdInfo& info, supv::ExecutionReport& report);
+  void handleExecutionFailureReport(ActiveCmdInfo& info, supv::ExecutionReport& report);
 };
 
 #endif /* LINUX_PAYLOAD_FRESHSUPVHANDLER_H_ */

linux/payload/plocSupvDefs.h

@@ -36,6 +36,12 @@ static const Event SUPV_CRC_FAILURE_EVENT = MAKE_EVENT(6, severity::LOW);
 static const Event SUPV_HELPER_EXECUTING = MAKE_EVENT(7, severity::LOW);
 //! [EXPORT] : [COMMENT] Failed to build the command to shutdown the MPSoC
 static const Event SUPV_MPSOC_SHUTDOWN_BUILD_FAILED = MAKE_EVENT(8, severity::LOW);
+//! [EXPORT] : [COMMENT] Received ACK, but no related command is unknown or has not been sent
+//  by this software instance. P1: Module APID. P2: Service ID.
+static const Event SUPV_ACK_UNKNOWN_COMMAND = MAKE_EVENT(9, severity::LOW);
+//! [EXPORT] : [COMMENT] Received ACK EXE, but no related command is unknown or has not been sent
+//  by this software instance. P1: Module APID. P2: Service ID.
+static const Event SUPV_EXE_ACK_UNKNOWN_COMMAND = MAKE_EVENT(10, severity::LOW);
 
 extern std::atomic_bool SUPV_ON;
 static constexpr uint32_t BOOT_TIMEOUT_MS = 4000;