diff --git a/src/logic/tool_change.cpp b/src/logic/tool_change.cpp
index c524486..7f8cb19 100644
--- a/src/logic/tool_change.cpp
+++ b/src/logic/tool_change.cpp
@@ -175,6 +175,8 @@ ProgressCode ToolChange::State() const {
             return ProgressCode::FeedingToFSensor;
         case FeedToBondtech::PushingFilamentIntoNozzle:
             return ProgressCode::FeedingToNozzle;
+        case FeedToBondtech::DisengagingIdler:
+            return ProgressCode::DisengagingIdler;
         }
         // [[fallthrough]] // everything else is reported as FeedingToBondtech
     default:
diff --git a/src/main.cpp b/src/main.cpp
index b83c8bb..b37af1b 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -32,7 +32,8 @@
 /// One-time setup of HW and SW components
 /// Called before entering the loop() function
 /// Green LEDs signalize the progress of initialization. If anything goes wrong we shall turn on a red LED
-void setup() {
+/// Executed with interrupts disabled
+static void setup() {
     hal::cpu::Init();
 
     mt::timebase.Init();
@@ -80,7 +81,10 @@ void setup() {
     ml::leds.Step();
 
     mu::cdc.Init();
+}
 
+/// Second part of setup - executed with interrupts enabled
+static void setup2() {
     // waits at least finda debounce period
     // which is abused to let the LEDs shine for ~100ms
     mf::finda.BlockingInit();
@@ -93,9 +97,7 @@ void setup() {
         // Ideally this should be signalled as an error state and displayed on the printer and recovered properly.
         mg::globals.SetFilamentLoaded(2, mg::InFSensor);
         logic::noCommand.SetInitError(ErrorCode::FINDA_VS_EEPROM_DISREPANCY);
-    }
-
-    if (!mf::finda.Pressed() && mg::globals.FilamentLoaded() >= mg::InSelector) {
+    } else if (!mf::finda.Pressed() && mg::globals.FilamentLoaded() >= mg::InSelector) {
         // Opposite situation - not so dangerous, but definitely confusing to users.
         // FINDA is not pressed but we have a record in the EEPROM.
         // It has been decided, that we shall clear such a record from EEPROM automagically
@@ -158,6 +160,7 @@ void loop() {
 int main() {
     setup();
     sei(); ///enable interrupts
+    setup2();
     for (;;) {
         loop();
     }
diff --git a/src/modules/finda.cpp b/src/modules/finda.cpp
index 41b46d6..77e9886 100644
--- a/src/modules/finda.cpp
+++ b/src/modules/finda.cpp
@@ -14,10 +14,10 @@ void FINDA::Step() {
 }
 
 void FINDA::BlockingInit() {
-    auto tgtMs = mt::timebase.Millis() + config::findaDebounceMs + 1;
-    Step(); // let FINDA settle down - we're gonna need its state for selector homing
-    while (tgtMs < mt::timebase.Millis()) {
-        mf::finda.Step();
+    uint16_t start = mt::timebase.Millis();
+    // let FINDA settle down - we're gonna need its state for selector homing
+    while (!mt::timebase.Elapsed(start, config::findaDebounceMs + 1)) {
+        Step();
     }
 }
 
diff --git a/src/modules/timebase.cpp b/src/modules/timebase.cpp
index 87c1f85..162d25f 100644
--- a/src/modules/timebase.cpp
+++ b/src/modules/timebase.cpp
@@ -20,6 +20,12 @@ void Timebase::Isr() {
     ms++;
 }
 
+bool Timebase::Elapsed(uint16_t start, uint16_t timeout) const {
+    uint16_t ms_from_start = Millis(); // beware the uint16_t!
+    ms_from_start -= start;
+    return ms_from_start > timeout;
+}
+
 uint16_t Timebase::Millis() const {
     return ms;
 }
diff --git a/src/modules/timebase.h b/src/modules/timebase.h
index 56e8992..ac0b065 100644
--- a/src/modules/timebase.h
+++ b/src/modules/timebase.h
@@ -24,6 +24,10 @@ public:
 
     void Isr();
 
+    /// @returns true if the timeout elapsed from the start
+    /// handles correctly timer counter overflows
+    bool Elapsed(uint16_t start, uint16_t timeout) const;
+
 private:
     uint16_t ms;
 };
diff --git a/tests/unit/modules/CMakeLists.txt b/tests/unit/modules/CMakeLists.txt
index 3dfd031..f487280 100644
--- a/tests/unit/modules/CMakeLists.txt
+++ b/tests/unit/modules/CMakeLists.txt
@@ -4,3 +4,4 @@ add_subdirectory(protocol)
 add_subdirectory(speed_table)
 add_subdirectory(pulse_gen)
 add_subdirectory(motion)
+add_subdirectory(timebase)
diff --git a/tests/unit/modules/stubs/stub_timebase.cpp b/tests/unit/modules/stubs/stub_timebase.cpp
index d4e57f9..ab1622c 100644
--- a/tests/unit/modules/stubs/stub_timebase.cpp
+++ b/tests/unit/modules/stubs/stub_timebase.cpp
@@ -24,5 +24,11 @@ void IncMillis(uint16_t inc /* = 1*/) {
     millis += inc;
 }
 
+bool Timebase::Elapsed(uint16_t start, uint16_t timeout) const {
+    uint16_t ms_from_start = Millis(); // beware the uint16_t!
+    ms_from_start -= start;
+    return ms_from_start > timeout;
+}
+
 } // namespace time
 } // namespace modules
diff --git a/tests/unit/modules/timebase/CMakeLists.txt b/tests/unit/modules/timebase/CMakeLists.txt
new file mode 100644
index 0000000..932d8b4
--- /dev/null
+++ b/tests/unit/modules/timebase/CMakeLists.txt
@@ -0,0 +1,8 @@
+# define the test executable
+add_executable(timebase_tests ${MODULES_STUBS_DIR}/stub_timebase.cpp test_timebase.cpp)
+
+# define required search paths
+target_include_directories(timebase_tests PUBLIC ${CMAKE_SOURCE_DIR}/src/modules)
+
+# tell build system about the test case
+add_catch_test(timebase_tests)
diff --git a/tests/unit/modules/timebase/test_timebase.cpp b/tests/unit/modules/timebase/test_timebase.cpp
new file mode 100644
index 0000000..672418e
--- /dev/null
+++ b/tests/unit/modules/timebase/test_timebase.cpp
@@ -0,0 +1,22 @@
+#include "catch2/catch.hpp"
+#include "timebase.h"
+#include "../stubs/stub_timebase.h"
+
+using Catch::Matchers::Equals;
+
+// this is not a pure test of the real implementation (it would require splitting the timebase.cpp into 2 parts)
+// but serves the sole purpose of debugging the Elapsed() impl.
+TEST_CASE("timebase::Elapsed", "[timebase]") {
+    {
+        mt::ReinitTimebase(0);
+        uint16_t start = mt::timebase.Millis();
+        mt::IncMillis(5);
+        REQUIRE(mt::timebase.Elapsed(start, 4));
+    }
+    {
+        mt::ReinitTimebase(0xffff);
+        uint16_t start = mt::timebase.Millis();
+        mt::IncMillis(5);
+        REQUIRE(mt::timebase.Elapsed(start, 4));
+    }
+}