diff --git a/src/logic/command_base.cpp b/src/logic/command_base.cpp
index c5f05d8..bb75403 100644
--- a/src/logic/command_base.cpp
+++ b/src/logic/command_base.cpp
@@ -17,6 +17,10 @@ inline ErrorCode &operator|=(ErrorCode &a, ErrorCode b) {
     return a = (ErrorCode)((uint16_t)a | (uint16_t)b);
 }
 
+inline ErrorCode operator<<(ErrorCode a, uint8_t b) {
+    return a = (ErrorCode)((uint16_t)a << (uint16_t)b);
+}
+
 static ErrorCode TMC2130ToErrorCode(const hal::tmc2130::ErrorFlags &ef) {
     ErrorCode e = ErrorCode::RUNNING;
 
@@ -39,21 +43,8 @@ static ErrorCode TMC2130ToErrorCode(const hal::tmc2130::ErrorFlags &ef) {
     return e;
 }
 
-static ErrorCode __attribute__((noinline)) AddErrorAxisBit(ErrorCode ec, uint8_t tmcIndex) {
-    switch (tmcIndex) {
-    case config::Axis::Pulley:
-        ec |= ErrorCode::TMC_PULLEY_BIT;
-        break;
-    case config::Axis::Selector:
-        ec |= ErrorCode::TMC_SELECTOR_BIT;
-        break;
-    case config::Axis::Idler:
-        ec |= ErrorCode::TMC_IDLER_BIT;
-        break;
-    default:
-        break;
-    }
-    return ec;
+ErrorCode __attribute__((noinline)) AddErrorAxisBit(ErrorCode ec, uint8_t axis) {
+    return ec |= (ErrorCode::TMC_PULLEY_BIT << axis);
 }
 
 ErrorCode CheckMovable(const mm::MovableBase &m) {
diff --git a/src/logic/command_base.h b/src/logic/command_base.h
index fce3c79..5bca87b 100644
--- a/src/logic/command_base.h
+++ b/src/logic/command_base.h
@@ -14,6 +14,10 @@ class MovableBase;
 /// The logic namespace handles the application logic on top of the modules.
 namespace logic {
 
+/// Bitwise OR (ErrorCode::TMC_PULLEY_BIT << axis) into ec
+/// where axis ranges from 0 to 2
+ErrorCode AddErrorAxisBit(ErrorCode ec, uint8_t axis);
+
 /// @brief Base class defining common API for high-level operations/commands/state machines
 ///
 /// Which state machines are high-level? Those which are being initiated either by a command over the serial line or from a button
diff --git a/src/logic/hw_sanity.cpp b/src/logic/hw_sanity.cpp
index 23b1f70..0b396fc 100644
--- a/src/logic/hw_sanity.cpp
+++ b/src/logic/hw_sanity.cpp
@@ -1,5 +1,7 @@
 /// @file hw_sanity.cpp
+#include <string.h>
 #include "hw_sanity.h"
+#include "command_base.h"
 #include "../modules/globals.h"
 #include "../modules/motion.h"
 #include "../modules/leds.h"
@@ -26,9 +28,7 @@ bool HWSanity::Reset(uint8_t param) {
     state = ProgressCode::HWTestBegin;
     error = ErrorCode::RUNNING;
     axis = config::Axis::Idler;
-    fault_masks[0] = 0;
-    fault_masks[1] = 0;
-    fault_masks[2] = 0;
+    memset(fault_masks, 0, sizeof(fault_masks));
     return true;
 }
 
@@ -140,20 +140,12 @@ bool HWSanity::StepInner() {
             // error, display it and return the code.
             state = ProgressCode::ErrHwTestFailed;
             error = ErrorCode::MMU_SOLDERING_NEEDS_ATTENTION;
-            uint8_t mask = fault_masks[Axis::Idler];
-            if (mask) {
-                error |= ErrorCode::TMC_IDLER_BIT;
-                SetFaultDisplay(0, mask);
-            }
-            mask = fault_masks[Axis::Pulley];
-            if (mask) {
-                error |= ErrorCode::TMC_PULLEY_BIT;
-                SetFaultDisplay(2, mask);
-            }
-            mask = fault_masks[Axis::Selector];
-            if (mask) {
-                error |= ErrorCode::TMC_SELECTOR_BIT;
-                SetFaultDisplay(1, mask);
+            for (uint8_t axis = 0; axis < 3; axis++) {
+                const uint8_t mask = fault_masks[axis];
+                if (mask) {
+                    error = logic::AddErrorAxisBit(error, axis);
+                    SetFaultDisplay(axis, mask);
+                }
             }
             ml::leds.SetMode(3, ml::red, ml::off);
             ml::leds.SetMode(3, ml::green, ml::off);