bsp_q7s/core/CoreController.cpp

@@ -85,6 +85,28 @@ ReturnValue_t CoreController::initialize() {
     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;
@@ -538,29 +560,6 @@ ReturnValue_t CoreController::executeAction(ActionId_t actionId, MessageQueueId_
     }
 }
 
-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");
-    sif::info << currentEnvPath << std::endl;
-    std::string updatedEnvPath = std::string(currentEnvPath) + ":/home/root/scripts";
-    setenv("PATH", updatedEnvPath.c_str(), true);
-    updateProtInfo();
-    initPrint();
-    return result;
-}
-
 ReturnValue_t CoreController::sdColdRedundantBlockingInit() {
     ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;
 
@@ -919,6 +918,7 @@ ReturnValue_t CoreController::checkAndSetBootCopyProtection(Chip targetChip, Cop
     }
     case(Chip::SELF_CHIP): {
         selfChip = true;
+        targetChip = CURRENT_CHIP;
         break;
     }
     default: {
@@ -935,6 +935,7 @@ ReturnValue_t CoreController::checkAndSetBootCopyProtection(Chip targetChip, Cop
     }
     case(Copy::SELF_COPY): {
         selfCopy = true;
+        targetCopy = CURRENT_COPY;
         break;
     }
     default: {
@@ -942,12 +943,18 @@ ReturnValue_t CoreController::checkAndSetBootCopyProtection(Chip targetChip, Cop
     }
     }
 
-    for(uint8_t arrIdx = 0; arrIdx < 4; arrIdx++) {
+    for(uint8_t arrIdx = 0; arrIdx < protArray.size(); arrIdx++) {
         bool currentProt = protArray[arrIdx];
         std::ostringstream oss;
         if(protect == currentProt) {
             continue;
         }
+        if(protOperationPerformed) {
+            if((selfChip and selfCopy) or (not allCopies and not allChips)) {
+                // No need to continue, only one operation was requested
+                break;
+            }
+        }
         Chip currentChip;
         Copy currentCopy;
         oss << "writeprotect ";
@@ -1045,6 +1052,9 @@ ReturnValue_t CoreController::updateProtInfo(bool regenerateChipStateFile) {
                     "Line counter larger than 4" << std::endl;
         }
     }
+    for(uint8_t idx = 0; idx < 4; idx ++) {
+        sif::debug << (int) protArray[idx] << std::endl;
+    }
     return HasReturnvaluesIF::RETURN_OK;
 }
 
@@ -1052,6 +1062,7 @@ 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;
     Copy currentCopy;
@@ -1063,13 +1074,13 @@ ReturnValue_t CoreController::handleProtInfoUpdateLine(std::string nextLine) {
             currentCopy = static_cast<Copy>(stoi(word));
         }
 
-        uint8_t arrayIdx = 0;
+        if(wordIdx == 3) {
             if(currentChip == Chip::CHIP_0) {
-            if(currentCopy == Copy::COPY_1) {
-                arrayIdx = 1;
+                if(currentCopy == Copy::COPY_0) {
+                    arrayIdx = 0;
                 }
-            else if(currentCopy != Copy::COPY_0) {
-                return HasReturnvaluesIF::RETURN_FAILED;
+                else if(currentCopy == Copy::COPY_1) {
+                    arrayIdx = 1;
                 }
             }
 
@@ -1080,11 +1091,8 @@ ReturnValue_t CoreController::handleProtInfoUpdateLine(std::string nextLine) {
                else if(currentCopy == Copy::COPY_1) {
                    arrayIdx = 3;
                }
-            else {
-                return HasReturnvaluesIF::RETURN_FAILED;
            }
         }
-
         if(wordIdx == 5) {
             if(word == "unlocked.") {
                 protArray[arrayIdx] = false;
@@ -1093,6 +1101,7 @@ ReturnValue_t CoreController::handleProtInfoUpdateLine(std::string nextLine) {
                 protArray[arrayIdx] = true;
             }
         }
+        wordIdx++;
     }
     return HasReturnvaluesIF::RETURN_OK;
 }