eive-obsw/bsp_q7s/core/CoreController.cpp

#include "CoreController.h"
#include "OBSWConfig.h"
#include "OBSWVersion.h"
#include "watchdogConf.h"

#include "fsfw/FSFWVersion.h"
#include "fsfw/timemanager/Stopwatch.h"
#include "fsfw/serviceinterface/ServiceInterface.h"
#if OBSW_USE_TMTC_TCP_BRIDGE == 0
#include "fsfw/osal/common/UdpTmTcBridge.h"
#else
#include "fsfw/osal/common/TcpTmTcServer.h"
#endif

#include "bsp_q7s/memory/scratchApi.h"
#include "bsp_q7s/memory/SdCardManager.h"

#include <fcntl.h>
#include <unistd.h>

#include <filesystem>

CoreController::Chip CoreController::CURRENT_CHIP = Chip::NO_CHIP;
CoreController::Copy CoreController::CURRENT_COPY = Copy::NO_COPY;

CoreController::CoreController(object_id_t objectId):
        ExtendedControllerBase(objectId, objects::NO_OBJECT, 5), opDivider(5) {
    ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;
    try {
        result = initWatchdogFifo();
        if(result != HasReturnvaluesIF::RETURN_OK) {
            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) {
            sdcMan->setBlocking(false);
        }
        sdStateMachine();

        result = initBootCopy();
        if(result != HasReturnvaluesIF::RETURN_OK) {
            sif::warning << "CoreController::CoreController: Boot copy init" << std::endl;
        }
    }
    catch(const std::filesystem::filesystem_error& e) {
        sif::error << "CoreController::CoreController: Failed with exception " <<
                e.what() << std::endl;
    }
}

ReturnValue_t CoreController::handleCommandMessage(CommandMessage *message) {
    return ExtendedControllerBase::handleCommandMessage(message);
}

void CoreController::performControlOperation() {
    performWatchdogControlOperation();
    sdStateMachine();
}

ReturnValue_t CoreController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
        LocalDataPoolManager &poolManager) {
    return HasReturnvaluesIF::RETURN_OK;
}

LocalPoolDataSetBase* CoreController::getDataSetHandle(sid_t sid) {
    return nullptr;
}

ReturnValue_t CoreController::initialize() {
    ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;

    result = scratch::writeNumber(scratch::ALLOC_FAILURE_COUNT, 0);
    if(result != HasReturnvaluesIF::RETURN_OK) {
        sif::warning << "CoreController::initialize: Setting up alloc failure "
                "count failed" << std::endl;
    }

    sdStateMachine();
    return ExtendedControllerBase::initialize();
}

ReturnValue_t CoreController::initializeAfterTaskCreation() {
    ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;
    if(BLOCKING_SD_INIT) {
        ReturnValue_t result = initSdCardBlocking();
        if(result != HasReturnvaluesIF::RETURN_OK and result != SdCardManager::ALREADY_MOUNTED) {
            sif::warning << "CoreController::CoreController: SD card init failed" << std::endl;
        }
    }
    sdStateMachine();
    result = initVersionFile();
    if(result != HasReturnvaluesIF::RETURN_OK) {
        sif::warning << "CoreController::initialize: Version initialization failed" << std::endl;
    }
    // Add script folder to path
    char* currentEnvPath = getenv("PATH");
    std::string updatedEnvPath = std::string(currentEnvPath) + ":/home/root/scripts";
    setenv("PATH", updatedEnvPath.c_str(), true);
    updateProtInfo();
    initPrint();
    return result;
}

ReturnValue_t CoreController::checkModeCommand(Mode_t mode, Submode_t submode,
        uint32_t *msToReachTheMode) {
    return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t CoreController::initSdCardBlocking() {
    // Create update status file
    ReturnValue_t result = sdcMan->updateSdCardStateFile();
    if(result != HasReturnvaluesIF::RETURN_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 HasReturnvaluesIF::RETURN_OK;
#else

    result = sdcMan->getSdCardActiveStatus(sdInfo.currentState);
    if(result != HasReturnvaluesIF::RETURN_OK) {
        sif::warning << "Getting SD card activity status failed" << std::endl;
    }

#if Q7S_SD_CARD_CONFIG == Q7S_SD_COLD_REDUNDANT
    determinePreferredSdCard();
    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();
    return HasReturnvaluesIF::RETURN_OK;
#endif

#endif /* Q7S_SD_CARD_CONFIG != Q7S_SD_NONE */

}

ReturnValue_t CoreController::sdStateMachine() {
    ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;
    SdCardManager::Operations operation;

    if(sdInfo.state == SdStates::IDLE) {
        // Nothing to do
        return result;
    }

    if(sdInfo.state == SdStates::START) {
        // Init will be performed by separate function
        if(BLOCKING_SD_INIT) {
            sdInfo.state = 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
        }
    }

    // This lambda checks the non-blocking operation and assigns the new state on success.
    // It returns true for an operation success and false otherwise
    auto nonBlockingOpChecking = [&](SdStates newStateOnSuccess,
            uint16_t maxCycleCount, std::string opPrintout) {
        SdCardManager::OpStatus status = sdcMan->checkCurrentOp(operation);
        if(status == SdCardManager::OpStatus::SUCCESS) {
            sdInfo.state = newStateOnSuccess;
            sdInfo.commandExecuted = false;
            sdInfo.cycleCount = 0;
            return true;
        }
        else if(sdInfo.cycleCount > 4) {
            sif::warning << "CoreController::sdInitStateMachine: " << opPrintout <<
                    " takes too long" << std::endl;
            return false;
        }
        return false;
    };

    if(sdInfo.state == SdStates::GET_INFO) {
        if(not sdInfo.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;
            }
            sdInfo.commandExecuted = true;
        }
        else {
            nonBlockingOpChecking(SdStates::SET_STATE_SELF, 4, "Updating SDC file");
        }
    }

    if(sdInfo.state == SdStates::SET_STATE_SELF) {
        if(not sdInfo.commandExecuted) {
            result = sdcMan->getSdCardActiveStatus(sdInfo.currentState);
            determinePreferredSdCard();
            updateSdInfoOther();
            if(sdInfo.pref != sd::SdCard::SLOT_0 and sdInfo.pref != sd::SdCard::SLOT_1) {
                sif::warning << "Preferred SD card invalid. Setting to card 0.." << std::endl;
                sdInfo.pref = sd::SdCard::SLOT_0;
            }
            if(result != HasReturnvaluesIF::RETURN_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 OBSW_VERBOSE_LEVEL >= 1
                std::string mountString;
                if(sdInfo.pref == sd::SdCard::SLOT_0) {
                    mountString = SdCardManager::SD_0_MOUNT_POINT;
                }
                else {
                    mountString = SdCardManager::SD_1_MOUNT_POINT;
                }
                sif::info << "SD card " << sdInfo.prefChar << " 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);
                sdInfo.commandExecuted = true;
            }
            else if(sdInfo.prefState == sd::SdState::ON) {
                sdInfo.state = SdStates::MOUNT_SELF;
            }
        }
        else {
            if(nonBlockingOpChecking(SdStates::MOUNT_SELF, 10, "Setting SDC state")) {
                sdInfo.prefState = sd::SdState::ON;
                currentStateSetter(sdInfo.pref, sd::SdState::ON);
            }
        }
    }

    if(sdInfo.state == SdStates::MOUNT_SELF) {
        if(not sdInfo.commandExecuted) {
            result = sdCardSetup(sdInfo.pref, sd::SdState::MOUNTED, sdInfo.prefChar);
            sdInfo.commandExecuted = true;
        }
        else {
            if(nonBlockingOpChecking(SdStates::DETERMINE_OTHER, 5, "Mounting SD card")) {
                sdInfo.prefState = sd::SdState::MOUNTED;
                currentStateSetter(sdInfo.pref, sd::SdState::MOUNTED);
            }
        }
    }

    if(sdInfo.state == 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;
        }
#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) {
        // 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);
            }
        }
#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) {
        // 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);
            }
        }
#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) {
        // 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
        sdcMan->setBlocking(true);
        // Update status file
        result = sdcMan->updateSdCardStateFile();
        if(result != HasReturnvaluesIF::RETURN_OK) {
            sif::warning << "CoreController::initialize: Updating SD card state file failed"
                    << std::endl;
        }
        sdInfo.commandExecuted = false;
        sdInfo.state = SdStates::IDLE;
        sdInfo.cycleCount = 0;
        sdcMan->setBlocking(false);
        sdcMan->getSdCardActiveStatus(sdInfo.currentState);
        if(not sdInfo.initFinished) {
            updateSdInfoOther();
            sdInfo.initFinished = true;
            sif::info << "SD card initialization finished" << std::endl;
        }
    }

    if(sdInfo.state == SdStates::SET_STATE_FROM_COMMAND) {
        if(not sdInfo.commandExecuted) {
            executeNextExternalSdCommand();
        }
        else {
            checkExternalSdCommandStatus();
        }
    }

    sdInfo.cycleCount++;
    return HasReturnvaluesIF::RETURN_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;
    }
    else {
        sdInfo.currentState.second = newState;
    }
}

ReturnValue_t CoreController::sdCardSetup(sd::SdCard sdCard, sd::SdState targetState,
        std::string sdChar, bool printOutput) {
    std::string mountString;
    sdcMan->setPrintCommandOutput(printOutput);
    if(sdCard == sd::SdCard::SLOT_0) {
        mountString = SdCardManager::SD_0_MOUNT_POINT;
    }
    else {
        mountString = SdCardManager::SD_1_MOUNT_POINT;
    }

    sd::SdState state = sd::SdState::OFF;
    if(sdCard == sd::SdCard::SLOT_0) {
        state = sdInfo.currentState.first;
    }
    else {
        state = sdInfo.currentState.second;
    }
    if(state == sd::SdState::MOUNTED) {
        if(targetState == sd::SdState::OFF) {
            sif::info << "Switching off SD card " << sdChar << std::endl;
            return sdcMan->switchOffSdCard(sdCard, true, &sdInfo.currentState);
        }
        else if(targetState == sd::SdState::ON) {
            sif::info << "Unmounting SD card " << sdChar << std::endl;
            return sdcMan->unmountSdCard(sdCard);
        }
        else {
            if(std::filesystem::exists(mountString)) {
                sif::info << "SD card " << sdChar << " 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;
            return SdCardManager::MOUNT_ERROR;
        }
    }

    if(state == sd::SdState::OFF) {
        if(targetState == sd::SdState::MOUNTED) {
            sif::info << "Switching on and mounting SD card " << sdChar << " at " <<
                    mountString << std::endl;
            return sdcMan->switchOnSdCard(sdCard, true, &sdInfo.currentState);
        }
        else if(targetState == sd::SdState::ON) {
            sif::info << "Switching on SD card " << sdChar << std::endl;
            return sdcMan->switchOnSdCard(sdCard, false, &sdInfo.currentState);
        }
    }

    else if(state == sd::SdState::ON) {
        if(targetState == sd::SdState::MOUNTED) {
            sif::info << "Mounting SD card " << sdChar << " at " << mountString << std::endl;
            return sdcMan->mountSdCard(sdCard);
        }
        else if(targetState == sd::SdState::OFF) {
            sif::info << "Switching off SD card " << sdChar << std::endl;
            return sdcMan->switchOffSdCard(sdCard, false, &sdInfo.currentState);
        }
    }
    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) {
    case(LIST_DIRECTORY_INTO_FILE): {
        return actionListDirectoryIntoFile(actionId, commandedBy, data, size);
    }
    case(REBOOT_OBC): {
        return actionPerformReboot(data, size);
    }
    default: {
        return HasActionsIF::INVALID_ACTION_ID;
    }
    }
}

ReturnValue_t CoreController::sdColdRedundantBlockingInit() {
    ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;

    result = sdCardSetup(sdInfo.pref, sd::SdState::MOUNTED, sdInfo.prefChar);
    if(result != SdCardManager::ALREADY_MOUNTED and result != HasReturnvaluesIF::RETURN_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);
        result = HasReturnvaluesIF::RETURN_FAILED;
    }

    if(result != HasReturnvaluesIF::RETURN_FAILED and sdInfo.otherState != sd::SdState::OFF) {
        sif::info << "Switching off secondary SD card " << sdInfo.otherChar << std::endl;
        // Switch off other SD card in cold redundant mode if setting up preferred one worked
        // without issues
        ReturnValue_t result2 = sdcMan->switchOffSdCard(sdInfo.other,
                sdInfo.otherState, &sdInfo.currentState);
        if(result2 != HasReturnvaluesIF::RETURN_OK and result2 != SdCardManager::ALREADY_OFF) {
            sif::warning << "Switching off secondary SD card " << sdInfo.otherChar <<
                    " in cold redundant mode failed" << std::endl;
        }
    }
    return result;
}

ReturnValue_t CoreController::incrementAllocationFailureCount() {
    uint32_t count = 0;
    ReturnValue_t result = scratch::readNumber(scratch::ALLOC_FAILURE_COUNT, count);
    if(result != HasReturnvaluesIF::RETURN_OK) {
        return result;
    }
    count++;
    return scratch::writeNumber(scratch::ALLOC_FAILURE_COUNT, count);
}

ReturnValue_t CoreController::initVersionFile() {

    std::string unameFileName = "/tmp/uname_version.txt";
    // TODO: No -v flag for now. If the kernel version is used, need to cut off first few letters
    std::string unameCmd = "uname -mnrso > " + unameFileName;
    int result = std::system(unameCmd.c_str());
    if(result != 0) {
        utility::handleSystemError(result, "CoreController::versionFileInit");
    }
    std::ifstream unameFile(unameFileName);
    std::string unameLine;
    if(not std::getline(unameFile, unameLine)) {
        sif::warning << "CoreController::versionFileInit: Retrieving uname line failed"
                << std::endl;
    }

    std::string fullObswVersionString = "OBSW: v" + std::to_string(SW_VERSION) + "." +
            std::to_string(SW_SUBVERSION) + "." + std::to_string(SW_REVISION);
    std::string fullFsfwVersionString = "FSFW: v" + std::to_string(FSFW_VERSION) + "." +
            std::to_string(FSFW_SUBVERSION) + "." + std::to_string(FSFW_REVISION);
    std::string systemString = "System: " + unameLine;
    std::string mountPrefix = SdCardManager::instance()->getCurrentMountPrefix();
    std::string versionFilePath = mountPrefix + VERSION_FILE;
    std::fstream versionFile;

    if(not std::filesystem::exists(versionFilePath)) {
        sif::info << "Writing version file " << versionFilePath << ".." << std::endl;
        versionFile.open(versionFilePath, std::ios_base::out);
        versionFile << fullObswVersionString << std::endl;
        versionFile << fullFsfwVersionString << std::endl;
        versionFile << systemString << std::endl;
        return HasReturnvaluesIF::RETURN_OK;
    }

    // Check whether any version has changed
    bool createNewFile = false;
    versionFile.open(versionFilePath);
    std::string currentVersionString;
    uint8_t idx = 0;
    while(std::getline(versionFile, currentVersionString)) {
        if(idx == 0) {
            if(currentVersionString != fullObswVersionString) {
                sif::info << "OBSW version changed" << std::endl;
                sif::info << "From " << currentVersionString << " to " <<
                        fullObswVersionString << std::endl;
                createNewFile = true;
            }
        }
        else if(idx == 1) {
            if(currentVersionString != fullFsfwVersionString) {
                sif::info << "FSFW version changed" << std::endl;
                sif::info << "From " << currentVersionString << " to " <<
                        fullFsfwVersionString << std::endl;
                createNewFile = true;
            }
        }
        else if(idx == 2) {
            if(currentVersionString != systemString) {
                sif::info << "System version changed" << std::endl;
                sif::info << "Old: " << currentVersionString << std::endl;
                sif::info << "New: " << systemString << std::endl;
                createNewFile = true;
            }
        }
        else {
            sif::warning << "Invalid version file! Rewriting it.." << std::endl;
            createNewFile = true;
        }
        idx++;
    }

    // Overwrite file if necessary
    if(createNewFile) {
        sif::info << "Rewriting version.txt file with updated versions.." << std::endl;
        versionFile.close();
        versionFile.open(versionFilePath, std::ios_base::out | std::ios_base::trunc);
        versionFile << fullObswVersionString << std::endl;
        versionFile << fullFsfwVersionString << std::endl;
        versionFile << systemString << std::endl;
    }

    return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t CoreController::actionListDirectoryIntoFile(ActionId_t actionId,
        MessageQueueId_t commandedBy, const uint8_t *data, size_t size) {
    // TODO: Packet definition for clean deserialization
    // 2 bytes for a and R flag, at least 5 bytes for minimum valid path /tmp with
    // null termination, at least 7 bytes for minimum target file name /tmp/a with
    // null termination.
    if(size < 14) {
        return HasActionsIF::INVALID_PARAMETERS;
    }
    // We could also make -l optional, but I can't think of a reason why to not use -l..

    // This flag specifies to run ls with -a
    bool aFlag = data[0];
    data += 1;
    // This flag specifies to run ls with -R
    bool RFlag = data[1];
    data += 1;

    size_t remainingSize = size - 2;
    // One larger for null termination, which prevents undefined behaviour if the sent
    // strings are not 0 terminated properly
    std::vector<uint8_t> repoAndTargetFileBuffer(remainingSize + 1, 0);
    std::memcpy(repoAndTargetFileBuffer.data(), data, remainingSize);
    const char* currentCharPtr = reinterpret_cast<const char*>(repoAndTargetFileBuffer.data());
    // Full target file name
    std::string repoName(currentCharPtr);
    size_t repoLength = repoName.length();
    // The other string needs to be at least one letter plus NULL termination to be valid at all
    // The first string also needs to be NULL terminated, but the termination is not included
    // in the string length, so this is subtracted from the remaining size as well
    if(repoLength > remainingSize - 3) {
        return HasActionsIF::INVALID_PARAMETERS;
    }
    // The file length will not include the NULL termination, so we skip it
    currentCharPtr += repoLength + 1;
    std::string targetFileName(currentCharPtr);
    std::ostringstream oss;
    oss << "ls -l";
    if(aFlag) {
        oss << "a";
    }
    if(RFlag) {
        oss << "R";
    }

    oss << " " << repoName << " > " << targetFileName;
    int result = std::system(oss.str().c_str());
    if(result != 0) {
        utility::handleSystemError(result, "CoreController::actionListDirectoryIntoFile");
        actionHelper.finish(false, commandedBy, actionId);
    }
    return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t CoreController::initBootCopy() {
    if(not std::filesystem::exists(CURR_COPY_FILE)) {
        // Thils file is created by the systemd service eive-early-config so this should
        // not happen normally
        std::string cmd = "xsc_boot_copy > " + std::string(CURR_COPY_FILE);
        int result = std::system(cmd.c_str());
        if(result != 0) {
            utility::handleSystemError(result, "CoreController::initBootCopy");
        }
    }
    std::ifstream file(CURR_COPY_FILE);
    std::string line;
    std::getline(file, line);
    std::istringstream iss(line);
    int value = 0;
    iss >> value;
    CURRENT_CHIP = static_cast<Chip>(value);
    iss >> value;
    CURRENT_COPY = static_cast<Copy>(value);
    return HasReturnvaluesIF::RETURN_OK;
}

void CoreController::getCurrentBootCopy(Chip &chip, Copy &copy) {
    // Not really thread-safe but it does not need to be
    chip = CURRENT_CHIP;
    copy = CURRENT_COPY;
}

ReturnValue_t CoreController::initWatchdogFifo() {
    if(not std::filesystem::exists(watchdog::FIFO_NAME)) {
        // Still return RETURN_OK for now
        sif::info << "Watchdog FIFO " << watchdog::FIFO_NAME << " does not exist, can't initiate" <<
                " watchdog" << std::endl;
        return HasReturnvaluesIF::RETURN_OK;
    }
    // Open FIFO write only and non-blocking to prevent SW from killing itself.
    watchdogFifoFd = open(watchdog::FIFO_NAME.c_str(), O_WRONLY | O_NONBLOCK);
    if(watchdogFifoFd < 0) {
        if(errno == ENXIO) {
            watchdogFifoFd = RETRY_FIFO_OPEN;
            sif::info << "eive-watchdog not running. FIFO can not be opened" << std::endl;
        }
        else {
            sif::error << "Opening pipe " << watchdog::FIFO_NAME << " write-only failed with " <<
                    errno << ": " << strerror(errno) << std::endl;
            return HasReturnvaluesIF::RETURN_FAILED;
        }
    }
    return HasReturnvaluesIF::RETURN_OK;
}

void CoreController::initPrint() {
#if OBSW_VERBOSE_LEVEL >= 1
    if(watchdogFifoFd > 0) {
        sif::info << "Opened watchdog FIFO successfully.." << std::endl;
    }
#endif
}

ReturnValue_t CoreController::actionPerformReboot(const uint8_t *data, size_t size) {
    if(size < 1) {
        return HasActionsIF::INVALID_PARAMETERS;
    }
    bool rebootSameBootCopy = data[0];
    bool protOpPerformed;
    if(rebootSameBootCopy) {
#if OBSW_VERBOSE_LEVEL >= 1
        sif::info << "CoreController::actionPerformReboot: Rebooting on current image" << std::endl;
#endif
        // Attempt graceful shutdown by unmounting and switching off SD cards
        SdCardManager::instance()->switchOffSdCard(sd::SdCard::SLOT_0);
        SdCardManager::instance()->switchOffSdCard(sd::SdCard::SLOT_1);
        // If any boot copies are unprotected
        ReturnValue_t retval = setBootCopyProtection(Chip::SELF_CHIP, Copy::SELF_COPY,
                true, protOpPerformed, false);
        if(retval == HasReturnvaluesIF::RETURN_OK and protOpPerformed) {
            sif::info << "Running slot was writeprotected before reboot" << std::endl;
        }
        int result = std::system("xsc_boot_copy -r");
        if(result != 0) {
            utility::handleSystemError(result, "CoreController::executeAction");
            return HasReturnvaluesIF::RETURN_FAILED;
        }
        return HasActionsIF::EXECUTION_FINISHED;
    }
    if(size < 3) {
        return HasActionsIF::INVALID_PARAMETERS;
    }
#if OBSW_VERBOSE_LEVEL >= 1
    sif::info << "CoreController::actionPerformReboot: Rebooting on " <<
            static_cast<int>(data[1]) << " " << static_cast<int>(data[2]) << std::endl;
#endif

    // Check that the target chip and copy is writeprotected first
    generateChipStateFile();
    // If any boot copies are unprotected, protect them here
    ReturnValue_t retval = setBootCopyProtection(static_cast<Chip>(data[1]),
            static_cast<Copy>(data[2]), true, protOpPerformed, false);
    if(retval == HasReturnvaluesIF::RETURN_OK and protOpPerformed) {
        sif::info << "Target slot was writeprotected before reboot" << std::endl;
    }

    // The second byte in data is the target chip, the third byte is the target copy
    std::string cmdString = "xsc_boot_copy " + std::to_string(data[1]) + " " +
            std::to_string(data[2]);
    int result = std::system(cmdString.c_str());
    if(result != 0) {
        utility::handleSystemError(result, "CoreController::executeAction");
        return HasReturnvaluesIF::RETURN_FAILED;
    }
    return HasActionsIF::EXECUTION_FINISHED;
}

CoreController::~CoreController() {
}

void CoreController::determinePreferredSdCard() {
    if(sdInfo.pref == sd::SdCard::NONE) {
        ReturnValue_t result = sdcMan->getPreferredSdCard(sdInfo.pref);
        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(sd::SdCard::SLOT_0);
                sdInfo.pref = sd::SdCard::SLOT_0;
            }
            else {
                sif::warning << "CoreController::sdCardInit: Could not get preferred SD card"
                        "information from the scratch buffer" << std::endl;
            }
        }
    }
}

void CoreController::updateSdInfoOther() {
    if(sdInfo.pref == sd::SdCard::SLOT_0) {
        sdInfo.prefChar = "0";
        sdInfo.otherChar = "1";
        sdInfo.otherState =  sdInfo.currentState.second;
        sdInfo.prefState = sdInfo.currentState.first;
        sdInfo.other = sd::SdCard::SLOT_1;

    }
    else if(sdInfo.pref == sd::SdCard::SLOT_1) {
        sdInfo.prefChar = "1";
        sdInfo.otherChar = "0";
        sdInfo.otherState = sdInfo.currentState.first;
        sdInfo.prefState = sdInfo.currentState.second;
        sdInfo.other = sd::SdCard::SLOT_0;
    }
    else {
        sif::warning << "CoreController::updateSdInfoOther: Invalid SD card passed" << std::endl;
    }
}

bool CoreController::sdInitFinished() const {
    return sdInfo.initFinished;
}

ReturnValue_t CoreController::generateChipStateFile() {
    int result = std::system(CHIP_PROT_SCRIPT);
    if(result != 0) {
        utility::handleSystemError(result, "CoreController::generateChipStateFile");
        return HasReturnvaluesIF::RETURN_FAILED;
    }
    return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t CoreController::setBootCopyProtection(Chip targetChip, Copy targetCopy,
        bool protect, bool& protOperationPerformed, bool updateProtFile) {
    bool allChips = false;
    bool allCopies = false;
    bool selfChip = false;
    bool selfCopy = false;
    protOperationPerformed = false;

    switch(targetChip) {
    case(Chip::ALL_CHIP): {
        allChips = true;
        break;
    }
    case(Chip::NO_CHIP): {
        return HasReturnvaluesIF::RETURN_OK;
    }
    case(Chip::SELF_CHIP): {
        selfChip = true;
        targetChip = CURRENT_CHIP;
        break;
    }
    default: {
        break;
    }
    }
    switch(targetCopy) {
    case(Copy::ALL_COPY): {
        allCopies = true;
        break;
    }
    case(Copy::NO_COPY): {
        return HasReturnvaluesIF::RETURN_OK;
    }
    case(Copy::SELF_COPY): {
        selfCopy = true;
        targetCopy = CURRENT_COPY;
        break;
    }
    default: {
        break;
    }
    }

    for(uint8_t arrIdx = 0; arrIdx < protArray.size(); arrIdx++) {
        int result = handleBootCopyProtAtIndex(targetChip, targetCopy, protect,
                protOperationPerformed, selfChip, selfCopy, allChips, allCopies, arrIdx);
        if(result != 0) {
            break;
        }
    }
    if(protOperationPerformed and updateProtFile) {
        updateProtInfo();
    }
    return HasReturnvaluesIF::RETURN_OK;
}

int CoreController::handleBootCopyProtAtIndex(Chip targetChip, Copy targetCopy, bool protect,
    bool &protOperationPerformed, bool selfChip, bool selfCopy, bool allChips,
    bool allCopies, uint8_t arrIdx) {
    bool currentProt = protArray[arrIdx];
    std::ostringstream oss;
    bool performOp = false;
    if(protect == currentProt) {
        return 0;
    }
    if(protOperationPerformed) {
        if((selfChip and selfCopy) or (not allCopies and not allChips)) {
            // No need to continue, only one operation was requested
            return 1;
        }
    }
    Chip currentChip;
    Copy currentCopy;
    oss << "writeprotect ";
    if(arrIdx == 0 or arrIdx == 1) {
        oss << "0 ";
        currentChip = Chip::CHIP_0;
    }
    else {
        oss << "1 ";
        currentChip = Chip::CHIP_1;
    }
    if(arrIdx == 0 or arrIdx == 2) {
        oss << "0 ";
        currentCopy = Copy::COPY_0;
    }
    else {
        oss << "1 ";
        currentCopy = Copy::COPY_1;
    }
    if(protect) {
        oss << "1";
    }
    else {
        oss << "0";
    }

    int result = 0;
    if(allChips and allCopies) {
        performOp = true;
    }
    else if(allChips) {
        if((selfCopy and CURRENT_COPY == targetCopy) or
                (currentCopy == targetCopy)) {
            performOp = true;
        }
    }
    else if(allCopies) {
        if((selfChip and CURRENT_COPY == targetCopy) or
                (currentChip == targetChip)) {
            performOp = true;
        }
    }
    else if(selfChip and (currentChip == targetChip)) {
        if(selfCopy) {
            if(currentCopy == targetCopy) {
                performOp = true;
            }
        }
        else {
            performOp = true;
        }

    }
    else if(selfCopy and (currentCopy == targetCopy)) {
        if(selfChip) {
            if(currentChip == targetChip) {
                performOp = true;
            }
        }
        else {
            performOp = true;
        }
    }
    else if((targetChip == currentChip) and (targetCopy == currentCopy)) {
        performOp = true;
    }
    if(result != 0) {
        utility::handleSystemError(result, "CoreController::checkAndSetBootCopyProtection");
    }
    if(performOp) {
        // TODO: Lock operation take a long time. Use command executor? That would require a
        // new state machine..
        protOperationPerformed = true;
        sif::info << "Executing command: " << oss.str() << std::endl;
        result = std::system(oss.str().c_str());
    }
    return 0;
}

ReturnValue_t CoreController::updateProtInfo(bool regenerateChipStateFile) {
    using namespace std;
    ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;
    if(regenerateChipStateFile) {
        result = generateChipStateFile();
        if(result != HasReturnvaluesIF::RETURN_OK) {
            sif::warning << "CoreController::updateProtInfo: Generating chip state file failed" <<
                    std::endl;
            return result;
        }
    }
    if(not filesystem::exists(CHIP_STATE_FILE)) {
        return HasReturnvaluesIF::RETURN_FAILED;
    }
    ifstream chipStateFile(CHIP_STATE_FILE);
    if(not chipStateFile.good()) {
        return HasReturnvaluesIF::RETURN_FAILED;
    }
    string nextLine;
    uint8_t lineCounter = 0;
    string word;
    while(getline(chipStateFile, nextLine)) {
        ReturnValue_t result = handleProtInfoUpdateLine(nextLine);
        if(result != HasReturnvaluesIF::RETURN_OK) {
            sif::warning << "CoreController::updateProtInfo: Protection info update failed!" <<
                    std::endl;
            return result;
        }
        ++lineCounter;
        if(lineCounter > 4) {
            sif::warning << "CoreController::checkAndProtectBootCopy: "
                    "Line counter larger than 4" << std::endl;
        }
    }
    return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t CoreController::handleProtInfoUpdateLine(std::string nextLine) {
    using namespace std;
    string word;
    uint8_t wordIdx = 0;
    uint8_t arrayIdx = 0;
    istringstream iss(nextLine);
    Chip currentChip = Chip::CHIP_0;
    Copy currentCopy = Copy::COPY_0;
    while(iss >> word) {
        if(wordIdx == 1) {
            currentChip = static_cast<Chip>(stoi(word));
        }
        if(wordIdx == 3) {
            currentCopy = static_cast<Copy>(stoi(word));
        }

        if(wordIdx == 3) {
            if(currentChip == Chip::CHIP_0) {
                if(currentCopy == Copy::COPY_0) {
                    arrayIdx = 0;
                }
                else if(currentCopy == Copy::COPY_1) {
                    arrayIdx = 1;
                }
            }

            else if(currentChip == Chip::CHIP_1) {
               if(currentCopy == Copy::COPY_0) {
                   arrayIdx = 2;
               }
               else if(currentCopy == Copy::COPY_1) {
                   arrayIdx = 3;
               }
           }
        }
        if(wordIdx == 5) {
            if(word == "unlocked.") {
                protArray[arrayIdx] = false;
            }
            else {
                protArray[arrayIdx] = true;
            }
        }
        wordIdx++;
    }
    return HasReturnvaluesIF::RETURN_OK;
}

void CoreController::performWatchdogControlOperation() {
    // Only perform each fifth iteration
    if(watchdogFifoFd != 0 and opDivider.checkAndIncrement()) {
        if(watchdogFifoFd == RETRY_FIFO_OPEN) {
            // Open FIFO write only and non-blocking
            watchdogFifoFd = open(watchdog::FIFO_NAME.c_str(), O_WRONLY | O_NONBLOCK);
            if(watchdogFifoFd < 0) {
                if(errno == ENXIO) {
                    watchdogFifoFd = RETRY_FIFO_OPEN;
                    // No printout for now, would be spam
                    return;
                }
                else {
                    sif::error << "Opening pipe " << watchdog::FIFO_NAME <<
                            " write-only failed with " <<  errno << ": " <<
                            strerror(errno) << std::endl;
                    return;
                }
            }
            sif::info << "Opened " << watchdog::FIFO_NAME << " successfully" << std::endl;
        }
        else if(watchdogFifoFd > 0) {
            // Write to OBSW watchdog FIFO here
            const char writeChar = 'a';
            ssize_t writtenBytes = write(watchdogFifoFd, &writeChar, 1);
            if(writtenBytes < 0) {
                sif::error << "Errors writing to watchdog FIFO, code " << errno << ": " <<
                        strerror(errno) << std::endl;
            }
        }
    }

}