SDC manager and core controller refactoring

bsp_q7s/core/CoreController.cpp

@@ -12,6 +12,8 @@
 #include "fsfw/osal/common/TcpTmTcBridge.h"
 #endif
 
+#include "fsfw/osal/linux/Timer.h"
+
 #include "bsp_q7s/memory/scratchApi.h"
 #include "bsp_q7s/memory/SdCardManager.h"
 
@@ -24,8 +26,7 @@ CoreController::Chip CoreController::currentChip = Chip::NO_CHIP;
 CoreController::Copy CoreController::currentCopy = Copy::NO_COPY;
 
 CoreController::CoreController(object_id_t objectId):
-                    ExtendedControllerBase(objectId, objects::NO_OBJECT, 5),
-                    opDivider(5) {
+        ExtendedControllerBase(objectId, objects::NO_OBJECT, 5), opDivider(5) {
     ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;
     try {
         result = initWatchdogFifo();
@@ -33,6 +34,13 @@ CoreController::CoreController(object_id_t objectId):
             sif::warning << "CoreController::CoreController: Watchdog FIFO init failed" <<
                     std::endl;
         }
+        sdcMan = SdCardManager::instance();
+        if(sdcMan == nullptr) {
+            sif::error << "CoreController::CoreController: SD card manager invalid!" << std::endl;
+        }
+        if(not BLOCKING_SD_INIT) {
+            initSdCardNonBlocking();
+        }
 
         result = initBootCopy();
         if(result != HasReturnvaluesIF::RETURN_OK) {
@@ -50,6 +58,9 @@ ReturnValue_t CoreController::handleCommandMessage(CommandMessage *message) {
 }
 
 void CoreController::performControlOperation() {
+    if(not BLOCKING_SD_INIT and not sdInitFinished) {
+        initSdCardNonBlocking();
+    }
     performWatchdogControlOperation();
 }
 
@@ -71,6 +82,10 @@ ReturnValue_t CoreController::initialize() {
                 "count failed" << std::endl;
     }
 
+    if(not BLOCKING_SD_INIT) {
+        initSdCardNonBlocking();
+    }
+
     return ExtendedControllerBase::initialize();
 }
 
@@ -79,16 +94,11 @@ ReturnValue_t CoreController::checkModeCommand(Mode_t mode, Submode_t submode,
     return HasReturnvaluesIF::RETURN_OK;
 }
 
-ReturnValue_t CoreController::initSdCard() {
+ReturnValue_t CoreController::initSdCardBlocking() {
 #if Q7S_SD_CARD_CONFIG == Q7S_SD_NONE
     sif::info << "No SD card initialization will be performed" << std::endl;
     return HasReturnvaluesIF::RETURN_OK;
 #else
-    SdCardManager* sdcMan = SdCardManager::instance();
-    if(sdcMan == nullptr) {
-        return HasReturnvaluesIF::RETURN_FAILED;
-    }
-
     // Create update status file
     ReturnValue_t result = sdcMan->updateSdCardStateFile();
     if(result != HasReturnvaluesIF::RETURN_OK) {
@@ -96,19 +106,24 @@ ReturnValue_t CoreController::initSdCard() {
                 << std::endl;
     }
 
-    auto statusPair = SdCardManager::SdStatusPair(sd::SdStatus::OFF, sd::SdStatus::OFF);
+    auto statusPair = SdCardManager::SdStatusPair(sd::SdState::OFF, sd::SdState::OFF);
     result = sdcMan->getSdCardActiveStatus(statusPair);
     if(result != HasReturnvaluesIF::RETURN_OK) {
         sif::warning << "Getting SD card activity status failed" << std::endl;
     }
 
 #if Q7S_SD_CARD_CONFIG == Q7S_SD_COLD_REDUNDANT
-    return sdCardColdRedundantInit(sdcMan, statusPair);
+    determinePreferredSdCard();
+    sif::info << "Cold redundant SD card configuration, preferred SD card: " <<
+            static_cast<int>(preferredSdCard) << std::endl;
+    result = sdCardColdRedundantInit(sdcMan, statusPair);
+    // 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;
-
-    setUpSdCard(sd::SdCard::SLOT_0, sdStatus.first, "0");
-    setUpSdCard(sd::SdCard::SLOT_1, sdStatus.second, "1");
+    sdCardSetup(statusPair, sd::SdCard::SLOT_0, "0");
+    sdCardSetup(statusPair, sd::SdCard::SLOT_1, "1");
     // Update status file
     sdcMan->updateSdCardStateFile();
     return HasReturnvaluesIF::RETURN_OK;
@@ -118,9 +133,130 @@ ReturnValue_t CoreController::initSdCard() {
 
 }
 
-ReturnValue_t CoreController::sdCardSetup(SdCardManager& sdcMan,
-        SdCardManager::SdStatusPair& statusPair,sd::SdCard sdCard, sd::SdStatus status,
-        std::string sdString) {
+ReturnValue_t CoreController::initSdCardNonBlocking() {
+#if Q7S_SD_CARD_CONFIG == Q7S_SD_NONE
+    sif::info << "No SD card initialization will be performed" << std::endl;
+    return HasReturnvaluesIF::RETURN_OK;
+#else
+    ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;
+    if(state == SdStates::START) {
+        sdcInitCycleCount = 0;
+        commandExecuted = false;
+        state = SdStates::GET_INFO;
+    }
+
+    if(state == SdStates::GET_INFO) {
+        if(not commandExecuted) {
+            // Create update status file
+            result = sdcMan->updateSdCardStateFile();
+            if(result != HasReturnvaluesIF::RETURN_OK) {
+                sif::warning << "CoreController::initialize: Updating SD card state file failed"
+                        << std::endl;
+            }
+            commandExecuted = true;
+        }
+        else {
+            SdCardManager::Operations operation;
+            SdCardManager::OpStatus status = sdcMan->checkCurrentOp(operation);
+            if(status == SdCardManager::OpStatus::SUCCESS) {
+                state = SdStates::SET_STATE;
+                commandExecuted = false;
+                sdcInitCycleCount = 0;
+            }
+            else if(sdcInitCycleCount > 2) {
+                sif::warning << "CoreController::initSdCardNonBlocking: "
+                        "Updating SDC file takes too long" << std::endl;
+            }
+        }
+    }
+
+    if(state == SdStates::SET_STATE) {
+        if(not commandExecuted) {
+            determinePreferredSdCard();
+            statusPair = SdCardManager::SdStatusPair(sd::SdState::OFF, sd::SdState::OFF);
+            result = sdcMan->getSdCardActiveStatus(statusPair);
+            if(result != HasReturnvaluesIF::RETURN_OK) {
+                sif::warning << "Getting SD card activity status failed" << std::endl;
+            }
+            sif::info << "Cold redundant SD card configuration, preferred SD card: " <<
+                    static_cast<int>(preferredSdCard) << std::endl;
+            result = sdCardColdRedundantInit(sdcMan, statusPair);
+            if(result == SdCardManager::ALREADY_ON) {
+                state = SdStates::MOUNT;
+            }
+            if(result == SdCardManager::ALREADY_MOUNTED) {
+                state = SdStates::UPDATE_INFO;
+            }
+            else {
+                commandExecuted = true;
+            }
+        }
+        else {
+            SdCardManager::Operations operation;
+            SdCardManager::OpStatus status = sdcMan->checkCurrentOp(operation);
+            if(status == SdCardManager::OpStatus::SUCCESS) {
+                state = SdStates::MOUNT;
+                commandExecuted = false;
+                sdcInitCycleCount = 0;
+            }
+            else if(sdcInitCycleCount > 10) {
+                sif::warning << "CoreController::initSdCardNonBlocking: "
+                        "Setting SDC state takes too long" << std::endl;
+            }
+        }
+    }
+
+    if(state == SdStates::MOUNT) {
+        if(not commandExecuted) {
+            result = sdCardColdRedundantInit(sdcMan, statusPair);
+            commandExecuted = false;
+        }
+        else {
+            SdCardManager::Operations operation;
+            SdCardManager::OpStatus status = sdcMan->checkCurrentOp(operation);
+            if(status == SdCardManager::OpStatus::SUCCESS) {
+                state = SdStates::UPDATE_INFO;
+                commandExecuted = false;
+                sdcInitCycleCount = 0;
+            }
+            else if(sdcInitCycleCount > 5) {
+                sif::warning << "CoreController::initSdCardNonBlocking: "
+                        "Mounting SD card takes too long" << std::endl;
+            }
+        }
+    }
+
+    if(state == SdStates::UPDATE_INFO) {
+        if(not commandExecuted) {
+            // Update status file
+            result = sdcMan->updateSdCardStateFile();
+            if(result != HasReturnvaluesIF::RETURN_OK) {
+                sif::warning << "CoreController::initialize: Updating SD card state file failed"
+                        << std::endl;
+            }
+            commandExecuted = true;
+        }
+        else {
+            SdCardManager::Operations operation;
+            SdCardManager::OpStatus status = sdcMan->checkCurrentOp(operation);
+            if(status == SdCardManager::OpStatus::SUCCESS) {
+                sdInitFinished = true;
+                state = SdStates::DONE;
+            }
+            else if(sdcInitCycleCount > 2) {
+                sif::warning << "CoreController::initSdCardNonBlocking: "
+                        "Updating SDC file takes too long" << std::endl;
+            }
+        }
+    }
+
+    return HasReturnvaluesIF::RETURN_OK;
+#endif /* Q7S_SD_CARD_CONFIG != Q7S_SD_NONE */
+}
+
+
+ReturnValue_t CoreController::sdCardSetup(SdCardManager::SdStatusPair& statusPair,
+        sd::SdCard sdCard, std::string sdString) {
     std::string mountString;
     if(sdCard == sd::SdCard::SLOT_0) {
         mountString = SdCardManager::SD_0_MOUNT_POINT;
@@ -129,27 +265,60 @@ ReturnValue_t CoreController::sdCardSetup(SdCardManager& sdcMan,
         mountString = SdCardManager::SD_1_MOUNT_POINT;
     }
 
-    if(status == sd::SdStatus::OFF) {
-        sif::info << "Switching on and mounting SD card " << sdString << " at " <<
-                mountString << std::endl;
-        return sdcMan.switchOnSdCard(sdCard, true, &statusPair);
-    }
-    else if(status == sd::SdStatus::ON) {
-        sif::info << "Mounting SD card " << sdString << " at " << mountString << std::endl;
-        return sdcMan.mountSdCard(sdCard);
+    sd::SdState status = sd::SdState::OFF;
+    if(sdCard == sd::SdCard::SLOT_0) {
+        status = statusPair.first;
     }
     else {
+        status = statusPair.second;
+    }
+    if(status == sd::SdState::MOUNTED) {
         if(std::filesystem::exists(mountString)) {
             sif::info << "SD card " << sdString << " already on and mounted at " <<
                     mountString << std::endl;
             return SdCardManager::ALREADY_MOUNTED;
         }
-        sif::error << "SD card mounted but expected mount point " << mountString << " not found!"
-                << std::endl;
+        sif::error << "SD card mounted but expected mount point " <<
+                mountString << " not found!" << std::endl;
         return SdCardManager::MOUNT_ERROR;
     }
+
+    if(status == sd::SdState::OFF) {
+        if(BLOCKING_SD_INIT) {
+            sif::info << "Switching on and mounting SD card " << sdString << " at " <<
+                    mountString << std::endl;
+            return sdcMan->switchOnSdCard(sdCard, true, &statusPair);
+        }
+        else {
+            if(state == SdStates::SET_STATE) {
+                sif::info << "Switching on SD card" << std::endl;
+                return sdcMan->switchOnSdCard(sdCard, false, &statusPair);
+            }
+        }
+    }
+
+    else if(status == sd::SdState::ON) {
+        if(BLOCKING_SD_INIT) {
+            sif::info << "Mounting SD card " << sdString << " at " << mountString << std::endl;
+            return sdcMan->mountSdCard(sdCard);
+        }
+        else {
+            if(state == SdStates::SET_STATE) {
+                return SdCardManager::ALREADY_ON;
+            }
+            if(state == SdStates::MOUNT) {
+                sif::info << "Mounting SD card " << sdString << " at " << mountString << std::endl;
+                return sdcMan->mountSdCard(sdCard);
+            }
+        }
+    }
+    else {
+        sif::warning << "CoreController::sdCardSetup: Invalid state for this call" << std::endl;
+    }
+return HasReturnvaluesIF::RETURN_OK;
 }
 
+
 ReturnValue_t CoreController::executeAction(ActionId_t actionId, MessageQueueId_t commandedBy,
         const uint8_t *data, size_t size) {
     switch(actionId) {
@@ -166,16 +335,20 @@ ReturnValue_t CoreController::executeAction(ActionId_t actionId, MessageQueueId_
 }
 
 ReturnValue_t CoreController::initializeAfterTaskCreation() {
-
+    ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;
     // TODO: Do this here because this can take a longer time. Later, use non-blocking mode
     // to start doing stuff in ctor and initialize function.
     // This operation can take 1-2 seconds on reboot, and around 50-100ms if the SD
     // card is already on.
-    ReturnValue_t result = initSdCard();
-    if(result != HasReturnvaluesIF::RETURN_OK) {
-        sif::warning << "CoreController::CoreController: SD card init failed" << std::endl;
+    if(BLOCKING_SD_INIT) {
+        ReturnValue_t result = initSdCardBlocking();
+        if(result != HasReturnvaluesIF::RETURN_OK) {
+            sif::warning << "CoreController::CoreController: SD card init failed" << std::endl;
+        }
+    }
+    else {
+        initSdCardNonBlocking();
     }
-
     result = initVersionFile();
     if(result != HasReturnvaluesIF::RETURN_OK) {
         sif::warning << "CoreController::initialize: Version initialization failed" << std::endl;
@@ -185,29 +358,15 @@ ReturnValue_t CoreController::initializeAfterTaskCreation() {
 }
 
 ReturnValue_t CoreController::sdCardColdRedundantInit(SdCardManager* sdcMan,
-        SdCardManager::SdStatusPair& statusPair) {
-    sd::SdCard preferredSdCard = sd::SdCard::SLOT_0;
-    ReturnValue_t result = sdcMan->getPreferredSdCard(preferredSdCard);
-    if(result != HasReturnvaluesIF::RETURN_OK) {
-        if(result == scratch::KEY_NOT_FOUND) {
-            sif::warning << "CoreController::sdCardInit: "
-                    "Preferred SD card not set. Setting to 0" << std::endl;
-            sdcMan->setPreferredSdCard(preferredSdCard);
-        }
-        else {
-            sif::warning << "CoreController::sdCardInit: Could not get preferred SD card"
-                    "information from the scratch buffer" << std::endl;
-        }
-    }
+        SdCardManager::SdStatusPair& statusPair, SdStates step) {
+    ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;
     std::string preferredString;
-    sd::SdStatus preferredStatus = sd::SdStatus::OFF;
 
-    sd::SdStatus otherStatus = sd::SdStatus::OFF;
+    sd::SdState otherStatus = sd::SdState::OFF;
     std::string otherString;
     sd::SdCard otherSdc = sd::SdCard::SLOT_0;
 
     if(preferredSdCard == sd::SdCard::SLOT_0) {
-        preferredStatus = statusPair.first;
         preferredString = "0";
         otherSdc = sd::SdCard::SLOT_1;
         otherStatus = statusPair.second;
@@ -215,25 +374,21 @@ ReturnValue_t CoreController::sdCardColdRedundantInit(SdCardManager* sdcMan,
     }
     else {
         preferredString = "1";
-        preferredStatus = statusPair.second;
         otherStatus = statusPair.first;
         otherSdc = sd::SdCard::SLOT_0;
         otherString = "0";
     }
 
-    sif::info << "Cold redundant SD card configuration, preferred SD card " <<
-            preferredString << std::endl;
-
-    result = sdCardSetup(*sdcMan, statusPair, preferredSdCard, preferredStatus, preferredString);
+    result = sdCardSetup(statusPair, preferredSdCard, preferredString);
     if(result != SdCardManager::ALREADY_MOUNTED and result != HasReturnvaluesIF::RETURN_OK) {
         sif::warning << "Setting up preferred card " << otherString <<
                 " in cold redundant mode failed" << std::endl;
         // Try other SD card and mark set up operation as failed
-        sdCardSetup(*sdcMan, statusPair, preferredSdCard, preferredStatus, preferredString);
+        sdCardSetup(statusPair, preferredSdCard, preferredString);
         result = HasReturnvaluesIF::RETURN_FAILED;
     }
 
-    if(result != HasReturnvaluesIF::RETURN_FAILED and otherStatus != sd::SdStatus::OFF) {
+    if(result != HasReturnvaluesIF::RETURN_FAILED and otherStatus != sd::SdState::OFF) {
         sif::info << "Switching off secondary SD card " << otherString << std::endl;
         // Switch off other SD card in cold redundant mode if setting up preferred one walked
         // without issues
@@ -243,10 +398,7 @@ ReturnValue_t CoreController::sdCardColdRedundantInit(SdCardManager* sdcMan,
                     " in cold redundant mode failed" << std::endl;
         }
     }
-
-    // Update status file
-    sdcMan->updateSdCardStateFile();
-    return HasReturnvaluesIF::RETURN_OK;
+    return result;
 }
 
 ReturnValue_t CoreController::incrementAllocationFailureCount() {
@@ -501,6 +653,26 @@ ReturnValue_t CoreController::actionPerformReboot(const uint8_t *data, size_t si
     return HasActionsIF::EXECUTION_FINISHED;
 }
 
+CoreController::~CoreController() {
+}
+
+void CoreController::determinePreferredSdCard() {
+    if(preferredSdCard == sd::SdCard::NONE) {
+        ReturnValue_t result = sdcMan->getPreferredSdCard(preferredSdCard);
+        if(result != HasReturnvaluesIF::RETURN_OK) {
+            if(result == scratch::KEY_NOT_FOUND) {
+                sif::warning << "CoreController::sdCardInit: "
+                        "Preferred SD card not set. Setting to 0" << std::endl;
+                sdcMan->setPreferredSdCard(preferredSdCard);
+            }
+            else {
+                sif::warning << "CoreController::sdCardInit: Could not get preferred SD card"
+                        "information from the scratch buffer" << std::endl;
+            }
+        }
+    }
+}
+
 void CoreController::performWatchdogControlOperation() {
     // Only perform each fifth iteration
     if(watchdogFifoFd != 0 and opDivider.checkAndIncrement()) {