diff --git a/bsp_q7s/core/CoreController.cpp b/bsp_q7s/core/CoreController.cpp
index 098124ac..554eb4d9 100644
--- a/bsp_q7s/core/CoreController.cpp
+++ b/bsp_q7s/core/CoreController.cpp
@@ -1189,7 +1189,7 @@ void CoreController::performRebootFileHandling(bool recreateFile) {
   std::string path = sdcMan->getCurrentMountPrefix(sdInfo.pref) + REBOOT_FILE;
   if (not std::filesystem::exists(path) or recreateFile) {
 #if OBSW_VERBOSE_LEVEL >= 1
-    std::cout << "CoreController::performRebootFileHandling: Recreating reboot file" << std::endl;
+    sif::info << "CoreController::performRebootFileHandling: Recreating reboot file" << std::endl;
 #endif
     rebootFile.enabled = true;
     rebootFile.img00Cnt = 0;
@@ -1198,6 +1198,12 @@ void CoreController::performRebootFileHandling(bool recreateFile) {
     rebootFile.img11Cnt = 0;
     rebootFile.lastChip = xsc::Chip::CHIP_0;
     rebootFile.lastCopy = xsc::Copy::COPY_0;
+    rebootFile.img00Lock = false;
+    rebootFile.img01Lock = false;
+    rebootFile.img10Lock = false;
+    rebootFile.img11Lock = false;
+    rebootFile.mechanismNextChip = xsc::Chip::NO_CHIP;
+    rebootFile.mechanismNextCopy = xsc::Copy::NO_COPY;
     rebootFile.bootFlag = false;
     rewriteRebootFile(rebootFile);
   } else {
@@ -1206,41 +1212,17 @@ void CoreController::performRebootFileHandling(bool recreateFile) {
     }
   }
 
-  switch (CURRENT_CHIP) {
-    case (xsc::CHIP_0): {
-      switch (CURRENT_COPY) {
-        case (xsc::COPY_0): {
-          rebootFile.img00Cnt++;
-          break;
-        }
-        case (xsc::COPY_1): {
-          rebootFile.img01Cnt++;
-          break;
-        }
-        default: {
-          break;
-        }
-      }
-      break;
+  if (CURRENT_CHIP == xsc::CHIP_0) {
+    if (CURRENT_COPY == xsc::COPY_0) {
+      rebootFile.img00Cnt++;
+    } else {
+      rebootFile.img01Cnt++;
     }
-    case (xsc::CHIP_1): {
-      switch (CURRENT_COPY) {
-        case (xsc::COPY_0): {
-          rebootFile.img10Cnt++;
-          break;
-        }
-        case (xsc::COPY_1): {
-          rebootFile.img11Cnt++;
-          break;
-        }
-        default: {
-          break;
-        }
-      }
-      break;
-    }
-    default: {
-      break;
+  } else {
+    if (CURRENT_COPY == xsc::COPY_0) {
+      rebootFile.img10Cnt++;
+    } else {
+      rebootFile.img11Cnt++;
     }
   }
 
@@ -1254,6 +1236,34 @@ void CoreController::performRebootFileHandling(bool recreateFile) {
     rebootFile.bootFlag = false;
   }
 
+  if (rebootFile.mechanismNextChip != xsc::NO_CHIP and
+      rebootFile.mechanismNextCopy != xsc::NO_COPY) {
+    if (CURRENT_CHIP != rebootFile.mechanismNextChip or
+        CURRENT_COPY != rebootFile.mechanismNextCopy) {
+      std::string infoString = static_cast<int>(rebootFile.mechanismNextChip) + " " +
+                               static_cast<int>(rebootFile.mechanismNextCopy);
+      sif::warning << "CoreController::performRebootFileHandling: Expected to be on image"
+                   << infoString << " but currently on other image. Locking" << infoString
+                   << std::endl;
+      // Firmware or other component might be corrupt and we are on another image then the target
+      // image specified by the mechanism. We can't really trust the target image anymore.
+      // Lock it for now
+      if (rebootFile.mechanismNextChip == xsc::CHIP_0) {
+        if (rebootFile.mechanismNextCopy == xsc::COPY_0) {
+          rebootFile.img00Lock = true;
+        } else {
+          rebootFile.img01Lock = true;
+        }
+      } else {
+        if (rebootFile.mechanismNextCopy == xsc::COPY_0) {
+          rebootFile.img10Lock = true;
+        } else {
+          rebootFile.img11Lock = true;
+        }
+      }
+    }
+  }
+
   // Only reboot if the reboot functionality is enabled.
   // The handler will still increment the boot counts
   if (rebootFile.enabled and (*rebootFile.relevantBootCnt >= rebootFile.maxCount)) {
@@ -1265,11 +1275,13 @@ void CoreController::performRebootFileHandling(bool recreateFile) {
     if (doReboot) {
       rebootFile.bootFlag = true;
 #if OBSW_VERBOSE_LEVEL >= 1
-      std::cout << "Boot counter for image " << CURRENT_CHIP << " " << CURRENT_COPY
+      sif::info << "Boot counter for image " << CURRENT_CHIP << " " << CURRENT_COPY
                 << " too high. Rebooting to " << tgtChip << " " << tgtCopy << std::endl;
 #endif
       rebootFile.lastChip = CURRENT_CHIP;
       rebootFile.lastCopy = CURRENT_COPY;
+      rebootFile.mechanismNextChip = tgtChip;
+      rebootFile.mechanismNextCopy = tgtCopy;
       rewriteRebootFile(rebootFile);
       xsc_boot_copy(static_cast<xsc_libnor_chip_t>(tgtChip),
                     static_cast<xsc_libnor_copy_t>(tgtCopy));
@@ -1287,82 +1299,84 @@ void CoreController::determineAndExecuteReboot(RebootFile &rf, bool &needsReboot
   if ((CURRENT_CHIP == xsc::CHIP_0) and (CURRENT_COPY == xsc::COPY_0) and
       (rf.img00Cnt >= rf.maxCount)) {
     needsReboot = true;
-    if (rf.img01Cnt >= rf.maxCount) {
-      if (rf.img10Cnt >= rf.maxCount) {
-        if (rf.img11Cnt >= rf.maxCount) {
-          // Can't really do much here. Stay on image
-          std::cout << "All reboot counts too high, but already on fallback image" << std::endl;
-          return;
-        } else {
-          tgtChip = xsc::CHIP_1;
-          tgtCopy = xsc::COPY_1;
-        }
-      } else {
-        tgtChip = xsc::CHIP_1;
-        tgtCopy = xsc::COPY_0;
-      }
-    } else {
+    if (rf.img01Cnt < rf.maxCount and not rf.img01Lock) {
       tgtCopy = xsc::COPY_1;
+      return;
     }
+    if (rf.img10Cnt < rf.maxCount and not rf.img10Lock) {
+      tgtChip = xsc::CHIP_1;
+      return;
+    }
+    if (rf.img11Cnt < rf.maxCount and not rf.img11Lock) {
+      tgtChip = xsc::CHIP_1;
+      tgtCopy = xsc::COPY_1;
+      return;
+    }
+    // Can't really do much here. Stay on image
+    sif::warning
+        << "All reboot counts too high or all fallback images locked, already on fallback image"
+        << std::endl;
+    needsReboot = false;
+    return;
   }
   if ((CURRENT_CHIP == xsc::CHIP_0) and (CURRENT_COPY == xsc::COPY_1) and
       (rf.img01Cnt >= rf.maxCount)) {
     needsReboot = true;
-    if (rf.img00Cnt >= rf.maxCount) {
-      if (rf.img10Cnt >= rf.maxCount) {
-        if (rf.img11Cnt >= rf.maxCount) {
-          // Reboot to fallback image
-        } else {
-          tgtChip = xsc::CHIP_1;
-          tgtCopy = xsc::COPY_1;
-        }
-      } else {
-        tgtChip = xsc::CHIP_1;
-        tgtCopy = xsc::COPY_0;
-      }
-    } else {
+    if (rf.img00Cnt < rf.maxCount and not rf.img00Lock) {
       // Reboot on fallback image
+      return;
     }
+    if (rf.img10Cnt < rf.maxCount and not rf.img10Lock) {
+      tgtChip = xsc::CHIP_1;
+      return;
+    }
+    if (rf.img11Cnt < rf.maxCount and not rf.img11Lock) {
+      tgtChip = xsc::CHIP_1;
+      tgtCopy = xsc::COPY_1;
+    }
+    if (rf.img00Lock) {
+      needsReboot = false;
+    }
+    // Reboot to fallback image
   }
   if ((CURRENT_CHIP == xsc::CHIP_1) and (CURRENT_COPY == xsc::COPY_0) and
       (rf.img10Cnt >= rf.maxCount)) {
     needsReboot = true;
-    if (rf.img11Cnt >= rf.maxCount) {
-      if (rf.img00Cnt >= rf.maxCount) {
-        if (rf.img01Cnt >= rf.maxCount) {
-          // Reboot to fallback image
-        } else {
-          tgtChip = xsc::CHIP_0;
-          tgtCopy = xsc::COPY_1;
-        }
-      } else {
-        tgtChip = xsc::CHIP_0;
-        tgtCopy = xsc::COPY_0;
-      }
-    } else {
+    if (rf.img11Cnt < rf.maxCount and not rf.img11Lock) {
       tgtChip = xsc::CHIP_1;
       tgtCopy = xsc::COPY_1;
+      return;
     }
+    if (rf.img00Cnt < rf.maxCount and not rf.img00Lock) {
+      return;
+    }
+    if (rf.img01Cnt < rf.maxCount and not rf.img01Lock) {
+      tgtCopy = xsc::COPY_1;
+      return;
+    }
+    if (rf.img00Lock) {
+      needsReboot = false;
+    }
+    // Reboot to fallback image
   }
   if ((CURRENT_CHIP == xsc::CHIP_1) and (CURRENT_COPY == xsc::COPY_1) and
       (rf.img11Cnt >= rf.maxCount)) {
     needsReboot = true;
-    if (rf.img10Cnt >= rf.maxCount) {
-      if (rf.img00Cnt >= rf.maxCount) {
-        if (rf.img01Cnt >= rf.maxCount) {
-          // Reboot to fallback image
-        } else {
-          tgtChip = xsc::CHIP_0;
-          tgtCopy = xsc::COPY_1;
-        }
-      } else {
-        tgtChip = xsc::CHIP_0;
-        tgtCopy = xsc::COPY_0;
-      }
-    } else {
+    if (rf.img10Cnt < rf.maxCount and not rf.img10Lock) {
       tgtChip = xsc::CHIP_1;
-      tgtCopy = xsc::COPY_0;
+      return;
     }
+    if (rf.img00Cnt < rf.maxCount and not rf.img00Lock) {
+      return;
+    }
+    if (rf.img01Cnt < rf.maxCount and not rf.img01Lock) {
+      tgtCopy = xsc::COPY_1;
+      return;
+    }
+    if (rf.img00Lock) {
+      needsReboot = false;
+    }
+    // Reboot to fallback image
   }
 }
 
@@ -1451,6 +1465,38 @@ bool CoreController::parseRebootFile(std::string path, RebootFile &rf) {
         break;
       }
       case 6: {
+        iss >> word;
+        if (word.find("img00lock:") == string::npos) {
+          return false;
+        }
+        iss >> rf.img00Lock;
+        break;
+      }
+      case 7: {
+        iss >> word;
+        if (word.find("img01lock:") == string::npos) {
+          return false;
+        }
+        iss >> rf.img01Lock;
+        break;
+      }
+      case 8: {
+        iss >> word;
+        if (word.find("img10lock:") == string::npos) {
+          return false;
+        }
+        iss >> rf.img10Lock;
+        break;
+      }
+      case 9: {
+        iss >> word;
+        if (word.find("img11lock:") == string::npos) {
+          return false;
+        }
+        iss >> rf.img11Lock;
+        break;
+      }
+      case 10: {
         iss >> word;
         if (word.find("bootflag:") == string::npos) {
           return false;
@@ -1458,7 +1504,7 @@ bool CoreController::parseRebootFile(std::string path, RebootFile &rf) {
         iss >> rf.bootFlag;
         break;
       }
-      case 7: {
+      case 11: {
         iss >> word;
         int copyRaw = 0;
         int chipRaw = 0;
@@ -1479,6 +1525,30 @@ bool CoreController::parseRebootFile(std::string path, RebootFile &rf) {
         }
         rf.lastChip = static_cast<xsc::Chip>(chipRaw);
         rf.lastCopy = static_cast<xsc::Copy>(copyRaw);
+        break;
+      }
+      case 12: {
+        iss >> word;
+        int copyRaw = 0;
+        int chipRaw = 0;
+        if (word.find("next:") == string::npos) {
+          return false;
+        }
+        iss >> chipRaw;
+        if (iss.fail()) {
+          return false;
+        }
+        iss >> copyRaw;
+        if (iss.fail()) {
+          return false;
+        }
+
+        if (chipRaw > 2 or copyRaw > 2) {
+          return false;
+        }
+        rf.mechanismNextChip = static_cast<xsc::Chip>(chipRaw);
+        rf.mechanismNextCopy = static_cast<xsc::Copy>(copyRaw);
+        break;
       }
     }
     if (iss.fail()) {
@@ -1486,7 +1556,7 @@ bool CoreController::parseRebootFile(std::string path, RebootFile &rf) {
     }
     lineIdx++;
   }
-  if (lineIdx < 8) {
+  if (lineIdx < 12) {
     return false;
   }
   return true;
@@ -1543,7 +1613,11 @@ void CoreController::rewriteRebootFile(RebootFile file) {
     rebootFile << "on: " << file.enabled << "\nmaxcnt: " << file.maxCount
                << "\nimg00: " << file.img00Cnt << "\nimg01: " << file.img01Cnt
                << "\nimg10: " << file.img10Cnt << "\nimg11: " << file.img11Cnt
+               << "\nimg00lock: " << file.img00Lock << "\nimg01lock: " << file.img01Lock
+               << "\nimg10lock: " << file.img01Lock << "\nimg11lock: " << file.img11Lock
                << "\nbootflag: " << file.bootFlag << "\nlast: " << static_cast<int>(file.lastChip)
-               << " " << static_cast<int>(file.lastCopy) << "\n";
+               << " " << static_cast<int>(file.lastCopy)
+               << "\nnext: " << static_cast<int>(file.mechanismNextChip) << " "
+               << static_cast<int>(file.mechanismNextCopy) << "\n";
   }
 }
diff --git a/bsp_q7s/core/CoreController.h b/bsp_q7s/core/CoreController.h
index 0d034c0b..f6835afa 100644
--- a/bsp_q7s/core/CoreController.h
+++ b/bsp_q7s/core/CoreController.h
@@ -29,10 +29,16 @@ struct RebootFile {
   uint32_t img01Cnt = 0;
   uint32_t img10Cnt = 0;
   uint32_t img11Cnt = 0;
+  bool img00Lock = false;
+  bool img01Lock = false;
+  bool img10Lock = false;
+  bool img11Lock = false;
   uint32_t* relevantBootCnt = &img00Cnt;
   bool bootFlag = false;
   xsc::Chip lastChip = xsc::Chip::CHIP_0;
   xsc::Copy lastCopy = xsc::Copy::COPY_0;
+  xsc::Chip mechanismNextChip = xsc::Chip::NO_CHIP;
+  xsc::Copy mechanismNextCopy = xsc::Copy::NO_COPY;
 };
 
 class CoreController : public ExtendedControllerBase {
diff --git a/unittest/rebootLogic/CMakeLists.txt b/unittest/rebootLogic/CMakeLists.txt
index dbaf2f28..a070f299 100644
--- a/unittest/rebootLogic/CMakeLists.txt
+++ b/unittest/rebootLogic/CMakeLists.txt
@@ -24,10 +24,6 @@ add_executable(reboot-logic)
 add_subdirectory(src)
 target_link_libraries(reboot-logic PRIVATE Catch2::Catch2WithMain)
 
-target_include_directories(reboot-logic PRIVATE
-    ${CMAKE_CURRENT_SOURCE_DIR}
-)
-
 set(CPACK_PROJECT_NAME ${PROJECT_NAME})
 set(CPACK_PROJECT_VERSION ${PROJECT_VERSION})
 include(CPack)
diff --git a/unittest/rebootLogic/src/CMakeLists.txt b/unittest/rebootLogic/src/CMakeLists.txt
index 674c288a..97b73226 100644
--- a/unittest/rebootLogic/src/CMakeLists.txt
+++ b/unittest/rebootLogic/src/CMakeLists.txt
@@ -4,4 +4,11 @@ target_sources(reboot-logic PRIVATE
     SdCardManager.cpp
     event.cpp
     libxiphos.cpp
+    print.c
 )
+
+target_include_directories(reboot-logic PRIVATE
+    ${CMAKE_CURRENT_SOURCE_DIR}
+)
+
+add_subdirectory(fsfw)
diff --git a/unittest/rebootLogic/src/CoreController.cpp b/unittest/rebootLogic/src/CoreController.cpp
index a813cde7..b9ba84be 100644
--- a/unittest/rebootLogic/src/CoreController.cpp
+++ b/unittest/rebootLogic/src/CoreController.cpp
@@ -4,6 +4,7 @@
 #include "SdCardManager.h"
 #include "event.h"
 #include "libxiphos.h"
+#include "fsfw/serviceinterface/ServiceInterface.h"
 
 #include <iostream>
 #include <fstream>
@@ -22,7 +23,7 @@ void CoreController::performRebootFileHandling(bool recreateFile) {
   std::string path = sdcMan->getCurrentMountPrefix(sdInfo.pref) + REBOOT_FILE;
   if (not std::filesystem::exists(path) or recreateFile) {
 #if OBSW_VERBOSE_LEVEL >= 1
-    std::cout << "CoreController::performRebootFileHandling: Recreating reboot file" << std::endl;
+    sif::info << "CoreController::performRebootFileHandling: Recreating reboot file" << std::endl;
 #endif
     rebootFile.enabled = true;
     rebootFile.img00Cnt = 0;
@@ -62,15 +63,22 @@ void CoreController::performRebootFileHandling(bool recreateFile) {
   if (rebootFile.bootFlag) {
     // Trigger event to inform ground that a reboot was triggered
     uint32_t p1 = rebootFile.lastChip << 16 | rebootFile.lastCopy;
-    uint32_t p2 = rebootFile.img00Cnt << 24 | rebootFile.img01Cnt << 16 |
-                  rebootFile.img10Cnt << 8 | rebootFile.img11Cnt;
+    uint32_t p2 = rebootFile.img00Cnt << 24 | rebootFile.img01Cnt << 16 | rebootFile.img10Cnt << 8 |
+                  rebootFile.img11Cnt;
     triggerEvent(REBOOT_MECHANISM_TRIGGERED, p1, p2);
     // Clear the boot flag
     rebootFile.bootFlag = false;
   }
 
-  if(rebootFile.mechanismNextChip != xsc::NO_CHIP and rebootFile.mechanismNextCopy != xsc::NO_COPY) {
-    if(CURRENT_CHIP != rebootFile.mechanismNextChip or CURRENT_COPY != rebootFile.mechanismNextCopy) {
+  if (rebootFile.mechanismNextChip != xsc::NO_CHIP and
+      rebootFile.mechanismNextCopy != xsc::NO_COPY) {
+    if (CURRENT_CHIP != rebootFile.mechanismNextChip or
+        CURRENT_COPY != rebootFile.mechanismNextCopy) {
+      std::string infoString = static_cast<int>(rebootFile.mechanismNextChip) + " " +
+                               static_cast<int>(rebootFile.mechanismNextCopy);
+      sif::warning << "CoreController::performRebootFileHandling: Expected to be on image"
+                   << infoString << " but currently on other image. Locking" << infoString
+                   << std::endl;
       // Firmware or other component might be corrupt and we are on another image then the target
       // image specified by the mechanism. We can't really trust the target image anymore.
       // Lock it for now
@@ -101,9 +109,8 @@ void CoreController::performRebootFileHandling(bool recreateFile) {
     if (doReboot) {
       rebootFile.bootFlag = true;
 #if OBSW_VERBOSE_LEVEL >= 1
-      std::cout << "Boot counter for image " << CURRENT_CHIP << " " << CURRENT_COPY
-                << " too high. Rebooting to " << tgtChip << " " << tgtCopy
-                << std::endl;
+      sif::info << "Boot counter for image " << CURRENT_CHIP << " " << CURRENT_COPY
+                << " too high. Rebooting to " << tgtChip << " " << tgtCopy << std::endl;
 #endif
       rebootFile.lastChip = CURRENT_CHIP;
       rebootFile.lastCopy = CURRENT_COPY;
@@ -118,7 +125,6 @@ void CoreController::performRebootFileHandling(bool recreateFile) {
   }
 }
 
-
 void CoreController::determineAndExecuteReboot(RebootFile &rf, bool &needsReboot,
                                                xsc::Chip &tgtChip, xsc::Copy &tgtCopy) {
   tgtChip = xsc::CHIP_0;
@@ -130,7 +136,7 @@ void CoreController::determineAndExecuteReboot(RebootFile &rf, bool &needsReboot
     if (rf.img01Cnt < rf.maxCount and not rf.img01Lock) {
       tgtCopy = xsc::COPY_1;
       return;
-    } 
+    }
     if (rf.img10Cnt < rf.maxCount and not rf.img10Lock) {
       tgtChip = xsc::CHIP_1;
       return;
@@ -141,7 +147,9 @@ void CoreController::determineAndExecuteReboot(RebootFile &rf, bool &needsReboot
       return;
     }
     // Can't really do much here. Stay on image
-    std::cout << "All reboot counts too high or all fallback images locked, already on fallback image" << std::endl;
+    sif::warning
+        << "All reboot counts too high or all fallback images locked, already on fallback image"
+        << std::endl;
     needsReboot = false;
     return;
   }
@@ -160,12 +168,12 @@ void CoreController::determineAndExecuteReboot(RebootFile &rf, bool &needsReboot
       tgtChip = xsc::CHIP_1;
       tgtCopy = xsc::COPY_1;
     }
-    if(rf.img00Lock) {
+    if (rf.img00Lock) {
       needsReboot = false;
     }
     // Reboot to fallback image
   }
-  if ((CURRENT_CHIP == xsc::CHIP_1) and (CURRENT_COPY == xsc::COPY_0) and 
+  if ((CURRENT_CHIP == xsc::CHIP_1) and (CURRENT_COPY == xsc::COPY_0) and
       (rf.img10Cnt >= rf.maxCount)) {
     needsReboot = true;
     if (rf.img11Cnt < rf.maxCount and not rf.img11Lock) {
@@ -180,7 +188,7 @@ void CoreController::determineAndExecuteReboot(RebootFile &rf, bool &needsReboot
       tgtCopy = xsc::COPY_1;
       return;
     }
-    if(rf.img00Lock) {
+    if (rf.img00Lock) {
       needsReboot = false;
     }
     // Reboot to fallback image
@@ -199,7 +207,7 @@ void CoreController::determineAndExecuteReboot(RebootFile &rf, bool &needsReboot
       tgtCopy = xsc::COPY_1;
       return;
     }
-    if(rf.img00Lock) {
+    if (rf.img00Lock) {
       needsReboot = false;
     }
     // Reboot to fallback image
@@ -382,7 +390,7 @@ bool CoreController::parseRebootFile(std::string path, RebootFile &rf) {
     }
     lineIdx++;
   }
-  if (lineIdx < 8) {
+  if (lineIdx < 12) {
     return false;
   }
   return true;
diff --git a/unittest/rebootLogic/src/fsfw/CMakeLists.txt b/unittest/rebootLogic/src/fsfw/CMakeLists.txt
new file mode 100644
index 00000000..13b08d67
--- /dev/null
+++ b/unittest/rebootLogic/src/fsfw/CMakeLists.txt
@@ -0,0 +1 @@
+add_subdirectory(serviceinterface)
diff --git a/unittest/rebootLogic/src/fsfw/FSFW.h b/unittest/rebootLogic/src/fsfw/FSFW.h
new file mode 100644
index 00000000..06d1f9b5
--- /dev/null
+++ b/unittest/rebootLogic/src/fsfw/FSFW.h
@@ -0,0 +1,9 @@
+#pragma once
+
+#include <cstddef>
+
+#define FSFW_CPP_OSTREAM_ENABLED 1
+
+namespace fsfwconfig {
+static constexpr size_t FSFW_PRINT_BUFFER_SIZE = 256;
+}
\ No newline at end of file
diff --git a/unittest/rebootLogic/src/fsfw/serviceinterface/CMakeLists.txt b/unittest/rebootLogic/src/fsfw/serviceinterface/CMakeLists.txt
new file mode 100644
index 00000000..7eded75e
--- /dev/null
+++ b/unittest/rebootLogic/src/fsfw/serviceinterface/CMakeLists.txt
@@ -0,0 +1,4 @@
+target_sources(reboot-logic PRIVATE
+  ServiceInterfaceStream.cpp
+  ServiceInterfaceBuffer.cpp
+)
\ No newline at end of file
diff --git a/unittest/rebootLogic/src/fsfw/serviceinterface/ServiceInterface.h b/unittest/rebootLogic/src/fsfw/serviceinterface/ServiceInterface.h
new file mode 100644
index 00000000..9f05b96c
--- /dev/null
+++ b/unittest/rebootLogic/src/fsfw/serviceinterface/ServiceInterface.h
@@ -0,0 +1,13 @@
+#ifndef FSFW_SERVICEINTERFACE_SERVICEINTERFACE_H_
+#define FSFW_SERVICEINTERFACE_SERVICEINTERFACE_H_
+
+#include "fsfw/FSFW.h"
+#include "serviceInterfaceDefintions.h"
+
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+#include "ServiceInterfaceStream.h"
+#else
+#include "ServiceInterfacePrinter.h"
+#endif
+
+#endif /* FSFW_SERVICEINTERFACE_SERVICEINTERFACE_H_ */
diff --git a/unittest/rebootLogic/src/fsfw/serviceinterface/ServiceInterfaceBuffer.cpp b/unittest/rebootLogic/src/fsfw/serviceinterface/ServiceInterfaceBuffer.cpp
new file mode 100644
index 00000000..eb951e5c
--- /dev/null
+++ b/unittest/rebootLogic/src/fsfw/serviceinterface/ServiceInterfaceBuffer.cpp
@@ -0,0 +1,252 @@
+#include "ServiceInterfaceBuffer.h"
+
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+
+#include <inttypes.h>
+
+#include <cstring>
+
+#include "fsfw/serviceinterface/serviceInterfaceDefintions.h"
+
+#if defined(WIN32) && FSFW_COLORED_OUTPUT == 1
+#include "Windows.h"
+#endif
+
+// to be implemented by bsp
+extern "C" void printChar(const char*, bool errStream);
+
+#ifndef UT699
+
+ServiceInterfaceBuffer::ServiceInterfaceBuffer(std::string setMessage, bool addCrToPreamble,
+                                               bool buffered, bool errStream, uint16_t port)
+    : isActive(true),
+      logMessage(setMessage),
+      addCrToPreamble(addCrToPreamble),
+      buffered(buffered),
+      errStream(errStream) {
+  if (buffered) {
+    // Set pointers if the stream is buffered.
+    setp(buf, buf + BUF_SIZE);
+  }
+
+#if FSFW_COLORED_OUTPUT == 1
+  if (setMessage.find("DEBUG") != std::string::npos) {
+    colorPrefix = sif::ANSI_COLOR_CYAN;
+  } else if (setMessage.find("INFO") != std::string::npos) {
+    colorPrefix = sif::ANSI_COLOR_GREEN;
+  } else if (setMessage.find("WARNING") != std::string::npos) {
+    colorPrefix = sif::ANSI_COLOR_MAGENTA;
+  } else if (setMessage.find("ERROR") != std::string::npos) {
+    colorPrefix = sif::ANSI_COLOR_RED;
+  } else {
+    colorPrefix = sif::ANSI_COLOR_RESET;
+  }
+
+#ifdef WIN32
+  HANDLE hOut = GetStdHandle(STD_OUTPUT_HANDLE);
+  DWORD dwMode = 0;
+  GetConsoleMode(hOut, &dwMode);
+  dwMode |= ENABLE_VIRTUAL_TERMINAL_PROCESSING;
+  SetConsoleMode(hOut, dwMode);
+#endif
+
+#endif
+
+  preamble.reserve(MAX_PREAMBLE_SIZE);
+  preamble.resize(MAX_PREAMBLE_SIZE);
+}
+
+void ServiceInterfaceBuffer::putChars(char const* begin, char const* end) {
+  char array[BUF_SIZE];
+  uint32_t length = end - begin;
+  if (length > sizeof(array)) {
+    length = sizeof(array);
+  }
+  memcpy(array, begin, length);
+
+  for (; begin != end; begin++) {
+    if (errStream) {
+      printChar(begin, true);
+    } else {
+      printChar(begin, false);
+    }
+  }
+}
+
+#endif
+
+int ServiceInterfaceBuffer::overflow(int c) {
+  if (not buffered and this->isActive) {
+    if (c != Traits::eof()) {
+      if (errStream) {
+        printChar(reinterpret_cast<const char*>(&c), true);
+      } else {
+        printChar(reinterpret_cast<const char*>(&c), false);
+      }
+    }
+    return 0;
+  }
+  // Handle output
+  putChars(pbase(), pptr());
+  if (c != Traits::eof()) {
+    char c2 = c;
+    // Handle the one character that didn't fit to buffer
+    putChars(&c2, &c2 + 1);
+  }
+  // This tells that buffer is empty again
+  setp(buf, buf + BUF_SIZE - 1);
+  // I'm not sure about this return value!
+  return 0;
+}
+
+int ServiceInterfaceBuffer::sync(void) {
+  if (not this->isActive and not buffered) {
+    if (not buffered) {
+      setp(buf, buf + BUF_SIZE - 1);
+    }
+    return 0;
+  }
+  if (not buffered) {
+    return 0;
+  }
+
+  size_t preambleSize = 0;
+  std::string* preamble = getPreamble(&preambleSize);
+  // Write logMessage and time
+  this->putChars(preamble->data(), preamble->data() + preambleSize);
+  // Handle output
+  this->putChars(pbase(), pptr());
+  // This tells that buffer is empty again
+  setp(buf, buf + BUF_SIZE - 1);
+  return 0;
+}
+
+bool ServiceInterfaceBuffer::isBuffered() const { return buffered; }
+
+std::string* ServiceInterfaceBuffer::getPreamble(size_t* preambleSize) {
+  size_t currentSize = 0;
+  char* parsePosition = &preamble[0];
+  if (addCrToPreamble) {
+    preamble[0] = '\r';
+    currentSize += 1;
+    parsePosition += 1;
+  }
+
+#if FSFW_COLORED_OUTPUT == 1
+  currentSize += sprintf(parsePosition, "%s", colorPrefix.c_str());
+  parsePosition += colorPrefix.size();
+#endif
+
+  int32_t charCount =
+      sprintf(parsePosition, "%s%s | ", this->logMessage.c_str(), sif::ANSI_COLOR_RESET);
+  if (charCount < 0) {
+    printf("ServiceInterfaceBuffer: Failure parsing preamble\r\n");
+    return &preamble;
+  }
+  if (charCount > MAX_PREAMBLE_SIZE) {
+    printf(
+        "ServiceInterfaceBuffer: Char count too large for maximum "
+        "preamble size");
+    return &preamble;
+  }
+  currentSize += charCount;
+  if (preambleSize != nullptr) {
+    *preambleSize = currentSize;
+  }
+  return &preamble;
+}
+
+bool ServiceInterfaceBuffer::crAdditionEnabled() const { return addCrToPreamble; }
+
+#if FSFW_COLORED_OUTPUT == 1
+void ServiceInterfaceBuffer::setAsciiColorPrefix(std::string colorPrefix) {
+  this->colorPrefix = colorPrefix;
+}
+#endif
+
+#ifdef UT699
+#include "../osal/rtems/Interrupt.h"
+
+ServiceInterfaceBuffer::ServiceInterfaceBuffer(std::string set_message, uint16_t port) {
+  this->log_message = set_message;
+  this->isActive = true;
+  setp(buf, buf + BUF_SIZE);
+}
+
+void ServiceInterfaceBuffer::putChars(char const* begin, char const* end) {
+  char array[BUF_SIZE];
+  uint32_t length = end - begin;
+  if (length > sizeof(array)) {
+    length = sizeof(array);
+  }
+  memcpy(array, begin, length);
+
+  if (!Interrupt::isInterruptInProgress()) {
+    std::cout << array;
+  } else {
+    // Uncomment the following line if you need ISR debug output.
+    //		printk(array);
+  }
+}
+#endif  // UT699
+
+#ifdef ML505
+#include <bsp_flp/network/networkconfig.h>
+ServiceInterfaceBuffer::ServiceInterfaceBuffer(std::string set_message, uint16_t port)
+    : isActive(true),
+      log_message(set_message),
+      udpSocket(0),
+      remoteAddressLength(sizeof(remoteAddress)) {
+  setp(buf, buf + BUF_SIZE);
+  memset((uint8_t*)&remoteAddress, 0, sizeof(remoteAddress));
+  remoteAddress.sin_family = AF_INET;
+  remoteAddress.sin_port = htons(port);
+  remoteAddress.sin_addr.s_addr = htonl(inet_addr("192.168.250.100"));
+}
+
+void ServiceInterfaceBuffer::putChars(char const* begin, char const* end) {
+  char array[BUF_SIZE];
+  uint32_t length = end - begin;
+  if (length > sizeof(array)) {
+    length = sizeof(array);
+  }
+  memcpy(array, begin, length);
+
+  if (udpSocket <= 0) {
+    initSocket();
+  }
+
+  if (udpSocket > 0) {
+    sendto(udpSocket, array, length, 0, (sockaddr*)&remoteAddress, sizeof(remoteAddress));
+  }
+}
+
+void ServiceInterfaceBuffer::initSocket() {
+  sockaddr_in address;
+  memset((uint8_t*)&address, 0, sizeof(address));
+  address.sin_family = AF_INET;
+  address.sin_port = htons(0);
+  address.sin_addr.s_addr = htonl(INADDR_ANY);
+
+  udpSocket = socket(PF_INET, SOCK_DGRAM, 0);
+  if (socket < 0) {
+    printf("Error opening socket!\n");
+    return;
+  }
+  timeval timeout = {0, 20};
+  if (setsockopt(udpSocket, SOL_SOCKET, SO_RCVTIMEO, &timeout, sizeof(timeout)) < 0) {
+    printf("Error setting SO_RCVTIMEO socket options!\n");
+    return;
+  }
+  if (setsockopt(udpSocket, SOL_SOCKET, SO_SNDTIMEO, &timeout, sizeof(timeout)) < 0) {
+    printf("Error setting SO_SNDTIMEO socket options!\n");
+    return;
+  }
+  if (bind(udpSocket, (sockaddr*)&address, sizeof(address)) < 0) {
+    printf("Error binding socket!\n");
+  }
+}
+
+#endif  // ML505
+
+#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
diff --git a/unittest/rebootLogic/src/fsfw/serviceinterface/ServiceInterfaceBuffer.h b/unittest/rebootLogic/src/fsfw/serviceinterface/ServiceInterfaceBuffer.h
new file mode 100644
index 00000000..5b720931
--- /dev/null
+++ b/unittest/rebootLogic/src/fsfw/serviceinterface/ServiceInterfaceBuffer.h
@@ -0,0 +1,159 @@
+#ifndef FRAMEWORK_SERVICEINTERFACE_SERVICEINTERFACEBUFFER_H_
+#define FRAMEWORK_SERVICEINTERFACE_SERVICEINTERFACEBUFFER_H_
+
+#include "fsfw/FSFW.h"
+
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+
+#include <iomanip>
+#include <iostream>
+#include <sstream>
+
+#ifndef UT699
+
+/**
+ * @brief 	This is the underlying stream buffer which implements the
+ * 			streambuf class and overloads the overflow() and sync() methods
+ * @details
+ * This class is used to modify the output of the stream, for example by adding.
+ * It also calls the char printing function which is implemented in the
+ * board supply package (BSP).
+ */
+class ServiceInterfaceBuffer : public std::streambuf {
+  friend class ServiceInterfaceStream;
+
+ public:
+  static constexpr uint8_t MAX_PREAMBLE_SIZE = 40;
+
+  ServiceInterfaceBuffer(std::string setMessage, bool addCrToPreamble, bool buffered,
+                         bool errStream, uint16_t port);
+
+ protected:
+  bool isActive;
+  //! This is called when buffer becomes full. If
+  //! buffer is not used, then this is called every
+  //! time when characters are put to stream.
+  int overflow(int c = Traits::eof()) override;
+
+  //! This function is called when stream is flushed,
+  //! for example when std::endl is put to stream.
+  int sync(void) override;
+
+  bool isBuffered() const;
+
+ private:
+  //! For additional message information
+  std::string logMessage;
+  std::string preamble;
+
+#if FSFW_COLORED_OUTPUT == 1
+  std::string colorPrefix;
+  void setAsciiColorPrefix(std::string colorPrefix);
+#endif
+
+  // For EOF detection
+  typedef std::char_traits<char> Traits;
+
+  //! This is useful for some terminal programs which do not have
+  //! implicit carriage return with newline characters.
+  bool addCrToPreamble;
+
+  //! Specifies whether the stream operates in buffered or unbuffered mode.
+  bool buffered;
+  //! This specifies to print to stderr and work in unbuffered mode.
+  bool errStream;
+
+  //! Needed for buffered mode.
+  static size_t const BUF_SIZE = fsfwconfig::FSFW_PRINT_BUFFER_SIZE;
+  char buf[BUF_SIZE];
+
+  //! In this function, the characters are parsed.
+  void putChars(char const* begin, char const* end);
+
+  std::string* getPreamble(size_t* preambleSize = nullptr);
+
+  bool crAdditionEnabled() const;
+};
+
+#endif
+
+#ifdef UT699
+class ServiceInterfaceBuffer : public std::basic_streambuf<char, std::char_traits<char> > {
+  friend class ServiceInterfaceStream;
+
+ public:
+  ServiceInterfaceBuffer(std::string set_message, uint16_t port);
+
+ protected:
+  bool isActive;
+  // This is called when buffer becomes full. If
+  // buffer is not used, then this is called every
+  // time when characters are put to stream.
+  virtual int overflow(int c = Traits::eof());
+
+  // This function is called when stream is flushed,
+  // for example when std::endl is put to stream.
+  virtual int sync(void);
+
+ private:
+  // For additional message information
+  std::string log_message;
+  // For EOF detection
+  typedef std::char_traits<char> Traits;
+
+  // Work in buffer mode. It is also possible to work without buffer.
+  static size_t const BUF_SIZE = 128;
+  char buf[BUF_SIZE];
+
+  // In this function, the characters are parsed.
+  void putChars(char const* begin, char const* end);
+};
+#endif  // UT699
+
+#ifdef ML505
+#include <arpa/inet.h>
+#include <netinet/in.h>
+#include <netinet/udp.h>
+#include <sys/socket.h>
+#include <sys/types.h>
+
+class ServiceInterfaceBuffer : public std::basic_streambuf<char, std::char_traits<char> > {
+  friend class ServiceInterfaceStream;
+
+ public:
+  ServiceInterfaceBuffer(std::string set_message, uint16_t port);
+
+ protected:
+  bool isActive;
+  // This is called when buffer becomes full. If
+  // buffer is not used, then this is called every
+  // time when characters are put to stream.
+  virtual int overflow(int c = Traits::eof());
+
+  // This function is called when stream is flushed,
+  // for example when std::endl is put to stream.
+  virtual int sync(void);
+
+ private:
+  // For additional message information
+  std::string log_message;
+  // For EOF detection
+  typedef std::char_traits<char> Traits;
+
+  // Work in buffer mode. It is also possible to work without buffer.
+  static size_t const BUF_SIZE = 128;
+  char buf[BUF_SIZE];
+
+  // In this function, the characters are parsed.
+  void putChars(char const* begin, char const* end);
+
+  int udpSocket;
+  sockaddr_in remoteAddress;
+  socklen_t remoteAddressLength;
+  void initSocket();
+};
+#endif  // ML505
+
+#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
+
+#endif /* FRAMEWORK_SERVICEINTERFACE_SERVICEINTERFACEBUFFER_H_ */
diff --git a/unittest/rebootLogic/src/fsfw/serviceinterface/ServiceInterfaceStream.cpp b/unittest/rebootLogic/src/fsfw/serviceinterface/ServiceInterfaceStream.cpp
new file mode 100644
index 00000000..3ef68c2a
--- /dev/null
+++ b/unittest/rebootLogic/src/fsfw/serviceinterface/ServiceInterfaceStream.cpp
@@ -0,0 +1,21 @@
+#include "ServiceInterfaceStream.h"
+
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+
+ServiceInterfaceStream::ServiceInterfaceStream(std::string setMessage, bool addCrToPreamble,
+                                               bool buffered, bool errStream, uint16_t port)
+    : std::ostream(&streambuf), streambuf(setMessage, addCrToPreamble, buffered, errStream, port) {}
+
+void ServiceInterfaceStream::setActive(bool myActive) { this->streambuf.isActive = myActive; }
+
+std::string* ServiceInterfaceStream::getPreamble() { return streambuf.getPreamble(); }
+
+bool ServiceInterfaceStream::crAdditionEnabled() const { return streambuf.crAdditionEnabled(); }
+
+#if FSFW_COLORED_OUTPUT == 1
+void ServiceInterfaceStream::setAsciiColorPrefix(std::string asciiColorCode) {
+  streambuf.setAsciiColorPrefix(asciiColorCode);
+}
+#endif
+
+#endif
diff --git a/unittest/rebootLogic/src/fsfw/serviceinterface/ServiceInterfaceStream.h b/unittest/rebootLogic/src/fsfw/serviceinterface/ServiceInterfaceStream.h
new file mode 100644
index 00000000..bf3424a6
--- /dev/null
+++ b/unittest/rebootLogic/src/fsfw/serviceinterface/ServiceInterfaceStream.h
@@ -0,0 +1,68 @@
+#ifndef FRAMEWORK_SERVICEINTERFACE_SERVICEINTERFACESTREAM_H_
+#define FRAMEWORK_SERVICEINTERFACE_SERVICEINTERFACESTREAM_H_
+
+#include "fsfw/FSFW.h"
+
+#include "ServiceInterfaceBuffer.h"
+
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+
+#include <cstdio>
+#include <iostream>
+
+/**
+ * Generic service interface stream which can be used like std::cout or
+ * std::cerr but has additional capability. Add preamble and timestamp
+ * to output. Can be run in buffered or unbuffered mode.
+ */
+class ServiceInterfaceStream : public std::ostream {
+ public:
+  /**
+   * This constructor is used by specifying the preamble message.
+   * Optionally, the output can be directed to stderr and a CR character
+   * can be prepended to the preamble.
+   * @param setMessage message of preamble.
+   * @param addCrToPreamble Useful for applications like Puttty.
+   * @param buffered specify whether to use buffered mode.
+   * @param errStream specify which output stream to use (stderr or stdout).
+   */
+  ServiceInterfaceStream(std::string setMessage, bool addCrToPreamble = false, bool buffered = true,
+                         bool errStream = false, uint16_t port = 1234);
+
+  //! An inactive stream will not print anything.
+  void setActive(bool);
+
+  /**
+   * This can be used to retrieve the preamble in case it should be printed in
+   * the unbuffered mode.
+   * @return Preamle consisting of log message and timestamp.
+   */
+  std::string* getPreamble();
+
+  /**
+   * Can be used to determine if the stream was configured to add CR characters in addition
+   * to newline characters.
+   * @return
+   */
+  bool crAdditionEnabled() const;
+
+#if FSFW_COLORED_OUTPUT == 1
+  void setAsciiColorPrefix(std::string asciiColorCode);
+#endif
+
+ protected:
+  ServiceInterfaceBuffer streambuf;
+};
+
+// Forward declaration of interface streams. These should be instantiated in
+// main. They can then be used like std::cout or std::cerr.
+namespace sif {
+extern ServiceInterfaceStream debug;
+extern ServiceInterfaceStream info;
+extern ServiceInterfaceStream warning;
+extern ServiceInterfaceStream error;
+}  // namespace sif
+
+#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
+
+#endif /* FRAMEWORK_SERVICEINTERFACE_SERVICEINTERFACESTREAM_H_ */
diff --git a/unittest/rebootLogic/src/fsfw/serviceinterface/serviceInterfaceDefintions.h b/unittest/rebootLogic/src/fsfw/serviceinterface/serviceInterfaceDefintions.h
new file mode 100644
index 00000000..85710578
--- /dev/null
+++ b/unittest/rebootLogic/src/fsfw/serviceinterface/serviceInterfaceDefintions.h
@@ -0,0 +1,18 @@
+#ifndef FSFW_SERVICEINTERFACE_SERVICEINTERFACEDEFINTIONS_H_
+#define FSFW_SERVICEINTERFACE_SERVICEINTERFACEDEFINTIONS_H_
+
+namespace sif {
+
+enum class OutputTypes { OUT_INFO, OUT_DEBUG, OUT_WARNING, OUT_ERROR };
+
+static const char* const ANSI_COLOR_RED = "\x1b[31m";
+static const char* const ANSI_COLOR_GREEN = "\x1b[32m";
+static const char* const ANSI_COLOR_YELLOW = "\x1b[33m";
+static const char* const ANSI_COLOR_BLUE = "\x1b[34m";
+static const char* const ANSI_COLOR_MAGENTA = "\x1b[35m";
+static const char* const ANSI_COLOR_CYAN = "\x1b[36m";
+static const char* const ANSI_COLOR_RESET = "\x1b[0m";
+
+}  // namespace sif
+
+#endif /* FSFW_SERVICEINTERFACE_SERVICEINTERFACEDEFINTIONS_H_ */
diff --git a/unittest/rebootLogic/src/main.cpp b/unittest/rebootLogic/src/main.cpp
index fbd6cc03..57470861 100644
--- a/unittest/rebootLogic/src/main.cpp
+++ b/unittest/rebootLogic/src/main.cpp
@@ -2,6 +2,7 @@
 #include "libxiphos.h"
 #include "HasActionsIF.h"
 #include "event.h"
+#include "fsfw/serviceinterface/ServiceInterface.h"
 
 #include <catch2/catch_test_macros.hpp>
 #include <iostream>
@@ -14,6 +15,11 @@ const std::string REBOOT_FILE = CONF_PATH + "/reboot.txt";
 
 void catFileToConsole();
 
+ServiceInterfaceStream sif::debug("DEBUG");
+ServiceInterfaceStream sif::info("INFO");
+ServiceInterfaceStream sif::warning("WARNING");
+ServiceInterfaceStream sif::error("ERROR", false, false, true);
+
 TEST_CASE( "Core Controller Reboot File Handling", "[reboot-file]" ) {
   if(not std::filesystem::exists(CONF_PATH)) {
     std::filesystem::create_directory(CONF_PATH);
diff --git a/unittest/rebootLogic/src/print.c b/unittest/rebootLogic/src/print.c
new file mode 100644
index 00000000..a59492b0
--- /dev/null
+++ b/unittest/rebootLogic/src/print.c
@@ -0,0 +1,10 @@
+#include <stdio.h>
+#include <stdbool.h>
+
+void printChar(const char* character, bool errStream) {
+  if (errStream) {
+    putc(*character, stderr);
+    return;
+  }
+  putc(*character, stdout);
+}