diff --git a/src/logic/CMakeLists.txt b/src/logic/CMakeLists.txt
index 295dcb0..481a2cf 100644
--- a/src/logic/CMakeLists.txt
+++ b/src/logic/CMakeLists.txt
@@ -6,6 +6,7 @@ target_sources(
           feed_to_bondtech.cpp
           feed_to_finda.cpp
           home.cpp
+          idle_mode.cpp
           load_filament.cpp
           move_selector.cpp
           no_command.cpp
diff --git a/src/logic/command_base.cpp b/src/logic/command_base.cpp
index 7152897..10a2673 100644
--- a/src/logic/command_base.cpp
+++ b/src/logic/command_base.cpp
@@ -1,5 +1,6 @@
 /// @file command_base.cpp
 #include "command_base.h"
+#include "idle_mode.h"
 #include "../modules/globals.h"
 #include "../modules/finda.h"
 #include "../modules/fsensor.h"
@@ -206,4 +207,10 @@ void CommandBase::GoToErrEngagingIdler() {
     mi::idler.Engage(mg::globals.ActiveSlot());
 }
 
+void CommandBase::FinishedOK() {
+    state = ProgressCode::OK;
+    error = ErrorCode::OK;
+    idleMode.CommandFinishedCorrectly();
+}
+
 } // namespace logic
diff --git a/src/logic/command_base.h b/src/logic/command_base.h
index afb6688..0d8eee5 100644
--- a/src/logic/command_base.h
+++ b/src/logic/command_base.h
@@ -109,6 +109,9 @@ protected:
     /// Transit the state machine into ErrEngagingIdler
     void GoToErrEngagingIdler();
 
+    /// Process end of command which finished OK
+    void FinishedOK();
+
     ProgressCode state; ///< current progress state of the state machine
     ErrorCode error; ///< current error code
     ProgressCode stateBeforeModuleFailed; ///< saved state of the state machine before a common error happened
diff --git a/src/logic/cut_filament.cpp b/src/logic/cut_filament.cpp
index 304b997..7326283 100644
--- a/src/logic/cut_filament.cpp
+++ b/src/logic/cut_filament.cpp
@@ -92,8 +92,7 @@ bool CutFilament::StepInner() {
         break;
     case ProgressCode::ReturningSelector:
         if (ms::selector.Slot() == 5) { // selector returned to position, feed the filament back to FINDA
-            state = ProgressCode::OK;
-            error = ErrorCode::OK;
+            FinishedOK();
             ml::leds.SetPairButOffOthers(mg::globals.ActiveSlot(), ml::on, ml::off);
             feed.Reset(true, true);
         }
diff --git a/src/logic/eject_filament.cpp b/src/logic/eject_filament.cpp
index de62f15..4eff71e 100644
--- a/src/logic/eject_filament.cpp
+++ b/src/logic/eject_filament.cpp
@@ -24,7 +24,7 @@ void EjectFilament::Reset(uint8_t param) {
     slot = param;
 
     if (mg::globals.FilamentLoaded() == mg::FilamentLoadState::NotLoaded) {
-        state = ProgressCode::OK;
+        FinishedOK();
         dbg_logic_P(PSTR("Already ejected"));
     } else if (mg::globals.FilamentLoaded() >= mg::FilamentLoadState::AtPulley) {
         state = ProgressCode::UnloadingFilament;
@@ -68,8 +68,7 @@ bool EjectFilament::StepInner() {
         if (!mi::idler.Engaged()) { // idler disengaged
             mpu::pulley.Disable();
             mg::globals.SetFilamentLoaded(mg::globals.ActiveSlot(), mg::FilamentLoadState::NotLoaded);
-            state = ProgressCode::OK;
-            error = ErrorCode::OK;
+            FinishedOK();
         }
         break;
     case ProgressCode::OK:
diff --git a/src/logic/home.cpp b/src/logic/home.cpp
index 6aec28b..642fcfa 100644
--- a/src/logic/home.cpp
+++ b/src/logic/home.cpp
@@ -22,8 +22,7 @@ bool Home::StepInner() {
     switch (state) {
     case ProgressCode::Homing:
         if (mi::idler.State() == mi::Idler::Ready && ms::selector.State() == ms::selector.Ready) {
-            state = ProgressCode::OK;
-            error = ErrorCode::OK;
+            FinishedOK();
         }
         break;
     case ProgressCode::OK:
diff --git a/src/logic/idle_mode.cpp b/src/logic/idle_mode.cpp
new file mode 100644
index 0000000..03eeb71
--- /dev/null
+++ b/src/logic/idle_mode.cpp
@@ -0,0 +1,264 @@
+/// @file
+#include "idle_mode.h"
+
+#include "../modules/timebase.h"
+#include "../modules/leds.h"
+#include "../modules/globals.h"
+#include "../modules/user_input.h"
+#include "../modules/finda.h"
+#include "../modules/fsensor.h"
+#include "../modules/serial.h"
+
+#include "command_base.h"
+#include "cut_filament.h"
+#include "eject_filament.h"
+#include "home.h"
+#include "load_filament.h"
+#include "move_selector.h"
+#include "no_command.h"
+#include "set_mode.h"
+#include "tool_change.h"
+#include "unload_filament.h"
+
+#include "../version.h"
+
+#include "../panic.h"
+
+/// Global instance of the protocol codec
+static mp::Protocol protocol;
+
+namespace logic {
+
+IdleMode idleMode;
+
+IdleMode::IdleMode()
+    : lastCommandProcessedMs(0)
+    , currentCommand(&noCommand)
+    , currentCommandRq(mp::RequestMsgCodes::Reset, 0) {}
+
+void IdleMode::CheckManualOperation() {
+    uint16_t ms = mt::timebase.Millis();
+    constexpr uint16_t idleDelay = 1000U;
+    if (ms - lastCommandProcessedMs < idleDelay) {
+        if (currentCommand->State() == ProgressCode::OK) {
+            mui::userInput.Clear(); // consume bogus UI events while no command in progress and not in idle state yet
+        }
+        return;
+    }
+
+    lastCommandProcessedMs = ms - idleDelay; // prevent future overflows
+
+    if (currentCommand->State() == ProgressCode::OK && mg::globals.FilamentLoaded() <= mg::FilamentLoadState::AtPulley) {
+        if (mui::userInput.AnyEvent()) {
+            switch (mui::userInput.ConsumeEvent()) {
+            case mui::Event::Left:
+                // move selector left if possible
+                if (mg::globals.ActiveSlot() > 0) {
+                    moveSelector.Reset(mg::globals.ActiveSlot() - 1);
+                    currentCommand = &moveSelector;
+                }
+                break;
+            case mui::Event::Middle:
+                // plan load
+                if (mg::globals.ActiveSlot() < config::toolCount) { // do we have a meaningful selector position?
+                    loadFilament.ResetUnlimited(mg::globals.ActiveSlot());
+                    currentCommand = &loadFilament;
+                }
+                break;
+            case mui::Event::Right:
+                // move selector right if possible (including the park position)
+                if (mg::globals.ActiveSlot() < config::toolCount) {
+                    moveSelector.Reset(mg::globals.ActiveSlot() + 1); // we allow also the park position
+                    currentCommand = &moveSelector;
+                }
+                break;
+            default:
+                break;
+            }
+        }
+    }
+}
+
+mp::ResponseCommandStatus IdleMode::RunningCommandStatus() const {
+    switch (currentCommand->Error()) {
+    case ErrorCode::RUNNING:
+        return mp::ResponseCommandStatus(mp::ResponseMsgParamCodes::Processing, (uint16_t)currentCommand->State());
+    case ErrorCode::OK:
+        return mp::ResponseCommandStatus(mp::ResponseMsgParamCodes::Finished, 0);
+    default:
+        return mp::ResponseCommandStatus(mp::ResponseMsgParamCodes::Error, (uint16_t)currentCommand->Error());
+    }
+}
+
+static constexpr const uint8_t maxMsgLen = 10;
+
+void IdleMode::ReportCommandAccepted(const mp::RequestMsg &rq, mp::ResponseMsgParamCodes status) {
+    uint8_t tmp[maxMsgLen];
+    uint8_t len = protocol.EncodeResponseCmdAR(rq, status, tmp);
+    modules::serial::WriteToUSART(tmp, len);
+}
+
+void IdleMode::PlanCommand(const modules::protocol::RequestMsg &rq) {
+    if (currentCommand->State() == ProgressCode::OK) {
+        // We are allowed to start a new command as the previous one is in the OK finished state
+        // The previous command may be in an error state, but as long as it is in ProgressCode::OK (aka finished)
+        // we are safe here. It is the responsibility of the printer to ask for a command error code
+        // before issuing another one - if needed.
+        switch (rq.code) {
+        case mp::RequestMsgCodes::Cut:
+            currentCommand = &cutFilament;
+            break;
+        case mp::RequestMsgCodes::Eject:
+            currentCommand = &ejectFilament;
+            break;
+        case mp::RequestMsgCodes::Home:
+            currentCommand = &logic::home;
+            break;
+        case mp::RequestMsgCodes::Load:
+            currentCommand = &loadFilament;
+            break;
+        case mp::RequestMsgCodes::Tool:
+            currentCommand = &toolChange;
+            break;
+        case mp::RequestMsgCodes::Unload:
+            currentCommand = &unloadFilament;
+            break;
+        case mp::RequestMsgCodes::Mode:
+            currentCommand = &setMode;
+            break;
+        default:
+            currentCommand = &noCommand;
+            break;
+        }
+        currentCommandRq = rq; // save the Current Command Request for indentification of responses
+        currentCommand->Reset(rq.value);
+        ReportCommandAccepted(rq, mp::ResponseMsgParamCodes::Accepted);
+    } else {
+        ReportCommandAccepted(rq, mp::ResponseMsgParamCodes::Rejected);
+    }
+}
+
+void IdleMode::ReportFINDA(const mp::RequestMsg &rq) {
+#ifdef DEBUG_FINDA
+    using namespace hal;
+    hu::usart1.puts("FINDA:");
+    if (hal::gpio::ReadPin(FINDA_PIN) == hal::gpio::Level::high) {
+        hu::usart1.puts(" TIRGGERED\n");
+    } else {
+        hu::usart1.puts(" NOT TRIGGERED\n");
+    }
+#endif //DEBUG_FINDA
+    uint8_t rsp[maxMsgLen];
+    uint8_t len = protocol.EncodeResponseReadFINDA(rq, mf::finda.Pressed(), rsp);
+    modules::serial::WriteToUSART(rsp, len);
+}
+
+void IdleMode::ReportVersion(const mp::RequestMsg &rq) {
+    uint8_t v = 0;
+
+    switch (rq.value) {
+    case 0:
+        v = project_version_major;
+        break;
+    case 1:
+        v = project_version_minor;
+        break;
+    case 2:
+        v = project_version_revision;
+        break;
+    case 3:
+        // @@TODO may be allow reporting uint16_t number of errors,
+        // but anything beyond 255 errors means there is something seriously wrong with the MMU
+        v = mg::globals.DriveErrors();
+        break;
+    default:
+        v = 0;
+        break;
+    }
+
+    uint8_t rsp[10];
+    uint8_t len = protocol.EncodeResponseVersion(rq, v, rsp);
+    modules::serial::WriteToUSART(rsp, len);
+}
+
+void IdleMode::ReportRunningCommand() {
+    uint8_t rsp[maxMsgLen];
+    uint8_t len = protocol.EncodeResponseQueryOperation(currentCommandRq, logic::idleMode.RunningCommandStatus(), rsp);
+    modules::serial::WriteToUSART(rsp, len);
+}
+
+void IdleMode::ProcessRequestMsg(const mp::RequestMsg &rq) {
+    switch (rq.code) {
+    case mp::RequestMsgCodes::Button:
+        // behave just like if the user pressed a button
+        mui::userInput.ProcessMessage(rq.value);
+        break;
+    case mp::RequestMsgCodes::Finda:
+        // immediately report FINDA status
+        ReportFINDA(rq);
+        break;
+    case mp::RequestMsgCodes::Query:
+        // immediately report progress of currently running command
+        ReportRunningCommand();
+        break;
+    case mp::RequestMsgCodes::Reset:
+        // immediately reset the board - there is no response in this case
+        hal::cpu::Reset();
+        break;
+    case mp::RequestMsgCodes::Version:
+        ReportVersion(rq);
+        break;
+    case mp::RequestMsgCodes::Wait:
+        break; // @@TODO - not used anywhere yet
+    case mp::RequestMsgCodes::Cut:
+    case mp::RequestMsgCodes::Eject:
+    case mp::RequestMsgCodes::Home:
+    case mp::RequestMsgCodes::Load:
+    case mp::RequestMsgCodes::Tool:
+    case mp::RequestMsgCodes::Unload:
+        PlanCommand(rq);
+        break;
+    case mp::RequestMsgCodes::FilamentSensor: // set filament sensor state in the printer
+        mfs::fsensor.ProcessMessage(rq.value != 0);
+        ReportCommandAccepted(rq, mp::ResponseMsgParamCodes::Accepted);
+        break;
+    default:
+        // respond with an error message
+        break;
+    }
+}
+
+bool IdleMode::CheckMsgs() {
+    using mpd = mp::DecodeStatus;
+    while (modules::serial::Available()) {
+        switch (protocol.DecodeRequest(modules::serial::ConsumeByte())) {
+        case mpd::MessageCompleted:
+            // process the input message
+            return true;
+            break;
+        case mpd::NeedMoreData:
+            // just continue reading
+            break;
+        case mpd::Error:
+            // @@TODO what shall we do? Start some watchdog? We cannot send anything spontaneously
+            break;
+        }
+    }
+    return false;
+}
+
+void IdleMode::Panic(ErrorCode ec) {
+    currentCommand->Panic(ec);
+}
+
+void IdleMode::Step() {
+    CheckManualOperation();
+
+    if (CheckMsgs()) {
+        ProcessRequestMsg(protocol.GetRequestMsg());
+    }
+
+    currentCommand->Step();
+}
+
+} // namespace logic
diff --git a/src/logic/idle_mode.h b/src/logic/idle_mode.h
new file mode 100644
index 0000000..2b54b74
--- /dev/null
+++ b/src/logic/idle_mode.h
@@ -0,0 +1,60 @@
+/// @file
+#pragma once
+#include <stdint.h>
+#include "../modules/timebase.h"
+#include "../modules/protocol.h"
+#include "error_codes.h"
+
+namespace logic {
+
+class CommandBase;
+
+class IdleMode {
+public:
+    IdleMode();
+
+    inline void CommandFinishedCorrectly() {
+        lastCommandProcessedMs = mt::timebase.Millis();
+    }
+
+    /// Perform firmware panic handling
+    void Panic(ErrorCode ec);
+
+    /// Perform one step of top level
+    void Step();
+
+private:
+    /// Checks if the MMU can enter manual mode (user can move the selector with buttons)
+    /// The MMU enters idle mode after 5s from the last command finished and there must be no filament present in the selector.
+    void CheckManualOperation();
+
+    /// Checks messages on the UART
+    bool CheckMsgs();
+
+    /// Tries to plan a logic::command if possible
+    void PlanCommand(const mp::RequestMsg &rq);
+
+    mp::ResponseCommandStatus RunningCommandStatus() const;
+    void ReportCommandAccepted(const mp::RequestMsg &rq, mp::ResponseMsgParamCodes status);
+    void ReportFINDA(const mp::RequestMsg &rq);
+    void ReportVersion(const mp::RequestMsg &rq);
+    void ReportRunningCommand();
+    void ProcessRequestMsg(const mp::RequestMsg &rq);
+
+    uint16_t lastCommandProcessedMs;
+
+    /// A command that resulted in the currently on-going operation
+    CommandBase *currentCommand;
+
+    /// Remember the request message that started the currently running command
+    /// For the start we report "Reset finished" which in fact corresponds with the MMU state pretty closely
+    /// and plays nicely even with the protocol implementation.
+    /// And, since the default startup command is the noCommand, which always returns "Finished"
+    /// the implementation is clean and straightforward - the response to the first Q0 messages
+    /// will look like "X0 F" until a command (T, L, U ...) has been issued.
+    mp::RequestMsg currentCommandRq;
+};
+
+extern IdleMode idleMode;
+
+} // namespace logic
diff --git a/src/logic/load_filament.cpp b/src/logic/load_filament.cpp
index 7d7abd6..10d2f4e 100644
--- a/src/logic/load_filament.cpp
+++ b/src/logic/load_filament.cpp
@@ -47,9 +47,8 @@ void logic::LoadFilament::GoToRetractingFromFinda() {
     retract.Reset();
 }
 
-void logic::LoadFilament::FinishedCorrectly() {
-    state = ProgressCode::OK;
-    error = ErrorCode::OK;
+void logic::LoadFilament::LoadFinishedCorrectly() {
+    FinishedOK();
     ml::leds.SetPairButOffOthers(mg::globals.ActiveSlot(), ml::off, ml::off);
     mpu::pulley.Disable();
 }
@@ -69,7 +68,7 @@ bool LoadFilament::StepInner() {
             case FeedToFinda::Stopped:
                 // the user stopped the load for whatever reason
                 // - we are considering the LoadFlament operation as completed
-                state = ProgressCode::OK;
+                FinishedOK();
                 break;
             }
         }
@@ -89,7 +88,7 @@ bool LoadFilament::StepInner() {
         // and if the selector decided to re-home, we have to wait for it as well
         // therefore: 'if (!mi::idler.Engaged())' : alone is not enough
         if (!mi::idler.Engaged() && ms::selector.Slot() == mg::globals.ActiveSlot()) {
-            FinishedCorrectly();
+            LoadFinishedCorrectly();
         }
         break;
     case ProgressCode::OK:
diff --git a/src/logic/load_filament.h b/src/logic/load_filament.h
index 0357c93..865a22f 100644
--- a/src/logic/load_filament.h
+++ b/src/logic/load_filament.h
@@ -29,7 +29,7 @@ private:
     void Reset2(bool feedPhaseLimited);
 
     /// Common code for a correct completion of UnloadFilament
-    void FinishedCorrectly();
+    void LoadFinishedCorrectly();
 
     FeedToFinda feed;
     RetractFromFinda retract;
diff --git a/src/logic/move_selector.cpp b/src/logic/move_selector.cpp
index bd86a26..6becc4b 100644
--- a/src/logic/move_selector.cpp
+++ b/src/logic/move_selector.cpp
@@ -23,8 +23,8 @@ bool MoveSelector::StepInner() {
     switch (state) {
     case ProgressCode::MovingSelector:
         if (ms::selector.State() == ms::selector.Ready) {
-            state = ProgressCode::OK;
-            error = ErrorCode::OK;
+            mg::globals.SetFilamentLoaded(ms::selector.Slot(), mg::FilamentLoadState::AtPulley);
+            FinishedOK();
         }
         break;
     case ProgressCode::OK:
diff --git a/src/logic/set_mode.cpp b/src/logic/set_mode.cpp
index e964958..60e7d70 100644
--- a/src/logic/set_mode.cpp
+++ b/src/logic/set_mode.cpp
@@ -11,8 +11,7 @@ void SetMode::Reset(uint8_t param) {
     mg::globals.SetMotorsMode(param != 0); // remember the last mode set
     // distribute the mode to all motors immediately
     mm::motion.SetMode((param == 0) ? mm::Normal : mm::Stealth);
-    state = ProgressCode::OK;
-    error = ErrorCode::OK;
+    FinishedOK();
 }
 
 } // namespace logic
diff --git a/src/logic/tool_change.cpp b/src/logic/tool_change.cpp
index 769cd56..62438bf 100644
--- a/src/logic/tool_change.cpp
+++ b/src/logic/tool_change.cpp
@@ -52,10 +52,9 @@ void logic::ToolChange::GoToFeedingToBondtech() {
     error = ErrorCode::RUNNING;
 }
 
-void logic::ToolChange::FinishedCorrectly() {
+void logic::ToolChange::ToolChangeFinishedCorrectly() {
     ml::leds.SetPairButOffOthers(mg::globals.ActiveSlot(), ml::on, ml::off);
-    state = ProgressCode::OK;
-    error = ErrorCode::OK;
+    FinishedOK();
 }
 
 bool ToolChange::StepInner() {
@@ -85,7 +84,7 @@ bool ToolChange::StepInner() {
             if (james.State() == FeedToBondtech::Failed) {
                 GoToErrDisengagingIdler(ErrorCode::FSENSOR_DIDNT_SWITCH_ON); // signal loading error
             } else {
-                FinishedCorrectly();
+                ToolChangeFinishedCorrectly();
             }
         }
         break;
@@ -122,7 +121,7 @@ bool ToolChange::StepInner() {
             } else {
                 // all sensors are ok, we assume the user pushed the filament into the nozzle
                 mg::globals.SetFilamentLoaded(plannedSlot, mg::FilamentLoadState::InNozzle);
-                FinishedCorrectly();
+                ToolChangeFinishedCorrectly();
             }
             break;
         default: // no event, continue waiting for user input
diff --git a/src/logic/tool_change.h b/src/logic/tool_change.h
index 2c74b41..2d84071 100644
--- a/src/logic/tool_change.h
+++ b/src/logic/tool_change.h
@@ -31,7 +31,7 @@ private:
     void GoToFeedingToBondtech();
 
     /// Common code for a correct completion of UnloadFilament
-    void FinishedCorrectly();
+    void ToolChangeFinishedCorrectly();
 
     UnloadFilament unl; ///< a high-level command/operation may be used as a building block of other operations as well
     FeedToFinda feed;
diff --git a/src/logic/unload_filament.cpp b/src/logic/unload_filament.cpp
index 6227bc6..2509231 100644
--- a/src/logic/unload_filament.cpp
+++ b/src/logic/unload_filament.cpp
@@ -32,9 +32,8 @@ void UnloadFilament::Reset(uint8_t /*param*/) {
     ml::leds.SetPairButOffOthers(mg::globals.ActiveSlot(), ml::off, ml::off);
 }
 
-void UnloadFilament::FinishedCorrectly() {
-    state = ProgressCode::OK;
-    error = ErrorCode::OK;
+void UnloadFilament::UnloadFinishedCorrectly() {
+    FinishedOK();
     mpu::pulley.Disable();
     mg::globals.SetFilamentLoaded(mg::globals.ActiveSlot(), mg::FilamentLoadState::AtPulley); // filament unloaded
     ml::leds.SetPairButOffOthers(mg::globals.ActiveSlot(), ml::off, ml::off);
@@ -80,7 +79,7 @@ bool UnloadFilament::StepInner() {
         return false;
     case ProgressCode::DisengagingIdler:
         if (!mi::idler.Engaged() && ms::selector.State() == ms::Selector::Ready) {
-            FinishedCorrectly();
+            UnloadFinishedCorrectly();
         }
         return false;
     case ProgressCode::ERRDisengagingIdler: // couldn't unload to FINDA
diff --git a/src/logic/unload_filament.h b/src/logic/unload_filament.h
index 2787d7b..23ba850 100644
--- a/src/logic/unload_filament.h
+++ b/src/logic/unload_filament.h
@@ -25,7 +25,7 @@ private:
     constexpr static const uint8_t maxRetries = 1;
 
     /// Common code for a correct completion of UnloadFilament
-    void FinishedCorrectly();
+    void UnloadFinishedCorrectly();
     void GoToRetractingFromFinda();
     void GoToRecheckFilamentAgainstFINDA();
 
diff --git a/src/main.cpp b/src/main.cpp
index a692246..d9a2a4b 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -25,35 +25,7 @@
 #include "modules/motion.h"
 #include "modules/usb_cdc.h"
 
-#include "logic/command_base.h"
-#include "logic/cut_filament.h"
-#include "logic/eject_filament.h"
-#include "logic/home.h"
-#include "logic/load_filament.h"
-#include "logic/move_selector.h"
-#include "logic/no_command.h"
-#include "logic/set_mode.h"
-#include "logic/tool_change.h"
-#include "logic/unload_filament.h"
-
-#include "version.h"
-#include "panic.h"
-
-/// Global instance of the protocol codec
-static mp::Protocol protocol;
-
-/// A command that resulted in the currently on-going operation
-static logic::CommandBase *currentCommand = &logic::noCommand;
-
-/// Remember the request message that started the currently running command
-/// For the start we report "Reset finished" which in fact corresponds with the MMU state pretty closely
-/// and plays nicely even with the protocol implementation.
-/// And, since the default startup command is the noCommand, which always returns "Finished"
-/// the implementation is clean and straightforward - the response to the first Q0 messages
-/// will look like "X0 F" until a command (T, L, U ...) has been issued.
-static mp::RequestMsg currentCommandRq(mp::RequestMsgCodes::Reset, 0);
-
-static uint16_t lastCommandProcessedMs = 0;
+#include "logic/idle_mode.h"
 
 /// One-time setup of HW and SW components
 /// Called before entering the loop() function
@@ -128,237 +100,8 @@ void setup() {
     }
 }
 
-static constexpr const uint8_t maxMsgLen = 10;
-
-bool WriteToUSART(const uint8_t *src, uint8_t len) {
-    // How to properly enqueue the message? Especially in case of a full buffer.
-    // We neither can stay here in an endless loop until the buffer drains.
-    // Nor can we save the message elsewhere ... it must be just skipped and the protocol must handle it.
-    // Under normal circumstances, such a situation should not happen.
-    // The MMU cannot produce response messages on its own - it only responds to requests from the printer.
-    // That means there is only one message in the output buffer at once as long as the printer waits for the response before sending another request.
-    for (uint8_t i = 0; i < len; ++i) {
-        if (hu::usart1.CanWrite()) {
-            // should not block waiting for the TX buffer to drain as there was an empty spot for at least 1 byte
-            hu::usart1.Write(src[i]);
-        } else {
-            //buffer full - must skip the rest of the message - the communication will drop out anyway
-            return false;
-        }
-    }
-    return true; // not sure if we can actually leverage the knowledge of success while sending the message
-}
-
-void ReportCommandAccepted(const mp::RequestMsg &rq, mp::ResponseMsgParamCodes status) {
-    uint8_t tmp[maxMsgLen];
-    uint8_t len = protocol.EncodeResponseCmdAR(rq, status, tmp);
-    WriteToUSART(tmp, len);
-}
-
-void ReportFINDA(const mp::RequestMsg &rq) {
-#ifdef DEBUG_FINDA
-    using namespace hal;
-    hu::usart1.puts("FINDA:");
-    if (hal::gpio::ReadPin(FINDA_PIN) == hal::gpio::Level::high) {
-        hu::usart1.puts(" TIRGGERED\n");
-    } else {
-        hu::usart1.puts(" NOT TRIGGERED\n");
-    }
-#endif //DEBUG_FINDA
-    uint8_t rsp[maxMsgLen];
-    uint8_t len = protocol.EncodeResponseReadFINDA(rq, mf::finda.Pressed(), rsp);
-    WriteToUSART(rsp, len);
-}
-
-void ReportVersion(const mp::RequestMsg &rq) {
-    uint8_t v = 0;
-
-    switch (rq.value) {
-    case 0:
-        v = project_version_major;
-        break;
-    case 1:
-        v = project_version_minor;
-        break;
-    case 2:
-        v = project_version_revision;
-        break;
-    case 3:
-        // @@TODO may be allow reporting uint16_t number of errors,
-        // but anything beyond 255 errors means there is something seriously wrong with the MMU
-        v = mg::globals.DriveErrors();
-        break;
-    default:
-        v = 0;
-        break;
-    }
-
-    uint8_t rsp[10];
-    uint8_t len = protocol.EncodeResponseVersion(rq, v, rsp);
-    WriteToUSART(rsp, len);
-}
-
-void ReportRunningCommand() {
-    mp::ResponseMsgParamCodes commandStatus;
-    uint16_t value = 0;
-    switch (currentCommand->Error()) {
-    case ErrorCode::RUNNING:
-        commandStatus = mp::ResponseMsgParamCodes::Processing;
-        value = (uint16_t)currentCommand->State();
-        break;
-    case ErrorCode::OK:
-        commandStatus = mp::ResponseMsgParamCodes::Finished;
-        lastCommandProcessedMs = mt::timebase.Millis();
-        break;
-    default:
-        commandStatus = mp::ResponseMsgParamCodes::Error;
-        value = (uint16_t)currentCommand->Error();
-        break;
-    }
-
-    uint8_t rsp[maxMsgLen];
-    uint8_t len = protocol.EncodeResponseQueryOperation(currentCommandRq, commandStatus, value, rsp);
-    WriteToUSART(rsp, len);
-}
-
-void PlanCommand(const mp::RequestMsg &rq) {
-    if (currentCommand->State() == ProgressCode::OK) {
-        // We are allowed to start a new command as the previous one is in the OK finished state
-        // The previous command may be in an error state, but as long as it is in ProgressCode::OK (aka finished)
-        // we are safe here. It is the responsibility of the printer to ask for a command error code
-        // before issuing another one - if needed.
-        switch (rq.code) {
-        case mp::RequestMsgCodes::Cut:
-            currentCommand = &logic::cutFilament;
-            break;
-        case mp::RequestMsgCodes::Eject:
-            currentCommand = &logic::ejectFilament;
-            break;
-        case mp::RequestMsgCodes::Home:
-            currentCommand = &logic::home;
-            break;
-        case mp::RequestMsgCodes::Load:
-            currentCommand = &logic::loadFilament;
-            break;
-        case mp::RequestMsgCodes::Tool:
-            currentCommand = &logic::toolChange;
-            break;
-        case mp::RequestMsgCodes::Unload:
-            currentCommand = &logic::unloadFilament;
-            break;
-        case mp::RequestMsgCodes::Mode:
-            currentCommand = &logic::setMode;
-            break;
-        default:
-            currentCommand = &logic::noCommand;
-            break;
-        }
-        currentCommandRq = rq; // save the Current Command Request for indentification of responses
-        currentCommand->Reset(rq.value);
-        ReportCommandAccepted(rq, mp::ResponseMsgParamCodes::Accepted);
-    } else {
-        ReportCommandAccepted(rq, mp::ResponseMsgParamCodes::Rejected);
-    }
-}
-
-void ProcessRequestMsg(const mp::RequestMsg &rq) {
-    switch (rq.code) {
-    case mp::RequestMsgCodes::Button:
-        // behave just like if the user pressed a button
-        mui::userInput.ProcessMessage(rq.value);
-        break;
-    case mp::RequestMsgCodes::Finda:
-        // immediately report FINDA status
-        ReportFINDA(rq);
-        break;
-    case mp::RequestMsgCodes::Query:
-        // immediately report progress of currently running command
-        ReportRunningCommand();
-        break;
-    case mp::RequestMsgCodes::Reset:
-        // immediately reset the board - there is no response in this case
-        hal::cpu::Reset();
-        break;
-    case mp::RequestMsgCodes::Version:
-        ReportVersion(rq);
-        break;
-    case mp::RequestMsgCodes::Wait:
-        break; // @@TODO - not used anywhere yet
-    case mp::RequestMsgCodes::Cut:
-    case mp::RequestMsgCodes::Eject:
-    case mp::RequestMsgCodes::Home:
-    case mp::RequestMsgCodes::Load:
-    case mp::RequestMsgCodes::Tool:
-    case mp::RequestMsgCodes::Unload:
-        PlanCommand(rq);
-        break;
-    case mp::RequestMsgCodes::FilamentSensor: // set filament sensor state in the printer
-        mfs::fsensor.ProcessMessage(rq.value != 0);
-        ReportCommandAccepted(rq, mp::ResponseMsgParamCodes::Accepted);
-        break;
-    default:
-        // respond with an error message
-        break;
-    }
-}
-
-/// @returns true if a request was successfully finished
-bool CheckMsgs() {
-    using mpd = mp::DecodeStatus;
-    while (!hu::usart1.ReadEmpty()) {
-        switch (protocol.DecodeRequest(hu::usart1.Read())) {
-        case mpd::MessageCompleted:
-            // process the input message
-            return true;
-            break;
-        case mpd::NeedMoreData:
-            // just continue reading
-            break;
-        case mpd::Error:
-            // @@TODO what shall we do? Start some watchdog? We cannot send anything spontaneously
-            break;
-        }
-    }
-    return false;
-}
-
 void Panic(ErrorCode ec) {
-    currentCommand->Panic(ec);
-}
-
-bool CheckManualOperation() {
-    if (currentCommand == &logic::noCommand
-        && mg::globals.FilamentLoaded() <= mg::FilamentLoadState::AtPulley
-        && lastCommandProcessedMs + 5000 < mt::timebase.Millis()) {
-        lastCommandProcessedMs = mt::timebase.Millis() - 5000; // @@TODO prevent future overflows - must be called every time
-        if (mui::userInput.AnyEvent()) {
-            switch (mui::userInput.ConsumeEvent()) {
-            case mui::Event::Left:
-                // move selector left if possible
-                if (mg::globals.ActiveSlot() > 0) {
-                    currentCommand = &logic::moveSelector;
-                    currentCommand->Reset(mg::globals.ActiveSlot() - 1);
-                }
-                break;
-            case mui::Event::Middle:
-                // plan load
-                if (mg::globals.ActiveSlot() < config::toolCount) { // do we have a meaningful selector position?
-                    logic::loadFilament.ResetUnlimited(mg::globals.ActiveSlot());
-                    currentCommand = &logic::loadFilament;
-                }
-                break;
-            case mui::Event::Right:
-                // move selector right if possible (including the park position)
-                if (mg::globals.ActiveSlot() < config::toolCount) {
-                    currentCommand = &logic::moveSelector;
-                    currentCommand->Reset(mg::globals.ActiveSlot() + 1); // we allow also the park position
-                }
-                break;
-            default:
-                break;
-            }
-        }
-    }
+    logic::idleMode.Panic(ec);
 }
 
 /// Main loop of the firmware
@@ -376,12 +119,6 @@ bool CheckManualOperation() {
 /// The idea behind the Step* routines is to keep each automaton non-blocking allowing for some “concurrency”.
 /// Some FW components will leverage ISR to do their stuff (UART, motor stepping?, etc.)
 void loop() {
-    CheckManualOperation();
-
-    if (CheckMsgs()) {
-        ProcessRequestMsg(protocol.GetRequestMsg());
-    }
-
     mb::buttons.Step();
     ml::leds.Step();
     mf::finda.Step();
@@ -390,10 +127,11 @@ void loop() {
     ms::selector.Step();
     mpu::pulley.Step();
     mui::userInput.Step();
-    currentCommand->Step();
     hal::cpu::Step();
     mu::cdc.Step();
 
+    logic::idleMode.Step();
+
     hal::watchdog::Reset();
 }
 
diff --git a/src/modules/CMakeLists.txt b/src/modules/CMakeLists.txt
index eefa559..0a89bcb 100644
--- a/src/modules/CMakeLists.txt
+++ b/src/modules/CMakeLists.txt
@@ -14,6 +14,7 @@ target_sources(
           pulley.cpp
           pulse_gen.cpp
           selector.cpp
+          serial.cpp
           speed_table.cpp
           timebase.cpp
           usb_cdc.cpp
diff --git a/src/modules/protocol.cpp b/src/modules/protocol.cpp
index 537bf07..7d723cd 100644
--- a/src/modules/protocol.cpp
+++ b/src/modules/protocol.cpp
@@ -210,33 +210,33 @@ uint8_t Protocol::EncodeResponseVersion(const RequestMsg &msg, uint8_t value, ui
     return dst - txbuff + 1;
 }
 
-uint8_t Protocol::EncodeResponseQueryOperation(const RequestMsg &msg, ResponseMsgParamCodes code, uint16_t value, uint8_t *txbuff) {
+uint8_t Protocol::EncodeResponseQueryOperation(const RequestMsg &msg, ResponseCommandStatus rcs, uint8_t *txbuff) {
     txbuff[0] = (uint8_t)msg.code;
     txbuff[1] = msg.value + '0';
     txbuff[2] = ' ';
-    txbuff[3] = (uint8_t)code;
+    txbuff[3] = (uint8_t)rcs.code;
     uint8_t *dst = txbuff + 4;
-    if (code != ResponseMsgParamCodes::Finished) {
-        if (value < 10) {
-            *dst++ = value + '0';
-        } else if (value < 100) {
-            *dst++ = value / 10 + '0';
-            *dst++ = value % 10 + '0';
-        } else if (value < 1000) {
-            *dst++ = value / 100 + '0';
-            *dst++ = (value / 10) % 10 + '0';
-            *dst++ = value % 10 + '0';
-        } else if (value < 10000) {
-            *dst++ = value / 1000 + '0';
-            *dst++ = (value / 100) % 10 + '0';
-            *dst++ = (value / 10) % 10 + '0';
-            *dst++ = value % 10 + '0';
+    if (rcs.code != ResponseMsgParamCodes::Finished) {
+        if (rcs.value < 10) {
+            *dst++ = rcs.value + '0';
+        } else if (rcs.value < 100) {
+            *dst++ = rcs.value / 10 + '0';
+            *dst++ = rcs.value % 10 + '0';
+        } else if (rcs.value < 1000) {
+            *dst++ = rcs.value / 100 + '0';
+            *dst++ = (rcs.value / 10) % 10 + '0';
+            *dst++ = rcs.value % 10 + '0';
+        } else if (rcs.value < 10000) {
+            *dst++ = rcs.value / 1000 + '0';
+            *dst++ = (rcs.value / 100) % 10 + '0';
+            *dst++ = (rcs.value / 10) % 10 + '0';
+            *dst++ = rcs.value % 10 + '0';
         } else {
-            *dst++ = value / 10000 + '0';
-            *dst++ = (value / 1000) % 10 + '0';
-            *dst++ = (value / 100) % 10 + '0';
-            *dst++ = (value / 10) % 10 + '0';
-            *dst++ = value % 10 + '0';
+            *dst++ = rcs.value / 10000 + '0';
+            *dst++ = (rcs.value / 1000) % 10 + '0';
+            *dst++ = (rcs.value / 100) % 10 + '0';
+            *dst++ = (rcs.value / 10) % 10 + '0';
+            *dst++ = rcs.value % 10 + '0';
         }
     }
     *dst = '\n';
diff --git a/src/modules/protocol.h b/src/modules/protocol.h
index 68fde9a..ed4143a 100644
--- a/src/modules/protocol.h
+++ b/src/modules/protocol.h
@@ -47,7 +47,7 @@ struct RequestMsg {
 
     /// @param code of the request message
     /// @param value of the request message
-    inline RequestMsg(RequestMsgCodes code, uint8_t value)
+    inline constexpr RequestMsg(RequestMsgCodes code, uint8_t value)
         : code(code)
         , value(value) {}
 };
@@ -61,12 +61,21 @@ struct ResponseMsg {
     /// @param request the source request message this response is a reply to
     /// @param paramCode code of the parameter
     /// @param paramValue value of the parameter
-    inline ResponseMsg(RequestMsg request, ResponseMsgParamCodes paramCode, uint16_t paramValue)
+    inline constexpr ResponseMsg(RequestMsg request, ResponseMsgParamCodes paramCode, uint16_t paramValue)
         : request(request)
         , paramCode(paramCode)
         , paramValue(paramValue) {}
 };
 
+/// Combined commandStatus and its value into one data structure (optimization purposes)
+struct ResponseCommandStatus {
+    ResponseMsgParamCodes code;
+    uint16_t value;
+    inline constexpr ResponseCommandStatus(ResponseMsgParamCodes code, uint16_t value)
+        : code(code)
+        , value(value) {}
+};
+
 /// Message decoding return values
 enum class DecodeStatus : uint_fast8_t {
     MessageCompleted, ///< message completed and successfully lexed
@@ -131,7 +140,7 @@ public:
     /// @param value related to status of operation(e.g. error code or progress)
     /// @param txbuff where to format the message
     /// @returns number of bytes written into txbuff
-    static uint8_t EncodeResponseQueryOperation(const RequestMsg &msg, ResponseMsgParamCodes code, uint16_t value, uint8_t *txbuff);
+    static uint8_t EncodeResponseQueryOperation(const RequestMsg &msg, ResponseCommandStatus rcs, uint8_t *txbuff);
 
     /// @returns the most recently lexed request message
     inline const RequestMsg GetRequestMsg() const { return requestMsg; }
diff --git a/src/modules/serial.cpp b/src/modules/serial.cpp
new file mode 100644
index 0000000..4b3b26e
--- /dev/null
+++ b/src/modules/serial.cpp
@@ -0,0 +1,37 @@
+#include "serial.h"
+#include "../hal/usart.h"
+
+namespace modules {
+
+namespace serial {
+
+bool WriteToUSART(const uint8_t *src, uint8_t len) {
+    // How to properly enqueue the message? Especially in case of a full buffer.
+    // We neither can stay here in an endless loop until the buffer drains.
+    // Nor can we save the message elsewhere ... it must be just skipped and the protocol must handle it.
+    // Under normal circumstances, such a situation should not happen.
+    // The MMU cannot produce response messages on its own - it only responds to requests from the printer.
+    // That means there is only one message in the output buffer at once as long as the printer waits for the response before sending another request.
+    for (uint8_t i = 0; i < len; ++i) {
+        if (hu::usart1.CanWrite()) {
+            // should not block waiting for the TX buffer to drain as there was an empty spot for at least 1 byte
+            hu::usart1.Write(src[i]);
+        } else {
+            //buffer full - must skip the rest of the message - the communication will drop out anyway
+            return false;
+        }
+    }
+    return true; // not sure if we can actually leverage the knowledge of success while sending the message
+}
+
+bool Available() {
+    return !hu::usart1.ReadEmpty();
+}
+
+uint8_t ConsumeByte() {
+    return hu::usart1.Read();
+}
+
+} // namespace serial
+
+} // namespace modules
diff --git a/src/modules/serial.h b/src/modules/serial.h
new file mode 100644
index 0000000..c50bd3a
--- /dev/null
+++ b/src/modules/serial.h
@@ -0,0 +1,17 @@
+/// @file
+#pragma once
+#include <stdint.h>
+
+namespace modules {
+
+namespace serial {
+
+bool WriteToUSART(const uint8_t *src, uint8_t len);
+
+bool Available();
+
+uint8_t ConsumeByte();
+
+} // namespace serial
+
+} // namespace modules
diff --git a/tests/unit/logic/cut_filament/CMakeLists.txt b/tests/unit/logic/cut_filament/CMakeLists.txt
index 1bb5360..062509f 100644
--- a/tests/unit/logic/cut_filament/CMakeLists.txt
+++ b/tests/unit/logic/cut_filament/CMakeLists.txt
@@ -3,8 +3,17 @@ add_executable(
   cut_filament_tests
   ${CMAKE_SOURCE_DIR}/src/logic/command_base.cpp
   ${CMAKE_SOURCE_DIR}/src/logic/cut_filament.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/eject_filament.cpp
   ${CMAKE_SOURCE_DIR}/src/logic/feed_to_finda.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/feed_to_bondtech.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/home.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/idle_mode.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/load_filament.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/move_selector.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/no_command.cpp
   ${CMAKE_SOURCE_DIR}/src/logic/retract_from_finda.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/set_mode.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/tool_change.cpp
   ${CMAKE_SOURCE_DIR}/src/logic/unload_filament.cpp
   ${CMAKE_SOURCE_DIR}/src/logic/unload_to_finda.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/buttons.cpp
@@ -16,14 +25,17 @@ add_executable(
   ${CMAKE_SOURCE_DIR}/src/modules/leds.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/movable_base.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/permanent_storage.cpp
+  ${CMAKE_SOURCE_DIR}/src/modules/protocol.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/pulley.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/selector.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/user_input.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/pulse_gen.cpp
   ${MODULES_STUBS_DIR}/stub_adc.cpp
+  ${MODULES_STUBS_DIR}/stub_cpu.cpp
   ${MODULES_STUBS_DIR}/stub_eeprom.cpp
   ${MODULES_STUBS_DIR}/stub_gpio.cpp
   ${MODULES_STUBS_DIR}/stub_shr16.cpp
+  ${MODULES_STUBS_DIR}/stub_serial.cpp
   ${MODULES_STUBS_DIR}/stub_timebase.cpp
   ${MODULES_STUBS_DIR}/stub_tmc2130.cpp
   ${LOGIC_STUBS_DIR}/homing.cpp
diff --git a/tests/unit/logic/eject_filament/CMakeLists.txt b/tests/unit/logic/eject_filament/CMakeLists.txt
index b60ef97..6d669a3 100644
--- a/tests/unit/logic/eject_filament/CMakeLists.txt
+++ b/tests/unit/logic/eject_filament/CMakeLists.txt
@@ -2,9 +2,18 @@
 add_executable(
   eject_filament_tests
   ${CMAKE_SOURCE_DIR}/src/logic/command_base.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/cut_filament.cpp
   ${CMAKE_SOURCE_DIR}/src/logic/eject_filament.cpp
   ${CMAKE_SOURCE_DIR}/src/logic/feed_to_finda.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/feed_to_bondtech.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/home.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/idle_mode.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/load_filament.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/move_selector.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/no_command.cpp
   ${CMAKE_SOURCE_DIR}/src/logic/retract_from_finda.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/set_mode.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/tool_change.cpp
   ${CMAKE_SOURCE_DIR}/src/logic/unload_filament.cpp
   ${CMAKE_SOURCE_DIR}/src/logic/unload_to_finda.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/buttons.cpp
@@ -16,14 +25,17 @@ add_executable(
   ${CMAKE_SOURCE_DIR}/src/modules/leds.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/movable_base.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/permanent_storage.cpp
+  ${CMAKE_SOURCE_DIR}/src/modules/protocol.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/pulley.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/selector.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/user_input.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/pulse_gen.cpp
   ${MODULES_STUBS_DIR}/stub_adc.cpp
+  ${MODULES_STUBS_DIR}/stub_cpu.cpp
   ${MODULES_STUBS_DIR}/stub_eeprom.cpp
   ${MODULES_STUBS_DIR}/stub_gpio.cpp
   ${MODULES_STUBS_DIR}/stub_shr16.cpp
+  ${MODULES_STUBS_DIR}/stub_serial.cpp
   ${MODULES_STUBS_DIR}/stub_timebase.cpp
   ${MODULES_STUBS_DIR}/stub_tmc2130.cpp
   ${LOGIC_STUBS_DIR}/homing.cpp
diff --git a/tests/unit/logic/failing_tmc/CMakeLists.txt b/tests/unit/logic/failing_tmc/CMakeLists.txt
index b8603dc..55064e6 100644
--- a/tests/unit/logic/failing_tmc/CMakeLists.txt
+++ b/tests/unit/logic/failing_tmc/CMakeLists.txt
@@ -2,9 +2,18 @@
 add_executable(
   failing_tmc_tests
   ${CMAKE_SOURCE_DIR}/src/logic/command_base.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/cut_filament.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/eject_filament.cpp
   ${CMAKE_SOURCE_DIR}/src/logic/feed_to_finda.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/feed_to_bondtech.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/home.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/idle_mode.cpp
   ${CMAKE_SOURCE_DIR}/src/logic/load_filament.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/move_selector.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/no_command.cpp
   ${CMAKE_SOURCE_DIR}/src/logic/retract_from_finda.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/set_mode.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/tool_change.cpp
   ${CMAKE_SOURCE_DIR}/src/logic/unload_filament.cpp
   ${CMAKE_SOURCE_DIR}/src/logic/unload_to_finda.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/buttons.cpp
@@ -16,14 +25,17 @@ add_executable(
   ${CMAKE_SOURCE_DIR}/src/modules/leds.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/movable_base.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/permanent_storage.cpp
+  ${CMAKE_SOURCE_DIR}/src/modules/protocol.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/pulley.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/selector.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/user_input.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/pulse_gen.cpp
   ${MODULES_STUBS_DIR}/stub_adc.cpp
+  ${MODULES_STUBS_DIR}/stub_cpu.cpp
   ${MODULES_STUBS_DIR}/stub_eeprom.cpp
   ${MODULES_STUBS_DIR}/stub_gpio.cpp
   ${MODULES_STUBS_DIR}/stub_shr16.cpp
+  ${MODULES_STUBS_DIR}/stub_serial.cpp
   ${MODULES_STUBS_DIR}/stub_timebase.cpp
   ${MODULES_STUBS_DIR}/stub_tmc2130.cpp
   ${LOGIC_STUBS_DIR}/homing.cpp
diff --git a/tests/unit/logic/feed_to_bondtech/CMakeLists.txt b/tests/unit/logic/feed_to_bondtech/CMakeLists.txt
index 3ef41b4..a79710e 100644
--- a/tests/unit/logic/feed_to_bondtech/CMakeLists.txt
+++ b/tests/unit/logic/feed_to_bondtech/CMakeLists.txt
@@ -2,7 +2,20 @@
 add_executable(
   feed_to_bondtech_tests
   ${CMAKE_SOURCE_DIR}/src/logic/command_base.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/cut_filament.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/eject_filament.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/feed_to_finda.cpp
   ${CMAKE_SOURCE_DIR}/src/logic/feed_to_bondtech.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/home.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/idle_mode.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/load_filament.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/move_selector.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/no_command.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/retract_from_finda.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/set_mode.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/tool_change.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/unload_filament.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/unload_to_finda.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/buttons.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/debouncer.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/finda.cpp
@@ -12,14 +25,17 @@ add_executable(
   ${CMAKE_SOURCE_DIR}/src/modules/leds.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/movable_base.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/permanent_storage.cpp
+  ${CMAKE_SOURCE_DIR}/src/modules/protocol.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/pulley.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/selector.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/user_input.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/pulse_gen.cpp
   ${MODULES_STUBS_DIR}/stub_adc.cpp
+  ${MODULES_STUBS_DIR}/stub_cpu.cpp
   ${MODULES_STUBS_DIR}/stub_eeprom.cpp
   ${MODULES_STUBS_DIR}/stub_gpio.cpp
   ${MODULES_STUBS_DIR}/stub_shr16.cpp
+  ${MODULES_STUBS_DIR}/stub_serial.cpp
   ${MODULES_STUBS_DIR}/stub_timebase.cpp
   ${MODULES_STUBS_DIR}/stub_tmc2130.cpp
   ${LOGIC_STUBS_DIR}/homing.cpp
diff --git a/tests/unit/logic/feed_to_finda/CMakeLists.txt b/tests/unit/logic/feed_to_finda/CMakeLists.txt
index c5d8211..1350d65 100644
--- a/tests/unit/logic/feed_to_finda/CMakeLists.txt
+++ b/tests/unit/logic/feed_to_finda/CMakeLists.txt
@@ -2,7 +2,20 @@
 add_executable(
   feed_to_finda_tests
   ${CMAKE_SOURCE_DIR}/src/logic/command_base.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/cut_filament.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/eject_filament.cpp
   ${CMAKE_SOURCE_DIR}/src/logic/feed_to_finda.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/feed_to_bondtech.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/home.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/idle_mode.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/load_filament.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/move_selector.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/no_command.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/retract_from_finda.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/set_mode.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/tool_change.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/unload_filament.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/unload_to_finda.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/buttons.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/debouncer.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/finda.cpp
@@ -12,14 +25,17 @@ add_executable(
   ${CMAKE_SOURCE_DIR}/src/modules/leds.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/movable_base.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/permanent_storage.cpp
+  ${CMAKE_SOURCE_DIR}/src/modules/protocol.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/pulley.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/selector.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/user_input.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/pulse_gen.cpp
   ${MODULES_STUBS_DIR}/stub_adc.cpp
+  ${MODULES_STUBS_DIR}/stub_cpu.cpp
   ${MODULES_STUBS_DIR}/stub_eeprom.cpp
   ${MODULES_STUBS_DIR}/stub_gpio.cpp
   ${MODULES_STUBS_DIR}/stub_shr16.cpp
+  ${MODULES_STUBS_DIR}/stub_serial.cpp
   ${MODULES_STUBS_DIR}/stub_timebase.cpp
   ${MODULES_STUBS_DIR}/stub_tmc2130.cpp
   ${LOGIC_STUBS_DIR}/homing.cpp
diff --git a/tests/unit/logic/homing/CMakeLists.txt b/tests/unit/logic/homing/CMakeLists.txt
index d41a763..2736e80 100644
--- a/tests/unit/logic/homing/CMakeLists.txt
+++ b/tests/unit/logic/homing/CMakeLists.txt
@@ -2,10 +2,18 @@
 add_executable(
   homing_tests
   ${CMAKE_SOURCE_DIR}/src/logic/command_base.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/cut_filament.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/eject_filament.cpp
   ${CMAKE_SOURCE_DIR}/src/logic/feed_to_finda.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/feed_to_bondtech.cpp
   ${CMAKE_SOURCE_DIR}/src/logic/home.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/idle_mode.cpp
   ${CMAKE_SOURCE_DIR}/src/logic/load_filament.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/move_selector.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/no_command.cpp
   ${CMAKE_SOURCE_DIR}/src/logic/retract_from_finda.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/set_mode.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/tool_change.cpp
   ${CMAKE_SOURCE_DIR}/src/logic/unload_filament.cpp
   ${CMAKE_SOURCE_DIR}/src/logic/unload_to_finda.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/buttons.cpp
@@ -17,14 +25,17 @@ add_executable(
   ${CMAKE_SOURCE_DIR}/src/modules/leds.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/movable_base.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/permanent_storage.cpp
+  ${CMAKE_SOURCE_DIR}/src/modules/protocol.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/pulley.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/selector.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/user_input.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/pulse_gen.cpp
   ${MODULES_STUBS_DIR}/stub_adc.cpp
+  ${MODULES_STUBS_DIR}/stub_cpu.cpp
   ${MODULES_STUBS_DIR}/stub_eeprom.cpp
   ${MODULES_STUBS_DIR}/stub_gpio.cpp
   ${MODULES_STUBS_DIR}/stub_shr16.cpp
+  ${MODULES_STUBS_DIR}/stub_serial.cpp
   ${MODULES_STUBS_DIR}/stub_timebase.cpp
   ${MODULES_STUBS_DIR}/stub_tmc2130.cpp
   ${LOGIC_STUBS_DIR}/homing.cpp
diff --git a/tests/unit/logic/load_filament/CMakeLists.txt b/tests/unit/logic/load_filament/CMakeLists.txt
index 45524cb..3718333 100644
--- a/tests/unit/logic/load_filament/CMakeLists.txt
+++ b/tests/unit/logic/load_filament/CMakeLists.txt
@@ -2,10 +2,21 @@
 add_executable(
   load_filament_tests
   ${CMAKE_SOURCE_DIR}/src/logic/command_base.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/cut_filament.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/eject_filament.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/feed_to_finda.cpp
   ${CMAKE_SOURCE_DIR}/src/logic/feed_to_bondtech.cpp
   ${CMAKE_SOURCE_DIR}/src/logic/feed_to_finda.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/home.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/idle_mode.cpp
   ${CMAKE_SOURCE_DIR}/src/logic/load_filament.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/move_selector.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/no_command.cpp
   ${CMAKE_SOURCE_DIR}/src/logic/retract_from_finda.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/set_mode.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/tool_change.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/unload_filament.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/unload_to_finda.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/buttons.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/debouncer.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/finda.cpp
@@ -15,14 +26,17 @@ add_executable(
   ${CMAKE_SOURCE_DIR}/src/modules/leds.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/movable_base.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/permanent_storage.cpp
+  ${CMAKE_SOURCE_DIR}/src/modules/protocol.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/pulley.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/selector.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/user_input.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/pulse_gen.cpp
   ${MODULES_STUBS_DIR}/stub_adc.cpp
+  ${MODULES_STUBS_DIR}/stub_cpu.cpp
   ${MODULES_STUBS_DIR}/stub_eeprom.cpp
   ${MODULES_STUBS_DIR}/stub_gpio.cpp
   ${MODULES_STUBS_DIR}/stub_shr16.cpp
+  ${MODULES_STUBS_DIR}/stub_serial.cpp
   ${MODULES_STUBS_DIR}/stub_timebase.cpp
   ${MODULES_STUBS_DIR}/stub_tmc2130.cpp
   ${LOGIC_STUBS_DIR}/homing.cpp
diff --git a/tests/unit/logic/stubs/stub_motion.cpp b/tests/unit/logic/stubs/stub_motion.cpp
index 106f14e..9b79169 100644
--- a/tests/unit/logic/stubs/stub_motion.cpp
+++ b/tests/unit/logic/stubs/stub_motion.cpp
@@ -58,6 +58,9 @@ void Motion::SetPosition(Axis axis, pos_t x) {
 void Motion::SetMode(Axis axis, hal::tmc2130::MotorMode mode) {
 }
 
+void Motion::SetMode(MotorMode mode) {
+}
+
 st_timer_t Motion::Step() {
     for (uint8_t i = 0; i < 3; ++i) {
         if (!axes[i].plannedMoves.empty()) {
diff --git a/tests/unit/logic/tool_change/CMakeLists.txt b/tests/unit/logic/tool_change/CMakeLists.txt
index d3e491c..1aad496 100644
--- a/tests/unit/logic/tool_change/CMakeLists.txt
+++ b/tests/unit/logic/tool_change/CMakeLists.txt
@@ -2,10 +2,18 @@
 add_executable(
   tool_change_tests
   ${CMAKE_SOURCE_DIR}/src/logic/command_base.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/cut_filament.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/eject_filament.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/feed_to_finda.cpp
   ${CMAKE_SOURCE_DIR}/src/logic/feed_to_bondtech.cpp
   ${CMAKE_SOURCE_DIR}/src/logic/feed_to_finda.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/home.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/idle_mode.cpp
   ${CMAKE_SOURCE_DIR}/src/logic/load_filament.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/move_selector.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/no_command.cpp
   ${CMAKE_SOURCE_DIR}/src/logic/retract_from_finda.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/set_mode.cpp
   ${CMAKE_SOURCE_DIR}/src/logic/tool_change.cpp
   ${CMAKE_SOURCE_DIR}/src/logic/unload_filament.cpp
   ${CMAKE_SOURCE_DIR}/src/logic/unload_to_finda.cpp
@@ -18,14 +26,17 @@ add_executable(
   ${CMAKE_SOURCE_DIR}/src/modules/leds.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/movable_base.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/permanent_storage.cpp
+  ${CMAKE_SOURCE_DIR}/src/modules/protocol.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/pulley.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/selector.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/user_input.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/pulse_gen.cpp
   ${MODULES_STUBS_DIR}/stub_adc.cpp
+  ${MODULES_STUBS_DIR}/stub_cpu.cpp
   ${MODULES_STUBS_DIR}/stub_eeprom.cpp
   ${MODULES_STUBS_DIR}/stub_gpio.cpp
   ${MODULES_STUBS_DIR}/stub_shr16.cpp
+  ${MODULES_STUBS_DIR}/stub_serial.cpp
   ${MODULES_STUBS_DIR}/stub_timebase.cpp
   ${MODULES_STUBS_DIR}/stub_tmc2130.cpp
   ${LOGIC_STUBS_DIR}/homing.cpp
diff --git a/tests/unit/logic/unload_filament/CMakeLists.txt b/tests/unit/logic/unload_filament/CMakeLists.txt
index 82b0f6f..a17424a 100644
--- a/tests/unit/logic/unload_filament/CMakeLists.txt
+++ b/tests/unit/logic/unload_filament/CMakeLists.txt
@@ -2,8 +2,18 @@
 add_executable(
   unload_filament_tests
   ${CMAKE_SOURCE_DIR}/src/logic/command_base.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/cut_filament.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/eject_filament.cpp
   ${CMAKE_SOURCE_DIR}/src/logic/feed_to_finda.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/feed_to_bondtech.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/home.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/idle_mode.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/load_filament.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/move_selector.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/no_command.cpp
   ${CMAKE_SOURCE_DIR}/src/logic/retract_from_finda.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/set_mode.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/tool_change.cpp
   ${CMAKE_SOURCE_DIR}/src/logic/unload_filament.cpp
   ${CMAKE_SOURCE_DIR}/src/logic/unload_to_finda.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/buttons.cpp
@@ -15,14 +25,17 @@ add_executable(
   ${CMAKE_SOURCE_DIR}/src/modules/leds.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/movable_base.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/permanent_storage.cpp
+  ${CMAKE_SOURCE_DIR}/src/modules/protocol.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/pulley.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/selector.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/user_input.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/pulse_gen.cpp
   ${MODULES_STUBS_DIR}/stub_adc.cpp
+  ${MODULES_STUBS_DIR}/stub_cpu.cpp
   ${MODULES_STUBS_DIR}/stub_eeprom.cpp
   ${MODULES_STUBS_DIR}/stub_gpio.cpp
   ${MODULES_STUBS_DIR}/stub_shr16.cpp
+  ${MODULES_STUBS_DIR}/stub_serial.cpp
   ${MODULES_STUBS_DIR}/stub_timebase.cpp
   ${MODULES_STUBS_DIR}/stub_tmc2130.cpp
   ${LOGIC_STUBS_DIR}/homing.cpp
diff --git a/tests/unit/logic/unload_to_finda/CMakeLists.txt b/tests/unit/logic/unload_to_finda/CMakeLists.txt
index a4b4471..54ccadc 100644
--- a/tests/unit/logic/unload_to_finda/CMakeLists.txt
+++ b/tests/unit/logic/unload_to_finda/CMakeLists.txt
@@ -2,6 +2,19 @@
 add_executable(
   unload_to_finda_tests
   ${CMAKE_SOURCE_DIR}/src/logic/command_base.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/cut_filament.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/eject_filament.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/feed_to_finda.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/feed_to_bondtech.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/home.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/idle_mode.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/load_filament.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/move_selector.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/no_command.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/retract_from_finda.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/set_mode.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/tool_change.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/unload_filament.cpp
   ${CMAKE_SOURCE_DIR}/src/logic/unload_to_finda.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/buttons.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/debouncer.cpp
@@ -12,14 +25,17 @@ add_executable(
   ${CMAKE_SOURCE_DIR}/src/modules/leds.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/movable_base.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/permanent_storage.cpp
+  ${CMAKE_SOURCE_DIR}/src/modules/protocol.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/pulley.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/selector.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/user_input.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/pulse_gen.cpp
   ${MODULES_STUBS_DIR}/stub_adc.cpp
+  ${MODULES_STUBS_DIR}/stub_cpu.cpp
   ${MODULES_STUBS_DIR}/stub_eeprom.cpp
   ${MODULES_STUBS_DIR}/stub_gpio.cpp
   ${MODULES_STUBS_DIR}/stub_shr16.cpp
+  ${MODULES_STUBS_DIR}/stub_serial.cpp
   ${MODULES_STUBS_DIR}/stub_timebase.cpp
   ${MODULES_STUBS_DIR}/stub_tmc2130.cpp
   ${LOGIC_STUBS_DIR}/homing.cpp
diff --git a/tests/unit/modules/protocol/test_protocol.cpp b/tests/unit/modules/protocol/test_protocol.cpp
index deb3893..2333aab 100644
--- a/tests/unit/modules/protocol/test_protocol.cpp
+++ b/tests/unit/modules/protocol/test_protocol.cpp
@@ -230,7 +230,7 @@ TEST_CASE("protocol::EncodeResponseQueryOperation", "[protocol]") {
 
     uint16_t encodedParamValue = responseStatus == mp::ResponseMsgParamCodes::Error ? (uint16_t)error : (uint16_t)value;
 
-    uint8_t msglen = mp::Protocol::EncodeResponseQueryOperation(requestMsg, responseStatus, encodedParamValue, txbuff.data());
+    uint8_t msglen = mp::Protocol::EncodeResponseQueryOperation(requestMsg, mp::ResponseCommandStatus(responseStatus, encodedParamValue), txbuff.data());
 
     CHECK(msglen <= txbuff.size());
     CHECK(txbuff[0] == (uint8_t)requestMsg.code);
diff --git a/tests/unit/modules/stubs/stub_cpu.cpp b/tests/unit/modules/stubs/stub_cpu.cpp
new file mode 100644
index 0000000..10bf5ea
--- /dev/null
+++ b/tests/unit/modules/stubs/stub_cpu.cpp
@@ -0,0 +1,7 @@
+namespace hal {
+namespace cpu {
+
+void Reset() {}
+
+} // namespace cpu
+} // namespace hal
diff --git a/tests/unit/modules/stubs/stub_serial.cpp b/tests/unit/modules/stubs/stub_serial.cpp
new file mode 100644
index 0000000..c44a73b
--- /dev/null
+++ b/tests/unit/modules/stubs/stub_serial.cpp
@@ -0,0 +1,13 @@
+#include "serial.h"
+
+namespace modules {
+namespace serial {
+
+bool WriteToUSART(const uint8_t *src, uint8_t len) { return false; }
+
+bool Available() { return false; }
+
+uint8_t ConsumeByte() { return 0; }
+
+} // namespace serial
+} // namespace modules