continue the refactoring

2024-04-12 18:30:18 +02:00 · 2024-04-12 18:30:18 +02:00 · 63e7b928cf
parent 4a4da86060
commit 63e7b928cf
7 changed files with 675 additions and 109 deletions
--- a/common/config/eive/resultClassIds.h
+++ b/common/config/eive/resultClassIds.h
@ -32,7 +32,6 @@ enum commonClassIds : uint8_t {
  NVM_PARAM_BASE,           // NVMB
  FILE_SYSTEM_HELPER,       // FSHLP
  PLOC_MPSOC_HELPER,        // PLMPHLP
-  PLOC_MPSOC_COM,           // PLMPCOM
  SA_DEPL_HANDLER,          // SADPL
  MPSOC_RETURN_VALUES_IF,   // MPSOCRTVIF
  SUPV_RETURN_VALUES_IF,    // SPVRTVIF
@ -43,6 +42,7 @@ enum commonClassIds : uint8_t {
  PERSISTENT_TM_STORE,      // PTM
  TM_SINK,                  // TMS
  VIRTUAL_CHANNEL,          // VCS
+  PLOC_MPSOC_COM,           // PLMPCOM
  COMMON_CLASS_ID_END       // [EXPORT] : [END]
 };
 }
--- a/linux/payload/FreshMpsocHandler.cpp
+++ b/linux/payload/FreshMpsocHandler.cpp
@ -1,5 +1,6 @@
 #include "FreshMpsocHandler.h"

+#include "eive/objects.h"
 #include "fsfw/action/CommandActionHelper.h"
 #include "fsfw/devicehandlers/FreshDeviceHandlerBase.h"
 #include "fsfw/ipc/QueueFactory.h"
@ -18,6 +19,8 @@ FreshMpsocHandler::FreshMpsocHandler(DhbConfig cfg, MpsocCommunication& comInter
      hkReport(this) {
  commandActionHelperQueue = QueueFactory::instance()->createMessageQueue(10);
  eventQueue = QueueFactory::instance()->createMessageQueue(10);
+  spParams.maxSize = sizeof(commandBuffer);
+  spParams.buf = commandBuffer;
 }

 void FreshMpsocHandler::performDeviceOperation(uint8_t opCode) {
@ -25,6 +28,69 @@ void FreshMpsocHandler::performDeviceOperation(uint8_t opCode) {
    // Nothing to do for now.
    return;
  }
+
+  if (opCode == OpCode::DEFAULT_OPERATION) {
+    performDefaultDeviceOperation();
+  } else if (opCode == OpCode::PARSE_TM) {
+    // Just need to call this once, this should take care of processing the whole received
+    // Linux UART RX buffer.
+    comInterface.readSerialInterface();
+    // Handle all received packets.
+    while (true) {
+      ReturnValue_t result = comInterface.parseAndRetrieveNextPacket();
+      if (result == returnvalue::OK) {
+        break;
+      }
+    }
+  }
+}
+
+void FreshMpsocHandler::performDefaultDeviceOperation() {
+  if (transitionActive) {
+    if (targetMode == MODE_ON or targetMode == MODE_NORMAL) {
+      handleTransitionToOn();
+    } else if (targetMode == MODE_OFF) {
+      handleTransitionToOff();
+    } else {
+      // This should never happen.
+      sif::error << "FreshMpsocHandler: Invalid transition mode: " << targetMode << std::endl;
+      targetMode = MODE_OFF;
+      targetSubmode = 0;
+      handleTransitionToOff();
+    }
+  } else {
+    if (mode == MODE_NORMAL and not activeCmdInfo.pending) {
+      // TODO: Take care of regular periodic commanding here.
+    }
+  }
+
+  if (activeCmdInfo.pending and activeCmdInfo.cmdCountdown.hasTimedOut()) {
+    sif::warning << "PlocMpsocHandler: Command " << activeCmdInfo.pendingCmd << " has timed out"
+                 << std::endl;
+    activeCmdInfo.reset();
+    cmdDoneHandler(false, mpsoc::COMMAND_TIMEOUT);
+  }
+  EventMessage event;
+  for (ReturnValue_t result = eventQueue->receiveMessage(&event); result == returnvalue::OK;
+       result = eventQueue->receiveMessage(&event)) {
+    switch (event.getMessageId()) {
+      case EventMessage::EVENT_MESSAGE:
+        handleEvent(&event);
+        break;
+      default:
+        sif::debug << "PlocMPSoCHandler::performOperationHook: Did not subscribe to this event"
+                   << " message" << std::endl;
+        break;
+    }
+  }
+  CommandMessage message;
+  for (ReturnValue_t result = commandActionHelperQueue->receiveMessage(&message);
+       result == returnvalue::OK; result = commandActionHelperQueue->receiveMessage(&message)) {
+    result = commandActionHelper.handleReply(&message);
+    if (result == returnvalue::OK) {
+      continue;
+    }
+  }
 }

 ReturnValue_t FreshMpsocHandler::handleCommandMessage(CommandMessage* message) {
@ -71,7 +137,7 @@ ReturnValue_t FreshMpsocHandler::initialize() {
    return ObjectManagerIF::CHILD_INIT_FAILED;
  }

-  specialComHelper->setSequenceCount(&sequenceCount);
+  specialComHelper->setSequenceCount(&commandSequenceCount);
  result = commandActionHelper.initialize();
  if (result != returnvalue::OK) {
    return ObjectManagerIF::CHILD_INIT_FAILED;
@ -154,7 +220,7 @@ ReturnValue_t FreshMpsocHandler::executeAction(ActionId_t actionId, MessageQueue
  }

  if (specialComHelperExecuting) {
-    return MPSoCReturnValuesIF::MPSOC_HELPER_EXECUTING;
+    return mpsoc::MPSOC_HELPER_EXECUTING;
  }

  switch (actionId) {
@ -188,16 +254,15 @@ ReturnValue_t FreshMpsocHandler::executeAction(ActionId_t actionId, MessageQueue
      return EXECUTION_FINISHED;
    }
    case (mpsoc::OBSW_RESET_SEQ_COUNT): {
-      sequenceCount = 0;
+      commandSequenceCount = 0;
      return EXECUTION_FINISHED;
    }
    default:
      break;
  }
  // For longer commands, do not set these.
-  commandIsPending = true;
-  cmdCountdown.resetTimer();
  // TODO: Do all the stuff the form buildDeviceFromDevice blah did.
+  executeRegularCmd(actionId, commandedBy, data, size);
  return returnvalue::OK;
 }

@ -227,7 +292,7 @@ void FreshMpsocHandler::stepFailedReceived(ActionId_t actionId, uint8_t step,
      break;
    }
    case supv::SHUTDOWN_MPSOC: {
-      triggerEvent(MPSOC_SHUTDOWN_FAILED);
+      triggerEvent(mpsoc::MPSOC_SHUTDOWN_FAILED);
      sif::warning << "PlocMPSoCHandler::stepFailedReceived: Failed to shutdown MPSoC" << std::endl;
      break;
    }
@ -247,12 +312,13 @@ void FreshMpsocHandler::completionSuccessfulReceived(ActionId_t actionId) {
  if (actionId == supv::ACK_REPORT) {
    // I seriously don't know why this happens..
    //    sif::warning
-    //        << "PlocMpsocHandler::completionSuccessfulReceived: Only received ACK report. Consider
+    //        << "FreshMpsocHandler::completionSuccessfulReceived: Only received ACK report.
+    //        Consider
    //        "
    //           "increasing the MPSoC boot timer."
    //        << std::endl;
  } else if (actionId != supv::EXE_REPORT) {
-    sif::warning << "PlocMpsocHandler::completionSuccessfulReceived: Did not expect the action "
+    sif::warning << "FreshMpsocHandler::completionSuccessfulReceived: Did not expect the action "
                 << "ID " << actionId << std::endl;
    return;
  }
@ -296,7 +362,7 @@ void FreshMpsocHandler::handleActionCommandFailure(ActionId_t actionId) {
    }
    case PowerState::PENDING_SHUTDOWN: {
      // FDIR will intercept event and switch PLOC power off
-      triggerEvent(MPSOC_SHUTDOWN_FAILED);
+      triggerEvent(mpsoc::MPSOC_SHUTDOWN_FAILED);
      sif::warning << "PlocMPSoCHandler::handleActionCommandFailure: Failed to shutdown MPSoC"
                   << std::endl;
      break;
@ -307,3 +373,443 @@ void FreshMpsocHandler::handleActionCommandFailure(ActionId_t actionId) {
  powerState = PowerState::SUPV_FAILED;
  return;
 }
+
+ReturnValue_t FreshMpsocHandler::executeRegularCmd(ActionId_t actionId,
+                                                   MessageQueueId_t commandedBy,
+                                                   const uint8_t* commandData,
+                                                   size_t commandDataLen) {
+  ReturnValue_t result;
+  switch (actionId) {
+    case (mpsoc::TC_MEM_WRITE): {
+      result = prepareTcMemWrite(commandData, commandDataLen);
+      break;
+    }
+    case (mpsoc::TC_MEM_READ): {
+      result = prepareTcMemRead(commandData, commandDataLen);
+      break;
+    }
+    case (mpsoc::TC_FLASHDELETE): {
+      result = prepareTcFlashDelete(commandData, commandDataLen);
+      break;
+    }
+    case (mpsoc::TC_REPLAY_START): {
+      result = prepareTcReplayStart(commandData, commandDataLen);
+      break;
+    }
+    case (mpsoc::TC_REPLAY_STOP): {
+      result = prepareTcReplayStop();
+      break;
+    }
+    case (mpsoc::TC_DOWNLINK_PWR_ON): {
+      result = prepareTcDownlinkPwrOn(commandData, commandDataLen);
+      break;
+    }
+    case (mpsoc::TC_DOWNLINK_PWR_OFF): {
+      result = prepareTcDownlinkPwrOff();
+      break;
+    }
+    case (mpsoc::TC_REPLAY_WRITE_SEQUENCE): {
+      result = prepareTcReplayWriteSequence(commandData, commandDataLen);
+      break;
+    }
+    case (mpsoc::TC_GET_HK_REPORT): {
+      result = prepareTcGetHkReport();
+      break;
+    }
+    case (mpsoc::TC_FLASH_GET_DIRECTORY_CONTENT): {
+      result = prepareTcGetDirContent(commandData, commandDataLen);
+      break;
+    }
+    case (mpsoc::TC_MODE_REPLAY): {
+      result = prepareTcModeReplay();
+      break;
+    }
+    case (mpsoc::TC_MODE_IDLE): {
+      result = prepareTcModeIdle();
+      break;
+    }
+    case (mpsoc::TC_CAM_CMD_SEND): {
+      result = prepareTcCamCmdSend(commandData, commandDataLen);
+      break;
+    }
+    case (mpsoc::TC_CAM_TAKE_PIC): {
+      result = prepareTcCamTakePic(commandData, commandDataLen);
+      break;
+    }
+    case (mpsoc::TC_SIMPLEX_SEND_FILE): {
+      result = prepareTcSimplexSendFile(commandData, commandDataLen);
+      break;
+    }
+    case (mpsoc::TC_DOWNLINK_DATA_MODULATE): {
+      result = prepareTcDownlinkDataModulate(commandData, commandDataLen);
+      break;
+    }
+    case (mpsoc::TC_MODE_SNAPSHOT): {
+      result = prepareTcModeSnapshot();
+      break;
+    }
+    default:
+      sif::debug << "PlocMPSoCHandler::buildCommandFromCommand: Command not implemented"
+                 << std::endl;
+      result = DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
+      break;
+  }
+
+  if (result == returnvalue::OK) {
+    activeCmdInfo.start(actionId, commandedBy);
+    /**
+     * Flushing the receive buffer to make sure there are no data left from a faulty reply.
+     */
+    comInterface.getComHelper().flushUartRxBuffer();
+  }
+
+  return result;
+}
+
+ReturnValue_t FreshMpsocHandler::prepareTcMemWrite(const uint8_t* commandData,
+                                                   size_t commandDataLen) {
+  ReturnValue_t result = returnvalue::OK;
+  mpsoc::TcMemWrite tcMemWrite(spParams, commandSequenceCount);
+  result = tcMemWrite.setPayload(commandData, commandDataLen);
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  finishTcPrep(tcMemWrite);
+  return returnvalue::OK;
+}
+
+ReturnValue_t FreshMpsocHandler::prepareTcMemRead(const uint8_t* commandData,
+                                                  size_t commandDataLen) {
+  ReturnValue_t result = returnvalue::OK;
+  mpsoc::TcMemRead tcMemRead(spParams, commandSequenceCount);
+  result = tcMemRead.setPayload(commandData, commandDataLen);
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  finishTcPrep(tcMemRead);
+  tmMemReadReport.rememberRequestedSize = tcMemRead.getMemLen() * 4 + TmMemReadReport::FIX_SIZE;
+  return returnvalue::OK;
+}
+
+ReturnValue_t FreshMpsocHandler::prepareTcFlashDelete(const uint8_t* commandData,
+                                                      size_t commandDataLen) {
+  if (commandDataLen > config::MAX_PATH_SIZE + config::MAX_FILENAME_SIZE) {
+    return mpsoc::NAME_TOO_LONG;
+  }
+  ReturnValue_t result = returnvalue::OK;
+  mpsoc::TcFlashDelete tcFlashDelete(spParams, commandSequenceCount);
+  std::string filename = std::string(reinterpret_cast<const char*>(commandData), commandDataLen);
+  result = tcFlashDelete.setPayload(filename);
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  finishTcPrep(tcFlashDelete);
+  return returnvalue::OK;
+}
+
+ReturnValue_t FreshMpsocHandler::prepareTcReplayStart(const uint8_t* commandData,
+                                                      size_t commandDataLen) {
+  ReturnValue_t result = returnvalue::OK;
+  mpsoc::TcReplayStart tcReplayStart(spParams, commandSequenceCount);
+  result = tcReplayStart.setPayload(commandData, commandDataLen);
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  finishTcPrep(tcReplayStart);
+  return returnvalue::OK;
+}
+
+ReturnValue_t FreshMpsocHandler::prepareTcReplayStop() {
+  mpsoc::TcReplayStop tcReplayStop(spParams, commandSequenceCount);
+  finishTcPrep(tcReplayStop);
+  return returnvalue::OK;
+}
+
+ReturnValue_t FreshMpsocHandler::prepareTcDownlinkPwrOn(const uint8_t* commandData,
+                                                        size_t commandDataLen) {
+  ReturnValue_t result = returnvalue::OK;
+  mpsoc::TcDownlinkPwrOn tcDownlinkPwrOn(spParams, commandSequenceCount);
+  result = tcDownlinkPwrOn.setPayload(commandData, commandDataLen);
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  finishTcPrep(tcDownlinkPwrOn);
+  return returnvalue::OK;
+}
+
+ReturnValue_t FreshMpsocHandler::prepareTcDownlinkPwrOff() {
+  mpsoc::TcDownlinkPwrOff tcDownlinkPwrOff(spParams, commandSequenceCount);
+  finishTcPrep(tcDownlinkPwrOff);
+  return returnvalue::OK;
+}
+
+ReturnValue_t FreshMpsocHandler::prepareTcGetHkReport() {
+  mpsoc::TcGetHkReport tcGetHkReport(spParams, commandSequenceCount);
+  finishTcPrep(tcGetHkReport);
+  return returnvalue::OK;
+}
+
+ReturnValue_t FreshMpsocHandler::prepareTcReplayWriteSequence(const uint8_t* commandData,
+                                                              size_t commandDataLen) {
+  mpsoc::TcReplayWriteSeq tcReplayWriteSeq(spParams, commandSequenceCount);
+  ReturnValue_t result = tcReplayWriteSeq.setPayload(commandData, commandDataLen);
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  finishTcPrep(tcReplayWriteSeq);
+  return returnvalue::OK;
+}
+
+ReturnValue_t FreshMpsocHandler::prepareTcModeReplay() {
+  mpsoc::TcModeReplay tcModeReplay(spParams, commandSequenceCount);
+  finishTcPrep(tcModeReplay);
+  return returnvalue::OK;
+}
+
+ReturnValue_t FreshMpsocHandler::prepareTcModeIdle() {
+  mpsoc::TcModeIdle tcModeIdle(spParams, commandSequenceCount);
+  finishTcPrep(tcModeIdle);
+  return returnvalue::OK;
+}
+
+ReturnValue_t FreshMpsocHandler::prepareTcCamCmdSend(const uint8_t* commandData,
+                                                     size_t commandDataLen) {
+  mpsoc::TcCamcmdSend tcCamCmdSend(spParams, commandSequenceCount);
+  ReturnValue_t result = tcCamCmdSend.setPayload(commandData, commandDataLen);
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  finishTcPrep(tcCamCmdSend);
+  // TODO: Send this synchronously now..
+  // nextReplyId = mpsoc::TM_CAM_CMD_RPT;
+  return returnvalue::OK;
+}
+
+ReturnValue_t FreshMpsocHandler::prepareTcCamTakePic(const uint8_t* commandData,
+                                                     size_t commandDataLen) {
+  mpsoc::TcCamTakePic tcCamTakePic(spParams, commandSequenceCount);
+  ReturnValue_t result = tcCamTakePic.setPayload(commandData, commandDataLen);
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  finishTcPrep(tcCamTakePic);
+  return returnvalue::OK;
+}
+
+ReturnValue_t FreshMpsocHandler::prepareTcSimplexSendFile(const uint8_t* commandData,
+                                                          size_t commandDataLen) {
+  mpsoc::TcSimplexSendFile tcSimplexSendFile(spParams, commandSequenceCount);
+  ReturnValue_t result = tcSimplexSendFile.setPayload(commandData, commandDataLen);
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  finishTcPrep(tcSimplexSendFile);
+  return returnvalue::OK;
+}
+
+ReturnValue_t FreshMpsocHandler::prepareTcGetDirContent(const uint8_t* commandData,
+                                                        size_t commandDataLen) {
+  mpsoc::TcGetDirContent tcGetDirContent(spParams, commandSequenceCount);
+  ReturnValue_t result = tcGetDirContent.setPayload(commandData, commandDataLen);
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  finishTcPrep(tcGetDirContent);
+  return returnvalue::OK;
+}
+
+ReturnValue_t FreshMpsocHandler::prepareTcDownlinkDataModulate(const uint8_t* commandData,
+                                                               size_t commandDataLen) {
+  mpsoc::TcDownlinkDataModulate tcDownlinkDataModulate(spParams, commandSequenceCount);
+  ReturnValue_t result = tcDownlinkDataModulate.setPayload(commandData, commandDataLen);
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  finishTcPrep(tcDownlinkDataModulate);
+  return returnvalue::OK;
+}
+
+ReturnValue_t FreshMpsocHandler::prepareTcModeSnapshot() {
+  mpsoc::TcModeSnapshot tcModeSnapshot(spParams, commandSequenceCount);
+  finishTcPrep(tcModeSnapshot);
+  return returnvalue::OK;
+}
+
+ReturnValue_t FreshMpsocHandler::finishTcPrep(mpsoc::TcBase& tcBase) {
+  // TODO: Fix this, similar to how it was done for the SUPV. might be able to implement this
+  // in a simpler way, only allowing strictly sequential commanding.
+  // nextReplyId = mpsoc::ACK_REPORT;
+  ReturnValue_t result = tcBase.finishPacket();
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  // rawPacket = commandBuffer;
+  // rawPacketLen = tcBase.getFullPacketLen();
+  commandSequenceCount++;
+
+  if (DEBUG_MPSOC_COMMUNICATION) {
+    sif::debug << "SEND MPSOC packet. APID 0x" << std::hex << std::setw(3) << tcBase.getApid()
+               << " Size " << std::dec << tcBase.getFullPacketLen() << " SSC "
+               << tcBase.getSeqCount() << std::endl;
+  }
+  activeCmdInfo.cmdCountdown.resetTimer();
+  return returnvalue::OK;
+}
+
+void FreshMpsocHandler::handleEvent(EventMessage* eventMessage) {
+  // TODO: Shouldn't we check for specific events?
+  object_id_t objectId = eventMessage->getReporter();
+  switch (objectId) {
+    case objects::PLOC_MPSOC_HANDLER: {
+      specialComHelperExecuting = false;
+      break;
+    }
+    default:
+      sif::debug << "PlocMPSoCHandler::handleEvent: Did not subscribe to this event" << std::endl;
+      break;
+  }
+}
+
+void FreshMpsocHandler::cmdDoneHandler(bool success, ReturnValue_t result) {
+  activeCmdInfo.pending = false;
+  if (activeCmdInfo.commandedBy != MessageQueueIF::NO_QUEUE) {
+    actionHelper.finish(success, activeCmdInfo.commandedBy, activeCmdInfo.pendingCmd, result);
+  }
+}
+
+ReturnValue_t FreshMpsocHandler::handleDeviceReply() {
+  ReturnValue_t result = returnvalue::OK;
+
+  // SpacePacketReader spacePacket;
+  // spacePacket.setReadOnlyData(start, remainingSize);
+  auto& replyReader = comInterface.getSpReader();
+  if (DEBUG_MPSOC_COMMUNICATION) {
+    sif::debug << "RECV MPSOC packet. APID 0x" << std::hex << std::setw(3) << replyReader.getApid()
+               << std::dec << " Size " << replyReader.getFullPacketLen() << " SSC "
+               << replyReader.getSequenceCount() << std::endl;
+  }
+  if (replyReader.isNull()) {
+    return returnvalue::FAILED;
+  }
+  auto res = replyReader.checkSize();
+  if (res != returnvalue::OK) {
+    return res;
+  }
+  uint16_t apid = replyReader.getApid();
+
+  // auto handleDedicatedReply = [&](DeviceCommandId_t replyId) {
+  //*foundLen = spacePacket.getFullPacketLen();
+  // foundPacketLen = *foundLen;
+  // *foundId = replyId;
+  // };
+  switch (apid) {
+    case (mpsoc::apid::ACK_SUCCESS):
+      // *foundLen = mpsoc::SIZE_ACK_REPORT;
+      //*foundId = mpsoc::ACK_REPORT;
+      result = handleAckReport();
+
+      break;
+    case (mpsoc::apid::ACK_FAILURE):
+      // *foundLen = mpsoc::SIZE_ACK_REPORT;
+      //*foundId = mpsoc::ACK_REPORT;
+      break;
+    case (mpsoc::apid::TM_MEMORY_READ_REPORT):
+      // *foundLen = tmMemReadReport.rememberRequestedSize;
+      //*foundId = mpsoc::TM_MEMORY_READ_REPORT;
+      result = handleMemoryReadReport(packet);
+      break;
+    case (mpsoc::apid::TM_CAM_CMD_RPT):
+      // handleDedicatedReply(mpsoc::TM_CAM_CMD_RPT);
+      result = handleCamCmdRpt(packet);
+      break;
+    case (mpsoc::apid::TM_HK_GET_REPORT): {
+      // handleDedicatedReply(mpsoc::TM_GET_HK_REPORT);
+      result = handleGetHkReport(packet);
+      break;
+    }
+    case (mpsoc::apid::TM_FLASH_DIRECTORY_CONTENT): {
+      // handleDedicatedReply(mpsoc::TM_FLASH_DIRECTORY_CONTENT);
+      // result = verifyPacket(packet, foundPacketLen);
+      // if (result == MPSoCReturnValuesIF::CRC_FAILURE) {
+      // sif::warning << "PLOC MPSoC: Flash directory content reply invalid CRC" << std::endl;
+      // }
+      /** Send data to commanding queue */
+      handleDeviceTm(packet + mpsoc::DATA_FIELD_OFFSET,
+                     foundPacketLen - mpsoc::DATA_FIELD_OFFSET - mpsoc::CRC_SIZE,
+                     mpsoc::TM_FLASH_DIRECTORY_CONTENT);
+      // nextReplyId = mpsoc::EXE_REPORT;
+      return result;
+      break;
+    }
+    case (mpsoc::apid::EXE_SUCCESS):
+      //*foundLen = mpsoc::SIZE_EXE_REPORT;
+      //*foundId = mpsoc::EXE_REPORT;
+      result = handleExecutionReport(packet);
+      break;
+    case (mpsoc::apid::EXE_FAILURE):
+      //*foundLen = mpsoc::SIZE_EXE_REPORT;
+      //*foundId = mpsoc::EXE_REPORT;
+      result = handleExecutionReport(packet);
+      break;
+    default: {
+      sif::debug << "FreshMpsocHandler:: Reply has invalid APID 0x" << std::hex << std::setfill('0')
+                 << std::setw(2) << apid << std::dec << std::endl;
+      //*foundLen = remainingSize;
+      return mpsoc::INVALID_APID;
+    }
+  }
+
+  // TODO: Rework sequence count handling.
+  uint16_t sequenceCount = replyReader.getSequenceCount();
+  if (sequenceCount != lastReplySequenceCount + 1) {
+    // TODO: Trigger event for possible missing reply packet to inform operator.
+  }
+  lastReplySequenceCount = sequenceCount;
+
+  return result;
+}
+
+ReturnValue_t FreshMpsocHandler::handleAckReport() {
+  ReturnValue_t result = returnvalue::OK;
+  auto& replyReader = comInterface.getSpReader();
+
+  // result = verifyPacket(data, mpsoc::SIZE_ACK_REPORT);
+  // if (result == mpsoc::CRC_FAILURE) {
+  // sif::warning << "PlocMPSoCHandler::handleAckReport: CRC failure" << std::endl;
+  // nextReplyId = mpsoc::NONE;
+  // replyRawReplyIfnotWiretapped(data, mpsoc::SIZE_ACK_REPORT);
+  // triggerEvent(MPSOC_HANDLER_CRC_FAILURE);
+  // sendFailureReport(mpsoc::ACK_REPORT, MPSoCReturnValuesIF::CRC_FAILURE);
+  // disableAllReplies();
+  // return IGNORE_REPLY_DATA;
+  //}
+
+  // uint16_t apid = (*(data) << 8 | *(data + 1)) & APID_MASK;
+
+  switch (replyReader.getApid()) {
+    case mpsoc::apid::ACK_FAILURE: {
+      // DeviceCommandId_t commandId = getPendingCommand();
+      uint16_t status = mpsoc::getStatusFromRawData(replyReader.getFullData());
+      sif::warning << "MPSoC ACK Failure: " << mpsoc::getStatusString(status) << std::endl;
+      if (activeCmdInfo.pendingCmd != DeviceHandlerIF::NO_COMMAND_ID) {
+        triggerEvent(ACK_FAILURE, commandId, status);
+      }
+      sendFailureReport(mpsoc::ACK_REPORT, status);
+      disableAllReplies();
+      nextReplyId = mpsoc::NONE;
+      result = IGNORE_REPLY_DATA;
+      break;
+    }
+    case mpsoc::apid::ACK_SUCCESS: {
+      setNextReplyId();
+      break;
+    }
+    default: {
+      sif::debug << "PlocMPSoCHandler::handleAckReport: Invalid APID in Ack report" << std::endl;
+      result = returnvalue::FAILED;
+      break;
+    }
+  }
+
+  return result;
+}
--- a/linux/payload/FreshMpsocHandler.h
+++ b/linux/payload/FreshMpsocHandler.h
@ -1,12 +1,22 @@
+#include "eive/resultClassIds.h"
 #include "fsfw/action/CommandsActionsIF.h"
+#include "fsfw/devicehandlers/DeviceHandlerIF.h"
 #include "fsfw/devicehandlers/FreshDeviceHandlerBase.h"
+#include "fsfw/ipc/MessageQueueIF.h"
+#include "fsfw/ipc/messageQueueDefinitions.h"
+#include "fsfw/modes/ModeMessage.h"
 #include "fsfw/objectmanager/SystemObjectIF.h"
+#include "fsfw/returnvalues/returnvalue.h"
 #include "fsfw_hal/linux/gpio/Gpio.h"
 #include "linux/payload/MpsocCommunication.h"
 #include "linux/payload/PlocMpsocSpecialComHelper.h"

+static constexpr bool DEBUG_MPSOC_COMMUNICATION = false;
+
 class FreshMpsocHandler : public FreshDeviceHandlerBase, public CommandsActionsIF {
 public:
+  enum OpCode { DEFAULT_OPERATION = 0, PARSE_TM = 1 };
+
  FreshMpsocHandler(DhbConfig cfg, MpsocCommunication& comInterface,
                    PlocMpsocSpecialComHelper* plocMPSoCHelper, Gpio uartIsolatorSwitch,
                    object_id_t supervisorHandler);
@ -16,6 +26,8 @@ class FreshMpsocHandler : public FreshDeviceHandlerBase, public CommandsActionsI
   */
  void performDeviceOperation(uint8_t opCode) override;

+  void performDefaultDeviceOperation();
+
  /**
   * Implemented by child class. Handle all command messages which are
   * not health, mode, action or housekeeping messages.
@ -27,72 +39,14 @@ class FreshMpsocHandler : public FreshDeviceHandlerBase, public CommandsActionsI
  ReturnValue_t initialize() override;

 private:
-  static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::PLOC_MPSOC_HANDLER;
-
-  //! [EXPORT] : [COMMENT] PLOC crc failure in telemetry packet
-  static const Event MEMORY_READ_RPT_CRC_FAILURE = MAKE_EVENT(1, severity::LOW);
-  //! [EXPORT] : [COMMENT] PLOC receive acknowledgment failure report
-  //! P1: Command Id which leads the acknowledgment failure report
-  //! P2: The status field inserted by the MPSoC into the data field
-  static const Event ACK_FAILURE = MAKE_EVENT(2, severity::LOW);
-  //! [EXPORT] : [COMMENT] PLOC receive execution failure report
-  //! P1: Command Id which leads the execution failure report
-  //! P2: The status field inserted by the MPSoC into the data field
-  static const Event EXE_FAILURE = MAKE_EVENT(3, severity::LOW);
-  //! [EXPORT] : [COMMENT] PLOC reply has invalid crc
-  static const Event MPSOC_HANDLER_CRC_FAILURE = MAKE_EVENT(4, severity::LOW);
-  //! [EXPORT] : [COMMENT] Packet sequence count in received space packet does not match expected
-  //! count P1: Expected sequence count P2: Received sequence count
-  static const Event MPSOC_HANDLER_SEQUENCE_COUNT_MISMATCH = MAKE_EVENT(5, severity::LOW);
-  //! [EXPORT] : [COMMENT] Supervisor fails to shutdown MPSoC. Requires to power off the PLOC and
-  //! thus also to shutdown the supervisor.
-  static const Event MPSOC_SHUTDOWN_FAILED = MAKE_EVENT(6, severity::HIGH);
-  //! [EXPORT] : [COMMENT] SUPV not on for boot or shutdown process. P1: 0 for OFF transition, 1 for
-  //! ON transition.
-  static constexpr Event SUPV_NOT_ON = event::makeEvent(SUBSYSTEM_ID, 7, severity::LOW);
-  static constexpr Event SUPV_REPLY_TIMEOUT = event::makeEvent(SUBSYSTEM_ID, 8, severity::LOW);
-
  enum class StartupState { IDLE, HW_INIT, DONE } startupState = StartupState::IDLE;
  enum class PowerState { IDLE, PENDING_STARTUP, PENDING_SHUTDOWN, SUPV_FAILED, DONE };

-  // HK manager abstract functions.
-  LocalPoolDataSetBase* getDataSetHandle(sid_t sid) override;
-  ReturnValue_t initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
-                                        LocalDataPoolManager& poolManager) override;
-
-  // Mode abstract functions
-  ReturnValue_t checkModeCommand(Mode_t mode, Submode_t submode,
-                                 uint32_t* msToReachTheMode) override;
-  // Action override. Forward to user.
-  ReturnValue_t executeAction(ActionId_t actionId, MessageQueueId_t commandedBy,
-                              const uint8_t* data, size_t size) override;
-
-  /**
-   * @overload
-   * @param submode
-   */
-  void startTransition(Mode_t newMode, Submode_t submode) override;
-
-  ReturnValue_t performDeviceOperationPreQueueHandling(uint8_t opCode) override;
-
-  // CommandsActionsIF overrides.
-  MessageQueueIF* getCommandQueuePtr() override;
-
-  void stepSuccessfulReceived(ActionId_t actionId, uint8_t step) override;
-  void stepFailedReceived(ActionId_t actionId, uint8_t step, ReturnValue_t returnCode) override;
-  void dataReceived(ActionId_t actionId, const uint8_t* data, uint32_t size) override;
-  void completionSuccessfulReceived(ActionId_t actionId) override;
-  void completionFailedReceived(ActionId_t actionId, ReturnValue_t returnCode) override;
-
-  void handleActionCommandFailure(ActionId_t actionId);
-  void handleTransitionToOn();
-  void handleTransitionToOff();
-
  bool transitionActive = false;
  MpsocCommunication comInterface;
  MessageQueueIF* eventQueue = nullptr;
  PlocMpsocSpecialComHelper* specialComHelper;
-  SourceSequenceCounter sequenceCount = SourceSequenceCounter(0);
+  SourceSequenceCounter commandSequenceCount = SourceSequenceCounter(0);
  MessageQueueIF* commandActionHelperQueue = nullptr;
  CommandActionHelper commandActionHelper;
  Gpio uartIsolatorSwitch;
@ -139,6 +93,97 @@ class FreshMpsocHandler : public FreshDeviceHandlerBase, public CommandsActionsI
  PowerState powerState;
  bool specialComHelperExecuting = false;

-  Countdown cmdCountdown = Countdown(15000);
-  bool commandIsPending = false;
+  struct ActionCommandInfo {
+    Countdown cmdCountdown = Countdown(5000);
+    bool pending = false;
+    MessageQueueId_t commandedBy = MessageQueueIF::NO_QUEUE;
+    DeviceCommandId_t pendingCmd = DeviceHandlerIF::NO_COMMAND_ID;
+    uint16_t pendingCmdMpsocApid = 0;
+
+    void reset() {
+      pending = false;
+      commandedBy = MessageQueueIF::NO_QUEUE;
+      pendingCmd = DeviceHandlerIF::NO_COMMAND_ID;
+    }
+
+    void start(DeviceCommandId_t commandId, MessageQueueId_t commandedBy) {
+      pending = true;
+      cmdCountdown.resetTimer();
+      pendingCmd = commandId;
+      this->commandedBy = commandedBy;
+    }
+  } activeCmdInfo;
+
+  uint8_t commandBuffer[mpsoc::MAX_COMMAND_SIZE];
+  SpacePacketCreator creator;
+  ploc::SpTcParams spParams = ploc::SpTcParams(creator);
+  Mode_t targetMode = HasModesIF::MODE_UNDEFINED;
+  Submode_t targetSubmode = 0;
+
+  struct TmMemReadReport {
+    static const uint8_t FIX_SIZE = 14;
+    size_t rememberRequestedSize = 0;
+  };
+
+  TmMemReadReport tmMemReadReport;
+  uint32_t lastReplySequenceCount = 0;
+
+  // HK manager abstract functions.
+  LocalPoolDataSetBase* getDataSetHandle(sid_t sid) override;
+  ReturnValue_t initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
+                                        LocalDataPoolManager& poolManager) override;
+
+  // Mode abstract functions
+  ReturnValue_t checkModeCommand(Mode_t mode, Submode_t submode,
+                                 uint32_t* msToReachTheMode) override;
+  // Action override. Forward to user.
+  ReturnValue_t executeAction(ActionId_t actionId, MessageQueueId_t commandedBy,
+                              const uint8_t* data, size_t size) override;
+
+  /**
+   * @overload
+   * @param submode
+   */
+  void startTransition(Mode_t newMode, Submode_t submode) override;
+
+  ReturnValue_t performDeviceOperationPreQueueHandling(uint8_t opCode) override;
+
+  // CommandsActionsIF overrides.
+  MessageQueueIF* getCommandQueuePtr() override;
+
+  void stepSuccessfulReceived(ActionId_t actionId, uint8_t step) override;
+  void stepFailedReceived(ActionId_t actionId, uint8_t step, ReturnValue_t returnCode) override;
+  void dataReceived(ActionId_t actionId, const uint8_t* data, uint32_t size) override;
+  void completionSuccessfulReceived(ActionId_t actionId) override;
+  void completionFailedReceived(ActionId_t actionId, ReturnValue_t returnCode) override;
+
+  void handleActionCommandFailure(ActionId_t actionId);
+  ReturnValue_t executeRegularCmd(ActionId_t actionId, MessageQueueId_t commandedBy,
+                                  const uint8_t* data, size_t dataLen);
+  void handleTransitionToOn();
+  void handleTransitionToOff();
+
+  ReturnValue_t prepareTcModeReplay();
+  ReturnValue_t prepareTcMemWrite(const uint8_t* commandData, size_t commandDataLen);
+  ReturnValue_t prepareTcMemRead(const uint8_t* commandData, size_t commandDataLen);
+  ReturnValue_t prepareTcFlashDelete(const uint8_t* commandData, size_t commandDataLen);
+  ReturnValue_t prepareTcReplayStart(const uint8_t* commandData, size_t commandDataLen);
+  ReturnValue_t prepareTcReplayStop();
+  ReturnValue_t prepareTcDownlinkPwrOn(const uint8_t* commandData, size_t commandDataLen);
+  ReturnValue_t prepareTcDownlinkPwrOff();
+  ReturnValue_t prepareTcGetHkReport();
+  ReturnValue_t prepareTcGetDirContent(const uint8_t* commandData, size_t commandDataLen);
+  ReturnValue_t prepareTcReplayWriteSequence(const uint8_t* commandData, size_t commandDataLen);
+  ReturnValue_t prepareTcCamCmdSend(const uint8_t* commandData, size_t commandDataLen);
+  ReturnValue_t prepareTcModeIdle();
+  ReturnValue_t prepareTcCamTakePic(const uint8_t* commandData, size_t commandDataLen);
+  ReturnValue_t prepareTcSimplexSendFile(const uint8_t* commandData, size_t commandDataLen);
+  ReturnValue_t prepareTcDownlinkDataModulate(const uint8_t* commandData, size_t commandDataLen);
+  ReturnValue_t prepareTcModeSnapshot();
+
+  ReturnValue_t finishTcPrep(mpsoc::TcBase& tcBase);
+  void handleEvent(EventMessage* eventMessage);
+  void cmdDoneHandler(bool success, ReturnValue_t result);
+  ReturnValue_t handleDeviceReply();
+  ReturnValue_t handleAckReport();
 };
--- a/linux/payload/PlocMpsocHandler.h
+++ b/linux/payload/PlocMpsocHandler.h
@ -2,7 +2,6 @@
 #define BSP_Q7S_DEVICES_PLOC_PLOCMPSOCHANDLER_H_

 #include <linux/payload/PlocMpsocSpecialComHelperLegacy.h>
-#include <linux/payload/mpsocRetvals.h>
 #include <linux/payload/plocMpsocHelpers.h>
 #include <linux/payload/plocSupvDefs.h>
 #include <mission/controller/controllerdefinitions/PowerCtrlDefinitions.h>
--- a/linux/payload/mpsocRetvals.h
+++ b/linux/payload/mpsocRetvals.h
@ -1,33 +0,0 @@
-#ifndef MPSOC_RETURN_VALUES_IF_H_
-#define MPSOC_RETURN_VALUES_IF_H_
-
-#include "eive/resultClassIds.h"
-#include "fsfw/returnvalues/returnvalue.h"
-
-class MPSoCReturnValuesIF {
- public:
-  static const uint8_t INTERFACE_ID = CLASS_ID::MPSOC_RETURN_VALUES_IF;
-
-  //! [EXPORT] : [COMMENT] Space Packet received from PLOC has invalid CRC
-  static const ReturnValue_t CRC_FAILURE = MAKE_RETURN_CODE(0xA0);
-  //! [EXPORT] : [COMMENT] Received ACK failure reply from PLOC
-  static const ReturnValue_t RECEIVED_ACK_FAILURE = MAKE_RETURN_CODE(0xA1);
-  //! [EXPORT] : [COMMENT] Received execution failure reply from PLOC
-  static const ReturnValue_t RECEIVED_EXE_FAILURE = MAKE_RETURN_CODE(0xA2);
-  //! [EXPORT] : [COMMENT] Received space packet with invalid APID from PLOC
-  static const ReturnValue_t INVALID_APID = MAKE_RETURN_CODE(0xA3);
-  //! [EXPORT] : [COMMENT] Received command with invalid length
-  static const ReturnValue_t INVALID_LENGTH = MAKE_RETURN_CODE(0xA4);
-  //! [EXPORT] : [COMMENT] Filename of file in OBC filesystem is too long
-  static const ReturnValue_t FILENAME_TOO_LONG = MAKE_RETURN_CODE(0xA5);
-  //! [EXPORT] : [COMMENT] MPSoC helper is currently executing a command
-  static const ReturnValue_t MPSOC_HELPER_EXECUTING = MAKE_RETURN_CODE(0xA6);
-  //! [EXPORT] : [COMMENT] Filename of MPSoC file is to long (max. 256 bytes)
-  static const ReturnValue_t MPSOC_FILENAME_TOO_LONG = MAKE_RETURN_CODE(0xA7);
-  //! [EXPORT] : [COMMENT] Command has invalid parameter
-  static const ReturnValue_t INVALID_PARAMETER = MAKE_RETURN_CODE(0xA8);
-  //! [EXPORT] : [COMMENT] Received command has file string with invalid length
-  static const ReturnValue_t NAME_TOO_LONG = MAKE_RETURN_CODE(0xA9);
-};
-
-#endif /* MPSOC_RETURN_VALUES_IF_H_ */
--- a/linux/payload/plocMpsocHelpers.h
+++ b/linux/payload/plocMpsocHelpers.h
@ -3,7 +3,6 @@

 #include <fsfw/datapoollocal/StaticLocalDataSet.h>
 #include <fsfw/devicehandlers/DeviceHandlerIF.h>
-#include <linux/payload/mpsocRetvals.h>
 #include <mission/payload/plocSpBase.h>

 #include "eive/definitions.h"
@ -13,6 +12,56 @@

 namespace mpsoc {

+static const uint8_t INTERFACE_ID = CLASS_ID::MPSOC_RETURN_VALUES_IF;
+
+//! [EXPORT] : [COMMENT] Space Packet received from PLOC has invalid CRC
+static const ReturnValue_t CRC_FAILURE = MAKE_RETURN_CODE(0xA0);
+//! [EXPORT] : [COMMENT] Received ACK failure reply from PLOC
+static const ReturnValue_t RECEIVED_ACK_FAILURE = MAKE_RETURN_CODE(0xA1);
+//! [EXPORT] : [COMMENT] Received execution failure reply from PLOC
+static const ReturnValue_t RECEIVED_EXE_FAILURE = MAKE_RETURN_CODE(0xA2);
+//! [EXPORT] : [COMMENT] Received space packet with invalid APID from PLOC
+static const ReturnValue_t INVALID_APID = MAKE_RETURN_CODE(0xA3);
+//! [EXPORT] : [COMMENT] Received command with invalid length
+static const ReturnValue_t INVALID_LENGTH = MAKE_RETURN_CODE(0xA4);
+//! [EXPORT] : [COMMENT] Filename of file in OBC filesystem is too long
+static const ReturnValue_t FILENAME_TOO_LONG = MAKE_RETURN_CODE(0xA5);
+//! [EXPORT] : [COMMENT] MPSoC helper is currently executing a command
+static const ReturnValue_t MPSOC_HELPER_EXECUTING = MAKE_RETURN_CODE(0xA6);
+//! [EXPORT] : [COMMENT] Filename of MPSoC file is to long (max. 256 bytes)
+static const ReturnValue_t MPSOC_FILENAME_TOO_LONG = MAKE_RETURN_CODE(0xA7);
+//! [EXPORT] : [COMMENT] Command has invalid parameter
+static const ReturnValue_t INVALID_PARAMETER = MAKE_RETURN_CODE(0xA8);
+//! [EXPORT] : [COMMENT] Received command has file string with invalid length
+static const ReturnValue_t NAME_TOO_LONG = MAKE_RETURN_CODE(0xA9);
+//! [EXPORT] : [COMMENT] Command has timed out.
+static const ReturnValue_t COMMAND_TIMEOUT = MAKE_RETURN_CODE(0x10);
+
+static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::PLOC_MPSOC_HANDLER;
+
+//! [EXPORT] : [COMMENT] PLOC crc failure in telemetry packet
+static const Event MEMORY_READ_RPT_CRC_FAILURE = MAKE_EVENT(1, severity::LOW);
+//! [EXPORT] : [COMMENT] PLOC receive acknowledgment failure report
+//! P1: Command Id which leads the acknowledgment failure report
+//! P2: The status field inserted by the MPSoC into the data field
+static const Event ACK_FAILURE = MAKE_EVENT(2, severity::LOW);
+//! [EXPORT] : [COMMENT] PLOC receive execution failure report
+//! P1: Command Id which leads the execution failure report
+//! P2: The status field inserted by the MPSoC into the data field
+static const Event EXE_FAILURE = MAKE_EVENT(3, severity::LOW);
+//! [EXPORT] : [COMMENT] PLOC reply has invalid crc
+static const Event MPSOC_HANDLER_CRC_FAILURE = MAKE_EVENT(4, severity::LOW);
+//! [EXPORT] : [COMMENT] Packet sequence count in received space packet does not match expected
+//! count P1: Expected sequence count P2: Received sequence count
+static const Event MPSOC_HANDLER_SEQUENCE_COUNT_MISMATCH = MAKE_EVENT(5, severity::LOW);
+//! [EXPORT] : [COMMENT] Supervisor fails to shutdown MPSoC. Requires to power off the PLOC and
+//! thus also to shutdown the supervisor.
+static const Event MPSOC_SHUTDOWN_FAILED = MAKE_EVENT(6, severity::HIGH);
+//! [EXPORT] : [COMMENT] SUPV not on for boot or shutdown process. P1: 0 for OFF transition, 1 for
+//! ON transition.
+static constexpr Event SUPV_NOT_ON = event::makeEvent(SUBSYSTEM_ID, 7, severity::LOW);
+static constexpr Event SUPV_REPLY_TIMEOUT = event::makeEvent(SUBSYSTEM_ID, 8, severity::LOW);
+
 enum ParamId : uint8_t { SKIP_SUPV_ON_COMMANDING = 0x01 };

 enum FileAccessModes : uint8_t {
@ -232,7 +281,7 @@ static const uint16_t RESERVED_4 = 0x5F4;
 /**
 * @brief   Abstract base class for TC space packet of MPSoC.
 */
-class TcBase : public ploc::SpTcBase, public MPSoCReturnValuesIF {
+class TcBase : public ploc::SpTcBase {
 public:
  virtual ~TcBase() = default;

@ -710,7 +759,7 @@ class TcReplayWriteSeq : public TcBase {
 /**
 * @brief   Helps to extract the fields of the flash write command from the PUS packet.
 */
-class FlashBasePusCmd : public MPSoCReturnValuesIF {
+class FlashBasePusCmd {
 public:
  FlashBasePusCmd() = default;
  virtual ~FlashBasePusCmd() = default;
--- a/2
+++ b/2
@ -1 +1 @@
-Subproject commit 37eafb722b36ebabb50252121c80ef7712216486
+Subproject commit a8d0143b1ed9a14f7e071ee3344dc4e8f1937c55