Merge remote-tracking branch 'origin/develop' into scex-additions

bsp_q7s/core/CoreController.cpp

@@ -1,6 +1,7 @@
 #include "CoreController.h"
 
 #include <fsfw/events/EventManager.h>
+#include <fsfw/filesystem/HasFileSystemIF.h>
 #include <fsfw/ipc/QueueFactory.h>
 
 #include "OBSWConfig.h"
@@ -20,9 +21,10 @@
 #include <algorithm>
 #include <filesystem>
 
-#include "bsp_q7s/memory/SdCardManager.h"
+#include "bsp_q7s/fs/SdCardManager.h"
 #include "bsp_q7s/memory/scratchApi.h"
 #include "bsp_q7s/xadc/Xadc.h"
+#include "eive/definitions.h"
 #include "linux/utility/utility.h"
 
 xsc::Chip CoreController::CURRENT_CHIP = xsc::Chip::NO_CHIP;
@@ -47,7 +49,6 @@ CoreController::CoreController(object_id_t objectId)
     if (not BLOCKING_SD_INIT) {
       sdcMan->setBlocking(false);
     }
-    sdStateMachine();
 
     result = initBootCopy();
     if (result != returnvalue::OK) {
@@ -60,6 +61,8 @@ CoreController::CoreController(object_id_t objectId)
   eventQueue = QueueFactory::instance()->createMessageQueue(5, EventMessage::MAX_MESSAGE_SIZE);
 }
 
+CoreController::~CoreController() {}
+
 ReturnValue_t CoreController::handleCommandMessage(CommandMessage *message) {
   return ExtendedControllerBase::handleCommandMessage(message);
 }
@@ -139,12 +142,18 @@ ReturnValue_t CoreController::initialize() {
 
 ReturnValue_t CoreController::initializeAfterTaskCreation() {
   ReturnValue_t result = returnvalue::OK;
-  auto sdCard = sdcMan->getPreferredSdCard();
+  auto sdCard =   sdcMan->setActiveSdCard(sdInfo.pref);
   if (not sdCard) {
     return returnvalue::FAILED;
   }
-  sdInfo.pref = sdCard.value();
-  sdcMan->setActiveSdCard(sdInfo.pref);
+  sdInfo.active = sdCard.value();
+  if (sdInfo.active == sd::SdCard::NONE) {
+    sif::error << "CoreController::initializeAfterTaskCreation: "
+                  "Issues getting preferred SD card, setting to 0"
+               << std::endl;
+    sdInfo.active = sd::SdCard::SLOT_0;
+  }
+  sdcMan->setActiveSdCard(sdInfo.active);
   currMntPrefix = sdcMan->getCurrentMountPrefix();
   if (BLOCKING_SD_INIT) {
     ReturnValue_t result = initSdCardBlocking();
@@ -202,6 +211,47 @@ ReturnValue_t CoreController::executeAction(ActionId_t actionId, MessageQueueId_
       }
       return HasActionsIF::EXECUTION_FINISHED;
     }
+    case (OBSW_UPDATE_FROM_SD_0): {
+      return executeSwUpdate(SwUpdateSources::SD_0, data, size);
+    }
+    case (OBSW_UPDATE_FROM_SD_1): {
+      return executeSwUpdate(SwUpdateSources::SD_1, data, size);
+    }
+    case (OBSW_UPDATE_FROM_TMP): {
+      return executeSwUpdate(SwUpdateSources::TMP_DIR, data, size);
+    }
+    case (SWITCH_TO_SD_0): {
+      if (not startSdStateMachine(sd::SdCard::SLOT_0, SdCfgMode::COLD_REDUNDANT, commandedBy,
+                                  actionId)) {
+        return HasActionsIF::IS_BUSY;
+      }
+      // Completion will be reported by SD card state machine
+      return returnvalue::OK;
+    }
+    case (SWITCH_TO_SD_1): {
+      if (not startSdStateMachine(sd::SdCard::SLOT_1, SdCfgMode::COLD_REDUNDANT, commandedBy,
+                                  actionId)) {
+        return HasActionsIF::IS_BUSY;
+      }
+      // Completion will be reported by SD card state machine
+      return returnvalue::OK;
+    }
+    case (SWITCH_TO_BOTH_SD_CARDS): {
+      // An active SD still needs to be specified because the system needs to know which SD
+      // card to use for regular operations like telemetry storage.
+      if (size != 1) {
+        return HasActionsIF::INVALID_PARAMETERS;
+      }
+      if (data[0] != 0 and data[0] != 1) {
+        return HasActionsIF::INVALID_PARAMETERS;
+      }
+      auto active = static_cast<sd::SdCard>(data[0]);
+      if (not startSdStateMachine(active, SdCfgMode::HOT_REDUNDANT, commandedBy, actionId)) {
+        return HasActionsIF::IS_BUSY;
+      }
+      // Completion will be reported by SD card state machine
+      return returnvalue::OK;
+    }
     case (SWITCH_IMG_LOCK): {
       if (size != 3) {
         return HasActionsIF::INVALID_PARAMETERS;
@@ -249,60 +299,59 @@ ReturnValue_t CoreController::initSdCardBlocking() {
   if (result != returnvalue::OK) {
     sif::warning << "CoreController::initialize: Updating SD card state file failed" << std::endl;
   }
-#if Q7S_SD_CARD_CONFIG == Q7S_SD_NONE
-  sif::info << "No SD card initialization will be performed" << std::endl;
-  return returnvalue::OK;
-#else
+  if (sdInfo.cfgMode == SdCfgMode::PASSIVE) {
+    sif::info << "No SD card initialization will be performed" << std::endl;
+    return returnvalue::OK;
+  }
 
   result = sdcMan->getSdCardsStatus(sdInfo.currentState);
   if (result != returnvalue::OK) {
     sif::warning << "Getting SD card activity status failed" << std::endl;
   }
 
-#if Q7S_SD_CARD_CONFIG == Q7S_SD_COLD_REDUNDANT
-  updateSdInfoOther();
-  sif::info << "Cold redundant SD card configuration, preferred SD card: "
-            << static_cast<int>(sdInfo.pref) << std::endl;
-  result = sdColdRedundantBlockingInit();
-  // Update status file
-  sdcMan->updateSdCardStateFile();
-  return result;
-#elif Q7S_SD_CARD_CONFIG == Q7S_SD_HOT_REDUNDANT
-  sif::info << "Hot redundant SD card configuration" << std::endl;
-  sdCardSetup(sd::SdCard::SLOT_0, sd::SdState::MOUNTED, "0", false);
-  sdCardSetup(sd::SdCard::SLOT_1, sd::SdState::MOUNTED, "1", false);
-  // Update status file
-  sdcMan->updateSdCardStateFile();
+  if (sdInfo.cfgMode == SdCfgMode::COLD_REDUNDANT) {
+    updateSdInfoOther();
+    sif::info << "Cold redundant SD card configuration, preferred SD card: "
+              << static_cast<int>(sdInfo.active) << std::endl;
+    result = sdColdRedundantBlockingInit();
+    // Update status file
+    sdcMan->updateSdCardStateFile();
+    return result;
+  }
+  if (sdInfo.cfgMode == SdCfgMode::HOT_REDUNDANT) {
+    sif::info << "Hot redundant SD card configuration" << std::endl;
+    sdCardSetup(sd::SdCard::SLOT_0, sd::SdState::MOUNTED, "0", false);
+    sdCardSetup(sd::SdCard::SLOT_1, sd::SdState::MOUNTED, "1", false);
+    // Update status file
+    sdcMan->updateSdCardStateFile();
+  }
   return returnvalue::OK;
-#endif
-
-#endif /* Q7S_SD_CARD_CONFIG != Q7S_SD_NONE */
 }
 
 ReturnValue_t CoreController::sdStateMachine() {
   ReturnValue_t result = returnvalue::OK;
   SdCardManager::Operations operation;
 
-  if (sdInfo.state == SdStates::IDLE) {
+  if (sdFsmState == SdStates::IDLE) {
     // Nothing to do
     return result;
   }
 
-  if (sdInfo.state == SdStates::START) {
+  if (sdFsmState == SdStates::START) {
     // Init will be performed by separate function
     if (BLOCKING_SD_INIT) {
-      sdInfo.state = SdStates::IDLE;
+      sdFsmState = SdStates::IDLE;
       sdInfo.initFinished = true;
       return result;
     } else {
       // Still update SD state file
-#if Q7S_SD_CARD_CONFIG == Q7S_SD_NONE
-      sdInfo.state = SdStates::UPDATE_INFO;
-#else
-      sdInfo.cycleCount = 0;
-      sdInfo.commandExecuted = false;
-      sdInfo.state = SdStates::GET_INFO;
-#endif
+      if (sdInfo.cfgMode == SdCfgMode::PASSIVE) {
+        sdFsmState = SdStates::UPDATE_INFO;
+      } else {
+        sdInfo.cycleCount = 0;
+        sdInfo.commandExecuted = false;
+        sdFsmState = SdStates::GET_INFO;
+      }
     }
   }
 
@@ -312,7 +361,7 @@ ReturnValue_t CoreController::sdStateMachine() {
                                    std::string opPrintout) {
     SdCardManager::OpStatus status = sdcMan->checkCurrentOp(operation);
     if (status == SdCardManager::OpStatus::SUCCESS) {
-      sdInfo.state = newStateOnSuccess;
+      sdFsmState = newStateOnSuccess;
       sdInfo.commandExecuted = false;
       sdInfo.cycleCount = 0;
       return true;
@@ -324,9 +373,9 @@ ReturnValue_t CoreController::sdStateMachine() {
     return false;
   };
 
-  if (sdInfo.state == SdStates::GET_INFO) {
+  if (sdFsmState == SdStates::GET_INFO) {
     if (not sdInfo.commandExecuted) {
-      // Create update status file
+      // Create updated status file
       result = sdcMan->updateSdCardStateFile();
       if (result != returnvalue::OK) {
         sif::warning << "CoreController::initialize: Updating SD card state file failed"
@@ -338,144 +387,149 @@ ReturnValue_t CoreController::sdStateMachine() {
     }
   }
 
-  if (sdInfo.state == SdStates::SET_STATE_SELF) {
+  if (sdFsmState == SdStates::SET_STATE_SELF) {
     if (not sdInfo.commandExecuted) {
       result = sdcMan->getSdCardsStatus(sdInfo.currentState);
-      auto sdCard = sdcMan->getPreferredSdCard();
-      if (not sdCard) {
-        return returnvalue::FAILED;
-      }
-      sdInfo.pref = sdCard.value();
       updateSdInfoOther();
-      if (sdInfo.pref != sd::SdCard::SLOT_0 and sdInfo.pref != sd::SdCard::SLOT_1) {
+      if (sdInfo.active != sd::SdCard::SLOT_0 and sdInfo.active != sd::SdCard::SLOT_1) {
         sif::warning << "Preferred SD card invalid. Setting to card 0.." << std::endl;
-        sdInfo.pref = sd::SdCard::SLOT_0;
+        sdInfo.active = sd::SdCard::SLOT_0;
       }
       if (result != returnvalue::OK) {
         sif::warning << "Getting SD card activity status failed" << std::endl;
       }
-#if Q7S_SD_CARD_CONFIG == Q7S_SD_COLD_REDUNDANT
-      sif::info << "Cold redundant SD card configuration, preferred SD card: "
-                << static_cast<int>(sdInfo.pref) << std::endl;
-#endif
-      if (sdInfo.prefState == sd::SdState::MOUNTED) {
+      if (sdInfo.cfgMode == SdCfgMode::COLD_REDUNDANT) {
+        sif::info << "Cold redundant SD card configuration, target SD card: "
+                  << static_cast<int>(sdInfo.active) << std::endl;
+      }
+      if (sdInfo.activeState == sd::SdState::MOUNTED) {
 #if OBSW_VERBOSE_LEVEL >= 1
         std::string mountString;
-        if (sdInfo.pref == sd::SdCard::SLOT_0) {
-          mountString = SdCardManager::SD_0_MOUNT_POINT;
+        if (sdInfo.active == sd::SdCard::SLOT_0) {
+          mountString = config::SD_0_MOUNT_POINT;
         } else {
-          mountString = SdCardManager::SD_1_MOUNT_POINT;
+          mountString = config::SD_1_MOUNT_POINT;
         }
-        sif::info << "SD card " << sdInfo.prefChar << " already on and mounted at " << mountString
+        sif::info << "SD card " << sdInfo.activeChar << " already on and mounted at " << mountString
                   << std::endl;
 #endif
-        sdInfo.state = SdStates::DETERMINE_OTHER;
-      } else if (sdInfo.prefState == sd::SdState::OFF) {
-        sdCardSetup(sdInfo.pref, sd::SdState::ON, sdInfo.prefChar, false);
+        sdcMan->setActiveSdCard(sdInfo.active);
+        currMntPrefix = sdcMan->getCurrentMountPrefix();
+        sdFsmState = SdStates::DETERMINE_OTHER;
+      } else if (sdInfo.activeState == sd::SdState::OFF) {
+        sdCardSetup(sdInfo.active, sd::SdState::ON, sdInfo.activeChar, false);
         sdInfo.commandExecuted = true;
-      } else if (sdInfo.prefState == sd::SdState::ON) {
-        sdInfo.state = SdStates::MOUNT_SELF;
+      } else if (sdInfo.activeState == sd::SdState::ON) {
+        sdFsmState = SdStates::MOUNT_SELF;
       }
     } else {
       if (nonBlockingOpChecking(SdStates::MOUNT_SELF, 10, "Setting SDC state")) {
-        sdInfo.prefState = sd::SdState::ON;
-        currentStateSetter(sdInfo.pref, sd::SdState::ON);
+        sdInfo.activeState = sd::SdState::ON;
+        currentStateSetter(sdInfo.active, sd::SdState::ON);
       }
     }
   }
 
-  if (sdInfo.state == SdStates::MOUNT_SELF) {
+  if (sdFsmState == SdStates::MOUNT_SELF) {
     if (not sdInfo.commandExecuted) {
-      result = sdCardSetup(sdInfo.pref, sd::SdState::MOUNTED, sdInfo.prefChar);
+      result = sdCardSetup(sdInfo.active, sd::SdState::MOUNTED, sdInfo.activeChar);
       sdInfo.commandExecuted = true;
     } else {
       if (nonBlockingOpChecking(SdStates::DETERMINE_OTHER, 5, "Mounting SD card")) {
+<<<<<<< HEAD
         sdInfo.prefState = sd::SdState::MOUNTED;
         currentStateSetter(sdInfo.pref, sd::SdState::MOUNTED);
         sdcMan->setActiveSdCard(sdInfo.pref);
+=======
+        sdcMan->setActiveSdCard(sdInfo.active);
+        currMntPrefix = sdcMan->getCurrentMountPrefix();
+        sdInfo.activeState = sd::SdState::MOUNTED;
+        currentStateSetter(sdInfo.active, sd::SdState::MOUNTED);
+>>>>>>> origin/develop
       }
     }
   }
 
-  if (sdInfo.state == SdStates::DETERMINE_OTHER) {
+  if (sdFsmState == SdStates::DETERMINE_OTHER) {
     // Determine whether any additional operations have to be done for the other SD card
     // 1. Cold redundant case: Other SD card needs to be unmounted and switched off
     // 2. Hot redundant case: Other SD card needs to be mounted and switched on
-#if Q7S_SD_CARD_CONFIG == Q7S_SD_COLD_REDUNDANT
-    if (sdInfo.otherState == sd::SdState::ON) {
-      sdInfo.state = SdStates::SET_STATE_OTHER;
-    } else if (sdInfo.otherState == sd::SdState::MOUNTED) {
-      sdInfo.state = SdStates::MOUNT_UNMOUNT_OTHER;
-    } else {
-      // Is already off, update info, but with a small delay
-      sdInfo.state = SdStates::SKIP_CYCLE_BEFORE_INFO_UPDATE;
+    if (sdInfo.cfgMode == SdCfgMode::COLD_REDUNDANT) {
+      if (sdInfo.otherState == sd::SdState::ON) {
+        sdFsmState = SdStates::SET_STATE_OTHER;
+      } else if (sdInfo.otherState == sd::SdState::MOUNTED) {
+        sdFsmState = SdStates::MOUNT_UNMOUNT_OTHER;
+      } else {
+        // Is already off, update info, but with a small delay
+        sdFsmState = SdStates::SKIP_CYCLE_BEFORE_INFO_UPDATE;
+      }
+    } else if (sdInfo.cfgMode == SdCfgMode::HOT_REDUNDANT) {
+      if (sdInfo.otherState == sd::SdState::OFF) {
+        sdFsmState = SdStates::SET_STATE_OTHER;
+      } else if (sdInfo.otherState == sd::SdState::ON) {
+        sdFsmState = SdStates::MOUNT_UNMOUNT_OTHER;
+      } else {
+        // Is already on and mounted, update info
+        sdFsmState = SdStates::SKIP_CYCLE_BEFORE_INFO_UPDATE;
+      }
     }
-#elif Q7S_SD_CARD_CONFIG == Q7S_SD_HOT_REDUNDANT
-    if (sdInfo.otherState == sd::SdState::OFF) {
-      sdInfo.state = SdStates::SET_STATE_OTHER;
-    } else if (sdInfo.otherState == sd::SdState::ON) {
-      sdInfo.state = SdStates::MOUNT_UNMOUNT_OTHER;
-    } else {
-      // Is already on and mounted, update info
-      sdInfo.state = SdStates::SKIP_CYCLE_BEFORE_INFO_UPDATE;
-    }
-#endif
   }
 
-  if (sdInfo.state == SdStates::SET_STATE_OTHER) {
+  if (sdFsmState == SdStates::SET_STATE_OTHER) {
     // Set state of other SD card to ON or OFF, depending on redundancy mode
-#if Q7S_SD_CARD_CONFIG == Q7S_SD_COLD_REDUNDANT
-    if (not sdInfo.commandExecuted) {
-      result = sdCardSetup(sdInfo.other, sd::SdState::OFF, sdInfo.otherChar, false);
-      sdInfo.commandExecuted = true;
-    } else {
-      if (nonBlockingOpChecking(SdStates::SKIP_CYCLE_BEFORE_INFO_UPDATE, 10,
-                                "Switching off other SD card")) {
-        sdInfo.otherState = sd::SdState::OFF;
-        currentStateSetter(sdInfo.other, sd::SdState::OFF);
+    if (sdInfo.cfgMode == SdCfgMode::COLD_REDUNDANT) {
+      if (not sdInfo.commandExecuted) {
+        result = sdCardSetup(sdInfo.other, sd::SdState::OFF, sdInfo.otherChar, false);
+        sdInfo.commandExecuted = true;
+      } else {
+        if (nonBlockingOpChecking(SdStates::SKIP_CYCLE_BEFORE_INFO_UPDATE, 10,
+                                  "Switching off other SD card")) {
+          sdInfo.otherState = sd::SdState::OFF;
+          currentStateSetter(sdInfo.other, sd::SdState::OFF);
+        }
+      }
+    } else if (sdInfo.cfgMode == SdCfgMode::HOT_REDUNDANT) {
+      if (not sdInfo.commandExecuted) {
+        result = sdCardSetup(sdInfo.other, sd::SdState::ON, sdInfo.otherChar, false);
+        sdInfo.commandExecuted = true;
+      } else {
+        if (nonBlockingOpChecking(SdStates::MOUNT_UNMOUNT_OTHER, 10,
+                                  "Switching on other SD card")) {
+          sdInfo.otherState = sd::SdState::ON;
+          currentStateSetter(sdInfo.other, sd::SdState::ON);
+        }
       }
     }
-#elif Q7S_SD_CARD_CONFIG == Q7S_SD_HOT_REDUNDANT
-    if (not sdInfo.commandExecuted) {
-      result = sdCardSetup(sdInfo.other, sd::SdState::ON, sdInfo.otherChar, false);
-      sdInfo.commandExecuted = true;
-    } else {
-      if (nonBlockingOpChecking(SdStates::MOUNT_UNMOUNT_OTHER, 10, "Switching on other SD card")) {
-        sdInfo.otherState = sd::SdState::ON;
-        currentStateSetter(sdInfo.other, sd::SdState::ON);
-      }
-    }
-#endif
   }
 
-  if (sdInfo.state == SdStates::MOUNT_UNMOUNT_OTHER) {
+  if (sdFsmState == SdStates::MOUNT_UNMOUNT_OTHER) {
     // Mount or unmount other SD card, depending on redundancy mode
-#if Q7S_SD_CARD_CONFIG == Q7S_SD_COLD_REDUNDANT
-    if (not sdInfo.commandExecuted) {
-      result = sdCardSetup(sdInfo.other, sd::SdState::ON, sdInfo.otherChar);
-      sdInfo.commandExecuted = true;
-    } else {
-      if (nonBlockingOpChecking(SdStates::SET_STATE_OTHER, 10, "Unmounting other SD card")) {
-        sdInfo.otherState = sd::SdState::ON;
-        currentStateSetter(sdInfo.other, sd::SdState::ON);
+    if (sdInfo.cfgMode == SdCfgMode::COLD_REDUNDANT) {
+      if (not sdInfo.commandExecuted) {
+        result = sdCardSetup(sdInfo.other, sd::SdState::ON, sdInfo.otherChar);
+        sdInfo.commandExecuted = true;
+      } else {
+        if (nonBlockingOpChecking(SdStates::SET_STATE_OTHER, 10, "Unmounting other SD card")) {
+          sdInfo.otherState = sd::SdState::ON;
+          currentStateSetter(sdInfo.other, sd::SdState::ON);
+        }
+      }
+    } else if (sdInfo.cfgMode == SdCfgMode::HOT_REDUNDANT) {
+      if (not sdInfo.commandExecuted) {
+        result = sdCardSetup(sdInfo.other, sd::SdState::MOUNTED, sdInfo.otherChar);
+        sdInfo.commandExecuted = true;
+      } else {
+        if (nonBlockingOpChecking(SdStates::UPDATE_INFO, 4, "Mounting other SD card")) {
+          sdInfo.otherState = sd::SdState::MOUNTED;
+          currentStateSetter(sdInfo.other, sd::SdState::MOUNTED);
+        }
       }
     }
-#elif Q7S_SD_CARD_CONFIG == Q7S_SD_HOT_REDUNDANT
-    if (not sdInfo.commandExecuted) {
-      result = sdCardSetup(sdInfo.other, sd::SdState::MOUNTED, sdInfo.otherChar);
-      sdInfo.commandExecuted = true;
-    } else {
-      if (nonBlockingOpChecking(SdStates::UPDATE_INFO, 4, "Mounting other SD card")) {
-        sdInfo.otherState = sd::SdState::MOUNTED;
-        currentStateSetter(sdInfo.other, sd::SdState::MOUNTED);
-      }
-    }
-#endif
   }
 
-  if (sdInfo.state == SdStates::SKIP_CYCLE_BEFORE_INFO_UPDATE) {
-    sdInfo.state = SdStates::UPDATE_INFO;
-  } else if (sdInfo.state == SdStates::UPDATE_INFO) {
+  if (sdFsmState == SdStates::SKIP_CYCLE_BEFORE_INFO_UPDATE) {
+    sdFsmState = SdStates::UPDATE_INFO;
+  } else if (sdFsmState == SdStates::UPDATE_INFO) {
     // It is assumed that all tasks are running by the point this section is reached.
     // Therefore, perform this operation in blocking mode because it does not take long
     // and the ready state of the SD card is available sooner
@@ -486,10 +540,15 @@ ReturnValue_t CoreController::sdStateMachine() {
       sif::warning << "CoreController::initialize: Updating SD card state file failed" << std::endl;
     }
     sdInfo.commandExecuted = false;
-    sdInfo.state = SdStates::IDLE;
+    sdFsmState = SdStates::IDLE;
     sdInfo.cycleCount = 0;
     sdcMan->setBlocking(false);
     sdcMan->getSdCardsStatus(sdInfo.currentState);
+    if (sdCommandingInfo.cmdPending) {
+      sdCommandingInfo.cmdPending = false;
+      actionHelper.finish(true, sdCommandingInfo.commander, sdCommandingInfo.actionId,
+                          returnvalue::OK);
+    }
     if (not sdInfo.initFinished) {
       updateSdInfoOther();
       sdInfo.initFinished = true;
@@ -497,81 +556,10 @@ ReturnValue_t CoreController::sdStateMachine() {
     }
   }
 
-  if (sdInfo.state == SdStates::SET_STATE_FROM_COMMAND) {
-    if (not sdInfo.commandExecuted) {
-      executeNextExternalSdCommand();
-    } else {
-      checkExternalSdCommandStatus();
-    }
-  }
-
   sdInfo.cycleCount++;
   return returnvalue::OK;
 }
 
-void CoreController::executeNextExternalSdCommand() {
-  std::string sdChar;
-  sd::SdState currentStateOfCard = sd::SdState::OFF;
-  if (sdInfo.commandedCard == sd::SdCard::SLOT_0) {
-    sdChar = "0";
-    currentStateOfCard = sdInfo.currentState.first;
-  } else {
-    sdChar = "1";
-    currentStateOfCard = sdInfo.currentState.second;
-  }
-  if (currentStateOfCard == sd::SdState::OFF) {
-    if (sdInfo.commandedState == sd::SdState::ON) {
-      sdInfo.currentlyCommandedState = sdInfo.commandedState;
-    } else if (sdInfo.commandedState == sd::SdState::MOUNTED) {
-      sdInfo.currentlyCommandedState = sd::SdState::ON;
-    } else {
-      // SD card is already on target state
-      sdInfo.commandFinished = true;
-      sdInfo.state = SdStates::IDLE;
-    }
-  } else if (currentStateOfCard == sd::SdState::ON) {
-    if (sdInfo.commandedState == sd::SdState::OFF or
-        sdInfo.commandedState == sd::SdState::MOUNTED) {
-      sdInfo.currentlyCommandedState = sdInfo.commandedState;
-    } else {
-      // Already on target state
-      sdInfo.commandFinished = true;
-      sdInfo.state = SdStates::IDLE;
-    }
-  } else if (currentStateOfCard == sd::SdState::MOUNTED) {
-    if (sdInfo.commandedState == sd::SdState::ON) {
-      sdInfo.currentlyCommandedState = sdInfo.commandedState;
-    } else if (sdInfo.commandedState == sd::SdState::OFF) {
-      // This causes an unmount in sdCardSetup
-      sdInfo.currentlyCommandedState = sd::SdState::ON;
-    } else {
-      sdInfo.commandFinished = true;
-    }
-  }
-  sdCardSetup(sdInfo.commandedCard, sdInfo.commandedState, sdChar);
-  sdInfo.commandExecuted = true;
-}
-
-void CoreController::checkExternalSdCommandStatus() {
-  SdCardManager::Operations operation;
-  SdCardManager::OpStatus status = sdcMan->checkCurrentOp(operation);
-  if (status == SdCardManager::OpStatus::SUCCESS) {
-    if (sdInfo.currentlyCommandedState == sdInfo.commandedState) {
-      sdInfo.state = SdStates::SKIP_CYCLE_BEFORE_INFO_UPDATE;
-      sdInfo.commandFinished = true;
-    } else {
-      // stay on same state machine state because the target state was not reached yet.
-      sdInfo.cycleCount = 0;
-    }
-    currentStateSetter(sdInfo.commandedCard, sdInfo.currentlyCommandedState);
-    sdInfo.commandExecuted = false;
-  } else if (sdInfo.cycleCount > 4) {
-    sif::warning << "CoreController::sdStateMachine: Commanding SD state "
-                    "takes too long"
-                 << std::endl;
-  }
-}
-
 void CoreController::currentStateSetter(sd::SdCard sdCard, sd::SdState newState) {
   if (sdCard == sd::SdCard::SLOT_0) {
     sdInfo.currentState.first = newState;
@@ -585,9 +573,9 @@ ReturnValue_t CoreController::sdCardSetup(sd::SdCard sdCard, sd::SdState targetS
   std::string mountString;
   sdcMan->setPrintCommandOutput(printOutput);
   if (sdCard == sd::SdCard::SLOT_0) {
-    mountString = SdCardManager::SD_0_MOUNT_POINT;
+    mountString = config::SD_0_MOUNT_POINT;
   } else {
-    mountString = SdCardManager::SD_1_MOUNT_POINT;
+    mountString = config::SD_1_MOUNT_POINT;
   }
 
   sd::SdState state = sd::SdState::OFF;
@@ -643,12 +631,12 @@ ReturnValue_t CoreController::sdCardSetup(sd::SdCard sdCard, sd::SdState targetS
 ReturnValue_t CoreController::sdColdRedundantBlockingInit() {
   ReturnValue_t result = returnvalue::OK;
 
-  result = sdCardSetup(sdInfo.pref, sd::SdState::MOUNTED, sdInfo.prefChar);
+  result = sdCardSetup(sdInfo.active, sd::SdState::MOUNTED, sdInfo.activeChar);
   if (result != SdCardManager::ALREADY_MOUNTED and result != returnvalue::OK) {
     sif::warning << "Setting up preferred card " << sdInfo.otherChar
                  << " in cold redundant mode failed" << std::endl;
     // Try other SD card and mark set up operation as failed
-    sdCardSetup(sdInfo.pref, sd::SdState::MOUNTED, sdInfo.prefChar);
+    sdCardSetup(sdInfo.active, sd::SdState::MOUNTED, sdInfo.activeChar);
     result = returnvalue::FAILED;
   }
 
@@ -965,21 +953,19 @@ ReturnValue_t CoreController::gracefulShutdownTasks(xsc::Chip chip, xsc::Copy co
   return result;
 }
 
-CoreController::~CoreController() {}
-
 void CoreController::updateSdInfoOther() {
-  if (sdInfo.pref == sd::SdCard::SLOT_0) {
-    sdInfo.prefChar = "0";
+  if (sdInfo.active == sd::SdCard::SLOT_0) {
+    sdInfo.activeChar = "0";
     sdInfo.otherChar = "1";
     sdInfo.otherState = sdInfo.currentState.second;
-    sdInfo.prefState = sdInfo.currentState.first;
+    sdInfo.activeState = sdInfo.currentState.first;
     sdInfo.other = sd::SdCard::SLOT_1;
 
-  } else if (sdInfo.pref == sd::SdCard::SLOT_1) {
-    sdInfo.prefChar = "1";
+  } else if (sdInfo.active == sd::SdCard::SLOT_1) {
+    sdInfo.activeChar = "1";
     sdInfo.otherChar = "0";
     sdInfo.otherState = sdInfo.currentState.first;
-    sdInfo.prefState = sdInfo.currentState.second;
+    sdInfo.activeState = sdInfo.currentState.second;
     sdInfo.other = sd::SdCard::SLOT_0;
   } else {
     sif::warning << "CoreController::updateSdInfoOther: Invalid SD card passed" << std::endl;
@@ -1252,7 +1238,7 @@ void CoreController::performWatchdogControlOperation() {
 void CoreController::performMountedSdCardOperations() {
   auto mountedSdCardOp = [&](bool &mntSwitch, sd::SdCard sdCard, std::string mntPoint) {
     if (mntSwitch) {
-      bool sdCardMounted = sdcMan->isSdCardMounted(sdCard);
+      bool sdCardMounted = sdcMan->isSdCardUsable(sdCard);
       if (sdCardMounted and not performOneShotSdCardOpsSwitch) {
         std::ostringstream path;
         path << mntPoint << "/" << CONF_FOLDER;
@@ -1267,12 +1253,12 @@ void CoreController::performMountedSdCardOperations() {
       mntSwitch = false;
     }
   };
-  if (sdInfo.pref == sd::SdCard::SLOT_1) {
-    mountedSdCardOp(sdInfo.mountSwitch.second, sd::SdCard::SLOT_1, SdCardManager::SD_1_MOUNT_POINT);
-    mountedSdCardOp(sdInfo.mountSwitch.first, sd::SdCard::SLOT_0, SdCardManager::SD_0_MOUNT_POINT);
+  if (sdInfo.active == sd::SdCard::SLOT_1) {
+    mountedSdCardOp(sdInfo.mountSwitch.second, sd::SdCard::SLOT_1, config::SD_1_MOUNT_POINT);
+    mountedSdCardOp(sdInfo.mountSwitch.first, sd::SdCard::SLOT_0, config::SD_0_MOUNT_POINT);
   } else {
-    mountedSdCardOp(sdInfo.mountSwitch.first, sd::SdCard::SLOT_0, SdCardManager::SD_0_MOUNT_POINT);
-    mountedSdCardOp(sdInfo.mountSwitch.second, sd::SdCard::SLOT_1, SdCardManager::SD_1_MOUNT_POINT);
+    mountedSdCardOp(sdInfo.mountSwitch.first, sd::SdCard::SLOT_0, config::SD_0_MOUNT_POINT);
+    mountedSdCardOp(sdInfo.mountSwitch.second, sd::SdCard::SLOT_1, config::SD_1_MOUNT_POINT);
   }
   timeFileHandler();
 }
@@ -1281,13 +1267,15 @@ ReturnValue_t CoreController::performSdCardCheck() {
   bool mountedReadOnly = false;
   SdCardManager::SdStatePair active;
   sdcMan->getSdCardsStatus(active);
+  if (sdFsmState != SdStates::IDLE) {
+    return returnvalue::OK;
+  }
   auto sdCardCheck = [&](sd::SdCard sdCard) {
     ReturnValue_t result = sdcMan->isSdCardMountedReadOnly(sdCard, mountedReadOnly);
     if (result != returnvalue::OK) {
       sif::error << "CoreController::performSdCardCheck: Could not check "
                     "read-only mount state"
                  << std::endl;
-      mountedReadOnly = true;
     }
     if (mountedReadOnly) {
       int linuxErrno = 0;
@@ -1854,6 +1842,175 @@ void CoreController::readHkData() {
   }
 }
 
+const char *CoreController::getXscMountDir(xsc::Chip chip, xsc::Copy copy) {
+  if (chip == xsc::Chip::CHIP_0) {
+    if (copy == xsc::Copy::COPY_0) {
+      return CHIP_0_COPY_0_MOUNT_DIR;
+    } else if (copy == xsc::Copy::COPY_1) {
+      return CHIP_0_COPY_1_MOUNT_DIR;
+    }
+  } else if (chip == xsc::Chip::CHIP_1) {
+    if (copy == xsc::Copy::COPY_0) {
+      return CHIP_1_COPY_0_MOUNT_DIR;
+    } else if (copy == xsc::Copy::COPY_1) {
+      return CHIP_1_COPY_1_MOUNT_DIR;
+    }
+  }
+  sif::error << "Invalid chip or copy passed to CoreController::getXscMountDir" << std::endl;
+  return CHIP_0_COPY_0_MOUNT_DIR;
+}
+
+ReturnValue_t CoreController::executeSwUpdate(SwUpdateSources sourceDir, const uint8_t *data,
+                                              size_t size) {
+  using namespace std;
+  using namespace std::filesystem;
+  // At the very least, chip and copy ID need to be included in the command
+  if (size < 2) {
+    return HasActionsIF::INVALID_PARAMETERS;
+  }
+  if (data[0] > 1 or data[1] > 1) {
+    return HasActionsIF::INVALID_PARAMETERS;
+  }
+  auto chip = static_cast<xsc::Chip>(data[0]);
+  auto copy = static_cast<xsc::Copy>(data[1]);
+  const char *sourceStr = "unknown";
+  if (sourceDir == SwUpdateSources::SD_0) {
+    sourceStr = "SD 0";
+  } else if (sourceDir == SwUpdateSources::SD_1) {
+    sourceStr = "SD 1";
+  } else {
+    sourceStr = "tmp directory";
+  }
+  bool sameChipAndCopy = false;
+  if(chip == CURRENT_CHIP and copy == CURRENT_COPY) {
+    // This is problematic if the OBSW is running as a systemd service.
+    // Do not allow for now.
+    return HasActionsIF::INVALID_PARAMETERS;
+    //sameChipAndCopy = true;
+  }
+  sif::info << "Executing SW update for Chip " << static_cast<int>(data[0]) << " Copy "
+            << static_cast<int>(data[1]) << " from " << sourceStr << std::endl;
+  path prefixPath;
+  if (sourceDir == SwUpdateSources::SD_0) {
+    prefixPath = path(config::SD_0_MOUNT_POINT);
+  } else if (sourceDir == SwUpdateSources::SD_1) {
+    prefixPath = path(config::SD_1_MOUNT_POINT);
+  } else if (sourceDir == SwUpdateSources::TMP_DIR) {
+    prefixPath = path("/tmp");
+  }
+  path archivePath = prefixPath / path(config::OBSW_UPDATE_ARCHIVE_FILE_NAME);
+  if (not exists(archivePath)) {
+    return HasFileSystemIF::FILE_DOES_NOT_EXIST;
+  }
+  ostringstream cmd("tar -xJf", ios::app);
+  cmd << " " << archivePath << " -C " << prefixPath;
+  int result = system(cmd.str().c_str());
+  if (result != 0) {
+    utility::handleSystemError(result, "CoreController::executeAction: SW Update Decompression");
+  }
+  path strippedImagePath = prefixPath / path(config::STRIPPED_OBSW_BINARY_FILE_NAME);
+  if (!exists(strippedImagePath)) {
+    // TODO: Custom returnvalue?
+    return returnvalue::FAILED;
+  }
+  path obswVersionFilePath = prefixPath / path(config::OBSW_VERSION_FILE_NAME);
+  if (!exists(obswVersionFilePath)) {
+    // TODO: Custom returnvalue?
+    return returnvalue::FAILED;
+  }
+  cmd.str("");
+  cmd.clear();
+  path obswDestPath;
+  path obswVersionDestPath;
+  if(not sameChipAndCopy) {
+    cmd << "xsc_mount_copy " << std::to_string(data[0]) << " " << std::to_string(data[1]);
+    result = system(cmd.str().c_str());
+    if (result != 0) {
+      std::string contextString = "CoreController::executeAction: SW Update Mounting " +
+                                  std::to_string(data[0]) + " " + std::to_string(data[1]);
+      utility::handleSystemError(result, contextString);
+    }
+    cmd.str("");
+    cmd.clear();
+    path xscMountDest(getXscMountDir(chip, copy));
+    obswDestPath = xscMountDest / path(relative(config::OBSW_PATH, "/"));
+    obswVersionDestPath = xscMountDest / path(relative(config::OBSW_VERSION_FILE_PATH, "/"));
+  } else {
+    obswDestPath = path(config::OBSW_PATH);
+    obswVersionDestPath = path(config::OBSW_VERSION_FILE_PATH);
+    cmd << "writeprotect " << std::to_string(CURRENT_CHIP) << " " << std::to_string(CURRENT_COPY)
+        << " 0";
+    result = system(cmd.str().c_str());
+    if (result != 0) {
+      std::string contextString = "CoreController::executeAction: Unlocking current chip";
+      utility::handleSystemError(result, contextString);
+    }
+    cmd.str("");
+    cmd.clear();
+  }
+
+  cmd << "cp " << strippedImagePath << " " << obswDestPath;
+  result = system(cmd.str().c_str());
+  if (result != 0) {
+    utility::handleSystemError(result, "CoreController::executeAction: Copying SW update");
+  }
+  cmd.str("");
+  cmd.clear();
+
+  cmd << "cp " << obswVersionFilePath << " " << obswVersionDestPath;
+  result = system(cmd.str().c_str());
+  if (result != 0) {
+    utility::handleSystemError(result, "CoreController::executeAction: Copying SW version file");
+  }
+  cmd.str("");
+  cmd.clear();
+
+  // Set correct permission for both files
+  cmd << "chmod 0755 " << obswDestPath;
+  result = system(cmd.str().c_str());
+  if (result != 0) {
+    utility::handleSystemError(result,
+                               "CoreController::executeAction: Setting SW permissions 0755");
+  }
+  cmd.str("");
+  cmd.clear();
+
+  cmd << "chmod 0644 " << obswVersionDestPath;
+  result = system(cmd.str().c_str());
+  if (result != 0) {
+    utility::handleSystemError(
+        result, "CoreController::executeAction: Setting version file permission 0644");
+  }
+  cmd.str("");
+  cmd.clear();
+
+  // TODO: This takes a long time and will block the core controller.. Maybe use command executor?
+  //       For now dont care..
+  cmd << "writeprotect " << std::to_string(data[0]) << " " << std::to_string(data[1]) << " 1";
+  result = system(cmd.str().c_str());
+  if (result != 0) {
+    std::string contextString = "CoreController::executeAction: Writeprotecting " +
+                                std::to_string(data[0]) + " " + std::to_string(data[1]);
+    utility::handleSystemError(result, contextString);
+  }
+  sif::info << "SW update complete" << std::endl;
+  return HasActionsIF::EXECUTION_FINISHED;
+}
+
+bool CoreController::startSdStateMachine(sd::SdCard targetActiveSd, SdCfgMode mode,
+                                         MessageQueueId_t commander, DeviceCommandId_t actionId) {
+  if (sdFsmState != SdStates::IDLE or sdCommandingInfo.cmdPending) {
+    return false;
+  }
+  sdFsmState = SdStates::START;
+  sdInfo.active = targetActiveSd;
+  sdInfo.cfgMode = mode;
+  sdCommandingInfo.actionId = actionId;
+  sdCommandingInfo.commander = commander;
+  sdCommandingInfo.cmdPending = true;
+  return true;
+}
+
 bool CoreController::isNumber(const std::string &s) {
   return !s.empty() && std::find_if(s.begin(), s.end(),
                                     [](unsigned char c) { return !std::isdigit(c); }) == s.end();