looking good

fsfw

@@ -1 +1 @@
-Subproject commit 35be7da3534423af2b4db2b86e75371749181213
+Subproject commit bf7fac071c6d181251dbf9dee908728455be0925

linux/payload/FreshSupvHandler.cpp

@@ -1,5 +1,7 @@
 #include "FreshSupvHandler.h"
 
+#include <fsfw/datapool/PoolReadGuard.h>
+
 #include <atomic>
 
 using namespace supv;
@@ -7,52 +9,93 @@ using namespace returnvalue;
 
 std::atomic_bool supv::SUPV_ON = false;
 
-FreshSupvHandler::FreshSupvHandler(DhbConfig cfg, CookieIF *comCookie, Gpio uartIsolatorSwitch,
+FreshSupvHandler::FreshSupvHandler(DhbConfig cfg, CookieIF* comCookie, Gpio uartIsolatorSwitch,
-                                   PowerSwitchIF &switchIF, power::Switch_t powerSwitch,
+                                   PowerSwitchIF& switchIF, power::Switch_t powerSwitch,
-                                   PlocSupvUartManager &supvHelper)
+                                   PlocSupvUartManager& supvHelper)
     : FreshDeviceHandlerBase(cfg),
+      uartManager(supvHelper),
+      comCookie(comCookie),
+      switchIF(switchIF),
+      switchId(powerSwitch),
+      uartIsolatorSwitch(uartIsolatorSwitch),
       hkSet(this),
       bootStatusReport(this),
       latchupStatusReport(this),
       countersReport(this),
-      adcReport(this),
+      adcReport(this) {}
-      uartManager(supvHelper) {}
 
 void FreshSupvHandler::performDeviceOperation(uint8_t opCode) {
-  if (!transitionActive and mode == MODE_OFF) {
+  if (not transitionActive and mode == MODE_OFF) {
     // Nothing to do for now.
     return;
   }
   if (opCode == OpCode::DEFAULT_OPERATION) {
     if (transitionActive) {
-      // TODO: Perform transition handling: OFF to ON and ON to OFF.
+      if (targetMode == MODE_ON or targetMode == MODE_NORMAL) {
-      // For OFF to ON, command power switch first, then to isolato switch handling, and lastly
+        handleTransitionToOn();
-      // ensure succesfull communication
+      } else if (targetMode == MODE_OFF) {
-      // For ON to OFF ????
+        handleTransitionToOff();
+      } else {
+        // This should never happen.
+        sif::error << "FreshSupvHandler: Invalid transition mode: " << targetMode << std::endl;
+        targetMode = MODE_OFF;
+        targetSubmode = 0;
+        handleTransitionToOff();
+      }
     } else {
       if (mode == MODE_NORMAL) {
-        // Normal mode handling. Request normal datasets and add them to command map.
+        if (hkRequestCmdInfo.isPending and hkRequestCmdInfo.cmdCountdown.hasTimedOut()) {
+          // trigger event? might lead to spam...
+          sif::warning << "FreshSupvHandler: No reply received for HK set request" << std::endl;
+          hkRequestCmdInfo.isPending = false;
+        }
+        // Normal mode handling. Request normal data sets and add them to command map.
+        if (not hkRequestCmdInfo.isPending) {
+          hkRequestCmdInfo.cmdCountdown.resetTimer();
+          hkRequestCmdInfo.ackExeRecv = false;
+          hkRequestCmdInfo.ackRecv = false;
+          sendEmptyCmd(Apid::HK, static_cast<uint8_t>(tc::HkId::GET_REPORT));
+        }
       }
     }
-    // TODO: Check whether any active commands have timeouted.
   } else if (opCode == OpCode::HANDLE_ACTIVE_CMDS) {
-    // TODO: Parse TM from ring buffer and check whether they complete any active commands.
+    std::vector<ActionId_t> cmdsToRemove;
-    // If they do, check whether anyone needs to be informed.
+    for (auto& activeCmd : activeActionCmds) {
+      if (activeCmd.second.cmdCountdown.hasTimedOut()) {
+        if (activeCmd.second.commandedBy != MessageQueueIF::NO_QUEUE) {
+          actionHelper.finish(false, activeCmd.second.commandedBy, activeCmd.first,
+                              DeviceHandlerIF::TIMEOUT);
+        }
+        activeCmd.second.isPending = false;
+      }
+    }
+
+    parseTmPackets();
   }
 }
 
-ReturnValue_t FreshSupvHandler::handleCommandMessage(CommandMessage *message) {
+ReturnValue_t FreshSupvHandler::handleCommandMessage(CommandMessage* message) {
   // No custom messages.
   return returnvalue::FAILED;
 }
 
-LocalPoolDataSetBase *FreshSupvHandler::getDataSetHandle(sid_t sid) {
+LocalPoolDataSetBase* FreshSupvHandler::getDataSetHandle(sid_t sid) {
-  // TODO: return all relevant datasets.
+  if (sid == hkSet.getSid()) {
+    return &hkSet;
+  } else if (sid == bootStatusReport.getSid()) {
+    return &bootStatusReport;
+  } else if (sid == latchupStatusReport.getSid()) {
+    return &latchupStatusReport;
+  } else if (sid == countersReport.getSid()) {
+    return &countersReport;
+  } else if (sid == adcReport.getSid()) {
+    return &adcReport;
+  }
   return nullptr;
 }
 
-ReturnValue_t FreshSupvHandler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
+ReturnValue_t FreshSupvHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
-                                                        LocalDataPoolManager &poolManager) {
+                                                        LocalDataPoolManager& poolManager) {
   // TODO: Copy code from god handler here.
   localDataPoolMap.emplace(supv::NUM_TMS, new PoolEntry<uint32_t>({0}));
   localDataPoolMap.emplace(supv::TEMP_PS, new PoolEntry<uint32_t>({0}));
@@ -71,7 +114,7 @@ ReturnValue_t FreshSupvHandler::initializeLocalDataPool(localpool::DataPool &loc
   localDataPoolMap.emplace(supv::BR_SOC_STATE, new PoolEntry<uint8_t>({0}));
   localDataPoolMap.emplace(supv::POWER_CYCLES, new PoolEntry<uint8_t>({0}));
   localDataPoolMap.emplace(supv::BOOT_AFTER_MS, new PoolEntry<uint32_t>({0}));
-  localDataPoolMap.emplace(supv::BOOT_TIMEOUT_MS, new PoolEntry<uint32_t>({0}));
+  localDataPoolMap.emplace(supv::BOOT_TIMEOUT_POOL_VAR_MS, new PoolEntry<uint32_t>({0}));
   localDataPoolMap.emplace(supv::ACTIVE_NVM, new PoolEntry<uint8_t>({0}));
   localDataPoolMap.emplace(supv::BP0_STATE, new PoolEntry<uint8_t>({0}));
   localDataPoolMap.emplace(supv::BP1_STATE, new PoolEntry<uint8_t>({0}));
@@ -128,22 +171,556 @@ ReturnValue_t FreshSupvHandler::initializeLocalDataPool(localpool::DataPool &loc
   return returnvalue::OK;
 }
 
-ReturnValue_t FreshSupvHandler::setHealth(HealthState health) {
-  // TODO: Go to off is the device is commanded faulty.
-  return returnvalue::OK;
-}
-
 ReturnValue_t FreshSupvHandler::executeAction(ActionId_t actionId, MessageQueueId_t commandedBy,
-                                              const uint8_t *data, size_t size) {
+                                              const uint8_t* data, size_t size) {
   // TODO: Handle all commands here. Need to add them to some command map. Send command immediately
   // then.
+
+  using namespace supv;
+  ReturnValue_t result;
+  if (isCommandAlreadyActive(actionId)) {
+    return HasActionsIF::IS_BUSY;
+  }
+  spParams.buf = commandBuffer.data();
+  switch (actionId) {
+    case GET_HK_REPORT: {
+      sendEmptyCmd(Apid::HK, static_cast<uint8_t>(tc::HkId::GET_REPORT));
+      result = returnvalue::OK;
+      break;
+    }
+    case START_MPSOC: {
+      sif::info << "PLOC SUPV: Starting MPSoC" << std::endl;
+      sendEmptyCmd(Apid::BOOT_MAN, static_cast<uint8_t>(tc::BootManId::START_MPSOC));
+      result = returnvalue::OK;
+      break;
+    }
+    case SHUTDOWN_MPSOC: {
+      sif::info << "PLOC SUPV: Shutting down MPSoC" << std::endl;
+      sendEmptyCmd(Apid::BOOT_MAN, static_cast<uint8_t>(tc::BootManId::SHUTDOWN_MPSOC));
+      result = returnvalue::OK;
+      break;
+    }
+    case SEL_MPSOC_BOOT_IMAGE: {
+      prepareSelBootImageCmd(data);
+      result = returnvalue::OK;
+      break;
+    }
+    case RESET_MPSOC: {
+      sif::info << "PLOC SUPV: Resetting MPSoC" << std::endl;
+      sendEmptyCmd(Apid::BOOT_MAN, static_cast<uint8_t>(tc::BootManId::RESET_MPSOC));
+      result = returnvalue::OK;
+      break;
+    }
+    case SET_TIME_REF: {
+      result = prepareSetTimeRefCmd();
+      break;
+    }
+    case SET_BOOT_TIMEOUT: {
+      prepareSetBootTimeoutCmd(data, size);
+      result = returnvalue::OK;
+      break;
+    }
+    case SET_MAX_RESTART_TRIES: {
+      prepareRestartTriesCmd(data, size);
+      result = returnvalue::OK;
+      break;
+    }
+    case DISABLE_PERIOIC_HK_TRANSMISSION: {
+      prepareDisableHk();
+      result = returnvalue::OK;
+      break;
+    }
+    case GET_BOOT_STATUS_REPORT: {
+      sendEmptyCmd(Apid::BOOT_MAN, static_cast<uint8_t>(tc::BootManId::GET_BOOT_STATUS_REPORT));
+      result = returnvalue::OK;
+      break;
+    }
+    case ENABLE_LATCHUP_ALERT: {
+      result = prepareLatchupConfigCmd(data, actionId, size);
+      break;
+    }
+    case DISABLE_LATCHUP_ALERT: {
+      result = prepareLatchupConfigCmd(data, actionId, size);
+      break;
+    }
+    case SET_ALERT_LIMIT: {
+      result = prepareSetAlertLimitCmd(data, size);
+      break;
+    }
+    case GET_LATCHUP_STATUS_REPORT: {
+      sendEmptyCmd(Apid::LATCHUP_MON, static_cast<uint8_t>(tc::LatchupMonId::GET_STATUS_REPORT));
+      result = returnvalue::OK;
+      break;
+    }
+    case SET_GPIO: {
+      result = prepareSetGpioCmd(data, size);
+      break;
+    }
+    case FACTORY_RESET: {
+      result = prepareFactoryResetCmd(data, size);
+      break;
+    }
+    case READ_GPIO: {
+      result = prepareReadGpioCmd(data, size);
+      break;
+    }
+    case SET_SHUTDOWN_TIMEOUT: {
+      prepareSetShutdownTimeoutCmd(data, size);
+      result = returnvalue::OK;
+      break;
+    }
+    case FACTORY_FLASH: {
+      sendEmptyCmd(Apid::BOOT_MAN, static_cast<uint8_t>(tc::BootManId::FACTORY_FLASH));
+      result = returnvalue::OK;
+      break;
+    }
+    case RESET_PL: {
+      sendEmptyCmd(Apid::BOOT_MAN, static_cast<uint8_t>(tc::BootManId::RESET_PL));
+      result = returnvalue::OK;
+      break;
+    }
+    case SET_ADC_ENABLED_CHANNELS: {
+      prepareSetAdcEnabledChannelsCmd(data);
+      result = returnvalue::OK;
+      break;
+    }
+    case SET_ADC_WINDOW_AND_STRIDE: {
+      prepareSetAdcWindowAndStrideCmd(data);
+      result = returnvalue::OK;
+      break;
+    }
+    case SET_ADC_THRESHOLD: {
+      prepareSetAdcThresholdCmd(data);
+      result = returnvalue::OK;
+      break;
+    }
+    case WIPE_MRAM: {
+      result = prepareWipeMramCmd(data, size);
+      break;
+    }
+    case REQUEST_ADC_REPORT: {
+      sendEmptyCmd(Apid::ADC_MON, static_cast<uint8_t>(tc::AdcMonId::REQUEST_ADC_SAMPLE));
+      result = returnvalue::OK;
+      break;
+    }
+    case REQUEST_LOGGING_COUNTERS: {
+      sendEmptyCmd(Apid::DATA_LOGGER,
+                   static_cast<uint8_t>(tc::DataLoggerServiceId::REQUEST_COUNTERS));
+      result = returnvalue::OK;
+      break;
+    }
+    default:
+      sif::debug << "PlocSupervisorHandler::buildCommandFromCommand: Command not implemented"
+                 << std::endl;
+      result = DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
+      break;
+  }
+  return result;
+}
+
+ReturnValue_t FreshSupvHandler::prepareSetTimeRefCmd() {
+  Clock::TimeOfDay_t time;
+  ReturnValue_t result = Clock::getDateAndTime(&time);
+  if (result != returnvalue::OK) {
+    sif::warning << "PlocSupervisorHandler::prepareSetTimeRefCmd: Failed to get current time"
+                 << std::endl;
+    return result::GET_TIME_FAILURE;
+  }
+  supv::SetTimeRef packet(spParams);
+  result = packet.buildPacket(&time);
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  sendCommand(packet);
   return returnvalue::OK;
 }
 
-ReturnValue_t FreshSupvHandler::checkModeCommand(Mode_t mode_, Submode_t submode_,
+ReturnValue_t FreshSupvHandler::checkModeCommand(Mode_t commandedMode, Submode_t commandedSubmode,
-                                                 uint32_t *msToReachTheMode) {
+                                                 uint32_t* msToReachTheMode) {
-  if (mode_ != HasModesIF::MODE_OFF and mode_ != HasModesIF::MODE_ON and mode_ != MODE_NORMAL) {
+  if (commandedMode != MODE_OFF) {
+    PoolReadGuard pg(&enablePl);
+    if (pg.getReadResult() == returnvalue::OK) {
+      if (enablePl.plUseAllowed.isValid() and not enablePl.plUseAllowed.value) {
+        return NON_OP_STATE_OF_CHARGE;
+      }
+    }
+  }
+  if (commandedMode != HasModesIF::MODE_OFF and commandedMode != HasModesIF::MODE_ON and
+      commandedMode != MODE_NORMAL) {
     return returnvalue::FAILED;
   }
+  if (commandedMode != mode and msToReachTheMode != nullptr) {
+    if (commandedMode == MODE_OFF) {
+      *msToReachTheMode = supv::MAX_TRANSITION_TIME_TO_OFF_MS;
+    } else {
+      *msToReachTheMode = supv::MAX_TRANSITION_TIME_TO_ON_MS;
+    }
+  }
   return returnvalue::OK;
 }
+
+ReturnValue_t FreshSupvHandler::prepareSelBootImageCmd(const uint8_t* commandData) {
+  supv::MPSoCBootSelect packet(spParams);
+  ReturnValue_t result =
+      packet.buildPacket(commandData[0], commandData[1], commandData[2], commandData[3]);
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  sendCommand(packet);
+  return returnvalue::OK;
+}
+
+void FreshSupvHandler::startTransition(Mode_t newMode, Submode_t newSubmode) {
+  if (newMode == mode) {
+    // Can finish immediately.
+    setMode(newMode, newSubmode);
+    return;
+  }
+  // Transition for both OFF to ON/NORMAL and ON/NORMAL to OFF require small state machine.
+  transitionActive = true;
+  targetMode = newMode;
+  targetSubmode = newSubmode;
+  if (targetMode == MODE_ON or targetMode == MODE_NORMAL) {
+    startupState = StartupState::IDLE;
+  } else if (targetMode == MODE_OFF) {
+    shutdownState = ShutdownState::IDLE;
+  }
+}
+
+void FreshSupvHandler::handleTransitionToOn() {
+  if (startupState == StartupState::IDLE) {
+    bootTimeout.resetTimer();
+    startupState = StartupState::POWER_SWITCHING;
+    switchIF.sendSwitchCommand(switchId, PowerSwitchIF::SWITCH_ON);
+  } else {
+    if (modeHelper.isTimedOut()) {
+      targetMode = MODE_OFF;
+      shutdownState = ShutdownState::IDLE;
+      handleTransitionToOff();
+      return;
+    }
+  }
+  if (startupState == StartupState::POWER_SWITCHING) {
+    if (switchIF.getSwitchState(switchId) == PowerSwitchIF::SWITCH_ON) {
+      startupState = StartupState::BOOTING;
+    }
+  }
+  if (startupState == StartupState::BOOTING) {
+    if (bootTimeout.hasTimedOut()) {
+      uartIsolatorSwitch.pullHigh();
+      uartManager.start();
+      if (SET_TIME_DURING_BOOT) {
+        // TODO: Send command ,add command to command map.
+        startupState = StartupState::SET_TIME;
+      } else {
+        startupState = StartupState::ON;
+      }
+    }
+  }
+  if (startupState == StartupState::TIME_WAS_SET) {
+    startupState = StartupState::ON;
+  }
+  if (startupState == StartupState::ON) {
+    hkSet.setReportingEnabled(true);
+    supv::SUPV_ON = true;
+    setMode(targetMode);
+  }
+}
+
+void FreshSupvHandler::handleTransitionToOff() {
+  if (shutdownState == ShutdownState::IDLE) {
+    hkSet.setReportingEnabled(false);
+    hkSet.setValidity(false, true);
+    uartManager.stop();
+    uartIsolatorSwitch.pullLow();
+    switchIF.sendSwitchCommand(switchId, PowerSwitchIF::SWITCH_OFF);
+    shutdownState = ShutdownState::POWER_SWITCHING;
+  }
+  if (shutdownState == ShutdownState::POWER_SWITCHING) {
+    if (switchIF.getSwitchState(switchId) == PowerSwitchIF::SWITCH_OFF or modeHelper.isTimedOut()) {
+      supv::SUPV_ON = false;
+      setMode(MODE_OFF);
+    }
+  }
+}
+
+ReturnValue_t FreshSupvHandler::sendCommand(TcBase& tc) {
+  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;
+  }
+  return uartManager.sendMessage(comCookie, tc.getFullPacket(), tc.getFullPacketLen());
+}
+
+ReturnValue_t FreshSupvHandler::initialize() {
+  uartManager.initializeInterface(comCookie);
+  return FreshDeviceHandlerBase::initialize();
+}
+
+ReturnValue_t FreshSupvHandler::sendEmptyCmd(uint16_t apid, uint8_t serviceId) {
+  supv::NoPayloadPacket packet(spParams, apid, serviceId);
+  ReturnValue_t result = packet.buildPacket();
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  sendCommand(packet);
+  return returnvalue::OK;
+}
+
+ReturnValue_t FreshSupvHandler::prepareSetBootTimeoutCmd(const uint8_t* commandData,
+                                                         size_t cmdDataLen) {
+  if (cmdDataLen < 4) {
+    return HasActionsIF::INVALID_PARAMETERS;
+  }
+  supv::SetBootTimeout packet(spParams);
+  uint32_t timeout = *(commandData) << 24 | *(commandData + 1) << 16 | *(commandData + 2) << 8 |
+                     *(commandData + 3);
+  ReturnValue_t result = packet.buildPacket(timeout);
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  sendCommand(packet);
+  return returnvalue::OK;
+}
+
+ReturnValue_t FreshSupvHandler::prepareRestartTriesCmd(const uint8_t* commandData,
+                                                       size_t cmdDataLen) {
+  if (cmdDataLen < 1) {
+    return HasActionsIF::INVALID_PARAMETERS;
+  }
+  uint8_t restartTries = *(commandData);
+  supv::SetRestartTries packet(spParams);
+  ReturnValue_t result = packet.buildPacket(restartTries);
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  sendCommand(packet);
+  return returnvalue::OK;
+}
+
+ReturnValue_t FreshSupvHandler::prepareDisableHk() {
+  supv::DisablePeriodicHkTransmission packet(spParams);
+  ReturnValue_t result = packet.buildPacket();
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  sendCommand(packet);
+  return returnvalue::OK;
+}
+
+ReturnValue_t FreshSupvHandler::prepareLatchupConfigCmd(const uint8_t* commandData,
+                                                        DeviceCommandId_t deviceCommand,
+                                                        size_t cmdDataLen) {
+  ReturnValue_t result = returnvalue::OK;
+  if (cmdDataLen < 1) {
+    return HasActionsIF::INVALID_PARAMETERS;
+  }
+  uint8_t latchupId = *commandData;
+  if (latchupId > 6) {
+    return result::INVALID_LATCHUP_ID;
+  }
+  switch (deviceCommand) {
+    case (supv::ENABLE_LATCHUP_ALERT): {
+      supv::LatchupAlert packet(spParams);
+      result = packet.buildPacket(true, latchupId);
+      if (result != returnvalue::OK) {
+        return result;
+      }
+      sendCommand(packet);
+      break;
+    }
+    case (supv::DISABLE_LATCHUP_ALERT): {
+      supv::LatchupAlert packet(spParams);
+      result = packet.buildPacket(false, latchupId);
+      if (result != returnvalue::OK) {
+        return result;
+      }
+      sendCommand(packet);
+      break;
+    }
+    default: {
+      sif::debug << "PlocSupervisorHandler::prepareLatchupConfigCmd: Invalid command id"
+                 << std::endl;
+      result = returnvalue::FAILED;
+      break;
+    }
+  }
+  return result;
+}
+
+ReturnValue_t FreshSupvHandler::prepareSetAlertLimitCmd(const uint8_t* commandData,
+                                                        size_t cmdDataLen) {
+  if (cmdDataLen < 5) {
+    return HasActionsIF::INVALID_PARAMETERS;
+  }
+  uint8_t offset = 0;
+  uint8_t latchupId = *commandData;
+  offset += 1;
+  uint32_t dutycycle = *(commandData + offset) << 24 | *(commandData + offset + 1) << 16 |
+                       *(commandData + offset + 2) << 8 | *(commandData + offset + 3);
+  if (latchupId > 6) {
+    return result::INVALID_LATCHUP_ID;
+  }
+  supv::SetAlertlimit packet(spParams);
+  ReturnValue_t result = packet.buildPacket(latchupId, dutycycle);
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  sendCommand(packet);
+  return returnvalue::OK;
+}
+
+ReturnValue_t FreshSupvHandler::prepareSetShutdownTimeoutCmd(const uint8_t* commandData,
+                                                             size_t cmdDataLen) {
+  if (cmdDataLen < 4) {
+    return HasActionsIF::INVALID_PARAMETERS;
+  }
+  uint32_t timeout = 0;
+  ReturnValue_t result = returnvalue::OK;
+  size_t size = sizeof(timeout);
+  result =
+      SerializeAdapter::deSerialize(&timeout, &commandData, &size, SerializeIF::Endianness::BIG);
+  if (result != returnvalue::OK) {
+    sif::warning
+        << "PlocSupervisorHandler::prepareSetShutdownTimeoutCmd: Failed to deserialize timeout"
+        << std::endl;
+    return result;
+  }
+  supv::SetShutdownTimeout packet(spParams);
+  result = packet.buildPacket(timeout);
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  sendCommand(packet);
+  return returnvalue::OK;
+}
+
+ReturnValue_t FreshSupvHandler::prepareSetGpioCmd(const uint8_t* commandData,
+                                                  size_t commandDataLen) {
+  if (commandDataLen < 3) {
+    return HasActionsIF::INVALID_PARAMETERS;
+  }
+  uint8_t port = *commandData;
+  uint8_t pin = *(commandData + 1);
+  uint8_t val = *(commandData + 2);
+  supv::SetGpio packet(spParams);
+  ReturnValue_t result = packet.buildPacket(port, pin, val);
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  sendCommand(packet);
+  return returnvalue::OK;
+}
+
+ReturnValue_t FreshSupvHandler::prepareReadGpioCmd(const uint8_t* commandData,
+                                                   size_t commandDataLen) {
+  if (commandDataLen < 2) {
+    return HasActionsIF::INVALID_PARAMETERS;
+  }
+  uint8_t port = *commandData;
+  uint8_t pin = *(commandData + 1);
+  supv::ReadGpio packet(spParams);
+  ReturnValue_t result = packet.buildPacket(port, pin);
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  sendCommand(packet);
+  return returnvalue::OK;
+}
+
+ReturnValue_t FreshSupvHandler::prepareFactoryResetCmd(const uint8_t* commandData, size_t len) {
+  FactoryReset resetCmd(spParams);
+  if (len < 1) {
+    return HasActionsIF::INVALID_PARAMETERS;
+  }
+  ReturnValue_t result = resetCmd.buildPacket(commandData[0]);
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  sendCommand(resetCmd);
+  return returnvalue::OK;
+}
+
+ReturnValue_t FreshSupvHandler::prepareSetAdcEnabledChannelsCmd(const uint8_t* commandData) {
+  uint16_t ch = *(commandData) << 8 | *(commandData + 1);
+  supv::SetAdcEnabledChannels packet(spParams);
+  ReturnValue_t result = packet.buildPacket(ch);
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  sendCommand(packet);
+  return returnvalue::OK;
+}
+
+ReturnValue_t FreshSupvHandler::prepareSetAdcWindowAndStrideCmd(const uint8_t* commandData) {
+  uint8_t offset = 0;
+  uint16_t windowSize = *(commandData + offset) << 8 | *(commandData + offset + 1);
+  offset += 2;
+  uint16_t stridingStepSize = *(commandData + offset) << 8 | *(commandData + offset + 1);
+  supv::SetAdcWindowAndStride packet(spParams);
+  ReturnValue_t result = packet.buildPacket(windowSize, stridingStepSize);
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  sendCommand(packet);
+  return returnvalue::OK;
+}
+
+ReturnValue_t FreshSupvHandler::prepareSetAdcThresholdCmd(const uint8_t* commandData) {
+  uint32_t threshold = *(commandData) << 24 | *(commandData + 1) << 16 | *(commandData + 2) << 8 |
+                       *(commandData + 3);
+  supv::SetAdcThreshold packet(spParams);
+  ReturnValue_t result = packet.buildPacket(threshold);
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  sendCommand(packet);
+  return returnvalue::OK;
+}
+
+ReturnValue_t FreshSupvHandler::prepareWipeMramCmd(const uint8_t* commandData, size_t cmdDataLen) {
+  if (cmdDataLen < 8) {
+    return HasActionsIF::INVALID_PARAMETERS;
+  }
+  uint32_t start = 0;
+  uint32_t stop = 0;
+  size_t size = sizeof(start) + sizeof(stop);
+  SerializeAdapter::deSerialize(&start, &commandData, &size, SerializeIF::Endianness::BIG);
+  SerializeAdapter::deSerialize(&stop, &commandData, &size, SerializeIF::Endianness::BIG);
+  if ((stop - start) <= 0) {
+    return result::INVALID_MRAM_ADDRESSES;
+  }
+  supv::MramCmd packet(spParams);
+  ReturnValue_t result = packet.buildPacket(start, stop, supv::MramCmd::MramAction::WIPE);
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  sendCommand(packet);
+  return returnvalue::OK;
+}
+
+ReturnValue_t FreshSupvHandler::parseTmPackets() {
+  uint8_t* receivedData = nullptr;
+  size_t receivedSize = 0;
+  ReturnValue_t result;
+  while (true) {
+    result = uartManager.readReceivedMessage(comCookie, &receivedData, &receivedSize);
+    if (receivedSize == 0) {
+      break;
+    }
+    // TODO: Implement TM packet parsing and call corresponding handler functions or verify
+    //       sent commands.
+  }
+  return returnvalue::OK;
+}
+
+bool FreshSupvHandler::isCommandAlreadyActive(ActionId_t actionId) const {
+  auto iter = activeActionCmds.find(actionId);
+  if (iter == activeActionCmds.end()) {
+    return false;
+  }
+  if (iter->second.isPending) {
+    return true;
+  }
+  return false;
+}

linux/payload/FreshSupvHandler.h

@@ -2,6 +2,9 @@
 #define LINUX_PAYLOAD_FRESHSUPVHANDLER_H_
 
 #include <fsfw/power/PowerSwitchIF.h>
+#include <mission/controller/controllerdefinitions/PowerCtrlDefinitions.h>
+
+#include <map>
 
 #include "PlocSupvUartMan.h"
 #include "fsfw/devicehandlers/FreshDeviceHandlerBase.h"
@@ -9,6 +12,10 @@
 #include "fsfw_hal/linux/gpio/Gpio.h"
 #include "plocSupvDefs.h"
 
+using supv::TcBase;
+
+static constexpr bool DEBUG_PLOC_SUPV = false;
+
 class FreshSupvHandler : public FreshDeviceHandlerBase {
  public:
   enum OpCode { DEFAULT_OPERATION = 0, HANDLE_ACTIVE_CMDS = 1 };
@@ -29,9 +36,9 @@ class FreshSupvHandler : public FreshDeviceHandlerBase {
    */
   ReturnValue_t handleCommandMessage(CommandMessage* message) override;
 
- private:
+  ReturnValue_t initialize() override;
-  ReturnValue_t setHealth(HealthState health) override;
 
+ private:
   // HK manager abstract functions.
   LocalPoolDataSetBase* getDataSetHandle(sid_t sid) override;
   ReturnValue_t initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
@@ -44,8 +51,38 @@ class FreshSupvHandler : public FreshDeviceHandlerBase {
   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;
+
+  void handleTransitionToOn();
+  void handleTransitionToOff();
+
  private:
+  static constexpr bool SET_TIME_DURING_BOOT = true;
+
+  enum class StartupState : uint8_t {
+    IDLE,
+    POWER_SWITCHING,
+    BOOTING,
+    SET_TIME,
+    WAIT_FOR_TIME_REPLY,
+    TIME_WAS_SET,
+    ON
+  };
+
+  StartupState startupState = StartupState::IDLE;
+
+  enum class ShutdownState : uint8_t { IDLE, POWER_SWITCHING };
+  ShutdownState shutdownState = ShutdownState::IDLE;
+
   PlocSupvUartManager uartManager;
+  CookieIF* comCookie;
+  PowerSwitchIF& switchIF;
+  power::Switch_t switchId;
+  Gpio uartIsolatorSwitch;
 
   supv::HkSet hkSet;
   supv::BootStatusReport bootStatusReport;
@@ -55,6 +92,12 @@ class FreshSupvHandler : public FreshDeviceHandlerBase {
 
   bool transitionActive = false;
 
+  Mode_t targetMode = HasModesIF::MODE_INVALID;
+  Submode_t targetSubmode = 0;
+
+  // Vorago nees some time to boot properly
+  Countdown bootTimeout = Countdown(supv::BOOT_TIMEOUT_MS);
+
   PoolEntry<uint16_t> adcRawEntry = PoolEntry<uint16_t>(16);
   PoolEntry<uint16_t> adcEngEntry = PoolEntry<uint16_t>(16);
   PoolEntry<uint32_t> latchupCounters = PoolEntry<uint32_t>(7);
@@ -62,6 +105,51 @@ class FreshSupvHandler : public FreshDeviceHandlerBase {
   PoolEntry<uint8_t> bootStateEntry = PoolEntry<uint8_t>(1);
   PoolEntry<uint8_t> bootCyclesEntry = PoolEntry<uint8_t>(1);
   PoolEntry<uint32_t> tempSupEntry = PoolEntry<uint32_t>(1);
+
+  pwrctrl::EnablePl enablePl = pwrctrl::EnablePl(objects::POWER_CONTROLLER);
+
+  struct ActiveCmdInfo {
+    ActiveCmdInfo(uint32_t cmdCountdownMs) : cmdCountdown(cmdCountdownMs) {}
+
+    bool isPending = false;
+    bool ackRecv = false;
+    bool ackExeRecv = false;
+    MessageQueueId_t commandedBy = MessageQueueIF::NO_QUEUE;
+    bool requiresActionReply = false;
+    Countdown cmdCountdown;
+  };
+
+  // Map for Action commands. For normal commands, a separate static structure will be used.
+  std::map<ActionId_t, ActiveCmdInfo> activeActionCmds;
+
+  ActiveCmdInfo hkRequestCmdInfo = ActiveCmdInfo(500);
+
+  std::array<uint8_t, supv::MAX_COMMAND_SIZE> commandBuffer;
+  SpacePacketCreator creator;
+  supv::TcParams spParams = supv::TcParams(creator);
+
+  ReturnValue_t parseTmPackets();
+
+  ReturnValue_t sendCommand(TcBase& tc);
+  ReturnValue_t sendEmptyCmd(uint16_t apid, uint8_t serviceId);
+  ReturnValue_t prepareSelBootImageCmd(const uint8_t* commandData);
+  ReturnValue_t prepareSetTimeRefCmd();
+  ReturnValue_t prepareSetBootTimeoutCmd(const uint8_t* commandData, size_t cmdDataLen);
+  ReturnValue_t prepareRestartTriesCmd(const uint8_t* commandData, size_t cmdDataLen);
+  ReturnValue_t prepareDisableHk();
+  ReturnValue_t prepareLatchupConfigCmd(const uint8_t* commandData, DeviceCommandId_t deviceCommand,
+                                        size_t cmdDataLen);
+  ReturnValue_t prepareSetAlertLimitCmd(const uint8_t* commandData, size_t cmdDataLen);
+  ReturnValue_t prepareFactoryResetCmd(const uint8_t* commandData, size_t len);
+  ReturnValue_t prepareSetShutdownTimeoutCmd(const uint8_t* commandData, size_t cmdDataLen);
+  ReturnValue_t prepareSetGpioCmd(const uint8_t* commandData, size_t commandDataLen);
+  ReturnValue_t prepareReadGpioCmd(const uint8_t* commandData, size_t commandDataLen);
+  ReturnValue_t prepareSetAdcEnabledChannelsCmd(const uint8_t* commandData);
+  ReturnValue_t prepareSetAdcWindowAndStrideCmd(const uint8_t* commandData);
+  ReturnValue_t prepareSetAdcThresholdCmd(const uint8_t* commandData);
+  ReturnValue_t prepareWipeMramCmd(const uint8_t* commandData, size_t cmdDataLen);
+
+  bool isCommandAlreadyActive(ActionId_t actionId) const;
 };
 
 #endif /* LINUX_PAYLOAD_FRESHSUPVHANDLER_H_ */

linux/payload/PlocSupervisorHandler.cpp

@@ -1518,6 +1518,7 @@ ReturnValue_t PlocSupervisorHandler::prepareSetShutdownTimeoutCmd(const uint8_t*
     sif::warning
         << "PlocSupervisorHandler::prepareSetShutdownTimeoutCmd: Failed to deserialize timeout"
         << std::endl;
+    return result;
   }
   supv::SetShutdownTimeout packet(spParams);
   result = packet.buildPacket(timeout);

linux/payload/PlocSupervisorHandler.h

@@ -102,8 +102,7 @@ class PlocSupervisorHandler : public DeviceHandlerBase {
   static const uint32_t COPY_ADC_TO_MRAM_TIMEOUT = 70000;
   // 60 s
   static const uint32_t MRAM_DUMP_TIMEOUT = 60000;
-  // 4 s
+
-  static const uint32_t BOOT_TIMEOUT = 4000;
   enum class StartupState : uint8_t {
     OFF,
     BOOTING,
@@ -172,7 +171,7 @@ class PlocSupervisorHandler : public DeviceHandlerBase {
   Countdown executionReportTimeout = Countdown(EXECUTION_DEFAULT_TIMEOUT, false);
   Countdown acknowledgementReportTimeout = Countdown(ACKNOWLEDGE_DEFAULT_TIMEOUT, false);
   // Vorago nees some time to boot properly
-  Countdown bootTimeout = Countdown(BOOT_TIMEOUT);
+  Countdown bootTimeout = Countdown(supv::BOOT_TIMEOUT_MS);
   Countdown mramDumpTimeout = Countdown(MRAM_DUMP_TIMEOUT);
 
   PoolEntry<uint16_t> adcRawEntry = PoolEntry<uint16_t>(16);

linux/payload/PlocSupvUartMan.h

@@ -121,6 +121,32 @@ class PlocSupvUartManager : public DeviceCommunicationIF,
 
   PlocSupvUartManager(object_id_t objectId);
   virtual ~PlocSupvUartManager();
+  /**
+   * @brief Device specific initialization, using the cookie.
+   * @details
+   * The cookie is already prepared in the factory. If the communication
+   * interface needs to be set up in some way and requires cookie information,
+   * this can be performed in this function, which is called on device handler
+   * initialization.
+   * @param cookie
+   * @return
+   *  - @c returnvalue::OK if initialization was successfull
+   *  - Everything else triggers failure event with returnvalue as parameter 1
+   */
+  ReturnValue_t initializeInterface(CookieIF* cookie) override;
+  /**
+   * Called by DHB in the SEND_WRITE doSendWrite().
+   * This function is used to send data to the physical device
+   * by implementing and calling related drivers or wrapper functions.
+   * @param cookie
+   * @param data
+   * @param len If this is 0, nothing shall be sent.
+   * @return
+   *  - @c returnvalue::OK for successfull send
+   *  - Everything else triggers failure event with returnvalue as parameter 1
+   */
+  ReturnValue_t sendMessage(CookieIF* cookie, const uint8_t* sendData, size_t sendLen) override;
+  ReturnValue_t readReceivedMessage(CookieIF* cookie, uint8_t** buffer, size_t* size) override;
 
   ReturnValue_t initialize() override;
   ReturnValue_t performOperation(uint8_t operationCode = 0) override;
@@ -319,32 +345,6 @@ class PlocSupvUartManager : public DeviceCommunicationIF,
   void resetSpParams();
   void pushIpcData(const uint8_t* data, size_t len);
 
-  /**
-   * @brief Device specific initialization, using the cookie.
-   * @details
-   * The cookie is already prepared in the factory. If the communication
-   * interface needs to be set up in some way and requires cookie information,
-   * this can be performed in this function, which is called on device handler
-   * initialization.
-   * @param cookie
-   * @return
-   *  - @c returnvalue::OK if initialization was successfull
-   *  - Everything else triggers failure event with returnvalue as parameter 1
-   */
-  ReturnValue_t initializeInterface(CookieIF* cookie) override;
-
-  /**
-   * Called by DHB in the SEND_WRITE doSendWrite().
-   * This function is used to send data to the physical device
-   * by implementing and calling related drivers or wrapper functions.
-   * @param cookie
-   * @param data
-   * @param len If this is 0, nothing shall be sent.
-   * @return
-   *  - @c returnvalue::OK for successfull send
-   *  - Everything else triggers failure event with returnvalue as parameter 1
-   */
-  ReturnValue_t sendMessage(CookieIF* cookie, const uint8_t* sendData, size_t sendLen) override;
   /**
    * Called by DHB in the GET_WRITE doGetWrite().
    * Get send confirmation that the data in sendMessage() was sent successfully.
@@ -369,7 +369,6 @@ class PlocSupvUartManager : public DeviceCommunicationIF,
    *           returnvalue as parameter 1
    */
   ReturnValue_t requestReceiveMessage(CookieIF* cookie, size_t requestLen) override;
-  ReturnValue_t readReceivedMessage(CookieIF* cookie, uint8_t** buffer, size_t* size) override;
 
   void performUartShutdown();
   void updateVtime(uint8_t vtime);

linux/payload/plocSupvDefs.h

@@ -18,6 +18,9 @@
 namespace supv {
 
 extern std::atomic_bool SUPV_ON;
+static constexpr uint32_t BOOT_TIMEOUT_MS = 4000;
+static constexpr uint32_t MAX_TRANSITION_TIME_TO_ON_MS = 6000;
+static constexpr uint32_t MAX_TRANSITION_TIME_TO_OFF_MS = 1000;
 
 namespace result {
 static const uint8_t INTERFACE_ID = CLASS_ID::SUPV_RETURN_VALUES_IF;
@@ -400,7 +403,7 @@ enum PoolIds : lp_id_t {
   BR_SOC_STATE,
   POWER_CYCLES,
   BOOT_AFTER_MS,
-  BOOT_TIMEOUT_MS,
+  BOOT_TIMEOUT_POOL_VAR_MS,
   ACTIVE_NVM,
   BP0_STATE,
   BP1_STATE,
@@ -1727,7 +1730,7 @@ class BootStatusReport : public StaticLocalDataSet<BOOT_REPORT_SET_ENTRIES> {
   lp_var_t<uint32_t> bootAfterMs = lp_var_t<uint32_t>(sid.objectId, PoolIds::BOOT_AFTER_MS, this);
   /** The currently set boot timeout */
   lp_var_t<uint32_t> bootTimeoutMs =
-      lp_var_t<uint32_t>(sid.objectId, PoolIds::BOOT_TIMEOUT_MS, this);
+      lp_var_t<uint32_t>(sid.objectId, PoolIds::BOOT_TIMEOUT_POOL_VAR_MS, this);
   lp_var_t<uint8_t> activeNvm = lp_var_t<uint8_t>(sid.objectId, PoolIds::ACTIVE_NVM, this);
   /** States of the boot partition pins */
   lp_var_t<uint8_t> bp0State = lp_var_t<uint8_t>(sid.objectId, PoolIds::BP0_STATE, this);