diff --git a/makefiles/virtual/uCNC-emulator.dev b/makefiles/virtual/uCNC-emulator.dev
index f2717d481..54a8cde1c 100644
--- a/makefiles/virtual/uCNC-emulator.dev
+++ b/makefiles/virtual/uCNC-emulator.dev
@@ -29,7 +29,7 @@ IncludeVersionInfo=0
 SupportXPThemes=0
 CompilerSet=1
 CompilerSettings=000000e0g0000000001000000
-UnitCount=94
+UnitCount=96
 
 [VersionInfo]
 Major=1
@@ -260,18 +260,8 @@ Priority=1000
 OverrideBuildCmd=0
 BuildCmd=
 
-[Unit23]
-FileName=..\..\uCNC\src\modules\ic74hc595.c
-CompileCpp=1
-Folder=
-Compile=1
-Link=1
-Priority=1000
-OverrideBuildCmd=0
-BuildCmd=
-
 [Unit24]
-FileName=..\..\uCNC\src\modules\ic74hc595.h
+FileName=..\..\uCNC\src\modules\modbus.c
 CompileCpp=1
 Folder=
 Compile=1
@@ -281,7 +271,7 @@ OverrideBuildCmd=0
 BuildCmd=
 
 [Unit25]
-FileName=..\..\uCNC\src\modules\modbus.c
+FileName=..\..\uCNC\src\modules\modbus.h
 CompileCpp=1
 Folder=
 Compile=1
@@ -291,7 +281,7 @@ OverrideBuildCmd=0
 BuildCmd=
 
 [Unit26]
-FileName=..\..\uCNC\src\modules\modbus.h
+FileName=..\..\uCNC\src\modules\pid.c
 CompileCpp=1
 Folder=
 Compile=1
@@ -301,7 +291,7 @@ OverrideBuildCmd=0
 BuildCmd=
 
 [Unit27]
-FileName=..\..\uCNC\src\modules\pid.c
+FileName=..\..\uCNC\src\modules\pid.h
 CompileCpp=1
 Folder=
 Compile=1
@@ -311,7 +301,7 @@ OverrideBuildCmd=0
 BuildCmd=
 
 [Unit28]
-FileName=..\..\uCNC\src\modules\pid.h
+FileName=..\..\uCNC\src\modules\softi2c.c
 CompileCpp=1
 Folder=
 Compile=1
@@ -321,7 +311,7 @@ OverrideBuildCmd=0
 BuildCmd=
 
 [Unit29]
-FileName=..\..\uCNC\src\modules\softi2c.c
+FileName=..\..\uCNC\src\modules\softi2c.h
 CompileCpp=1
 Folder=
 Compile=1
@@ -331,7 +321,7 @@ OverrideBuildCmd=0
 BuildCmd=
 
 [Unit30]
-FileName=..\..\uCNC\src\modules\softi2c.h
+FileName=..\..\uCNC\src\modules\softspi.c
 CompileCpp=1
 Folder=
 Compile=1
@@ -341,7 +331,7 @@ OverrideBuildCmd=0
 BuildCmd=
 
 [Unit31]
-FileName=..\..\uCNC\src\modules\softspi.c
+FileName=..\..\uCNC\src\modules\softspi.h
 CompileCpp=1
 Folder=
 Compile=1
@@ -351,7 +341,7 @@ OverrideBuildCmd=0
 BuildCmd=
 
 [Unit32]
-FileName=..\..\uCNC\src\modules\softspi.h
+FileName=..\..\uCNC\src\modules\softuart.c
 CompileCpp=1
 Folder=
 Compile=1
@@ -361,7 +351,7 @@ OverrideBuildCmd=0
 BuildCmd=
 
 [Unit33]
-FileName=..\..\uCNC\src\modules\softuart.c
+FileName=..\..\uCNC\src\modules\softuart.h
 CompileCpp=1
 Folder=
 Compile=1
@@ -371,7 +361,7 @@ OverrideBuildCmd=0
 BuildCmd=
 
 [Unit34]
-FileName=..\..\uCNC\src\modules\softuart.h
+FileName=..\..\uCNC\src\modules\system_languages.h
 CompileCpp=1
 Folder=
 Compile=1
@@ -381,7 +371,7 @@ OverrideBuildCmd=0
 BuildCmd=
 
 [Unit35]
-FileName=..\..\uCNC\src\modules\system_languages.h
+FileName=..\..\uCNC\src\modules\language\language_en.h
 CompileCpp=1
 Folder=
 Compile=1
@@ -391,7 +381,7 @@ OverrideBuildCmd=0
 BuildCmd=
 
 [Unit38]
-FileName=..\..\uCNC\src\interface\grbl_interface.h
+FileName=..\..\uCNC\src\hal\io_hal.h
 CompileCpp=1
 Folder=
 Compile=1
@@ -401,7 +391,7 @@ OverrideBuildCmd=0
 BuildCmd=
 
 [Unit42]
-FileName=..\..\uCNC\src\hal\tools\tool_helper.h
+FileName=..\..\uCNC\src\hal\tools\tools\laser_ppi.c
 CompileCpp=1
 Folder=
 Compile=1
@@ -411,7 +401,7 @@ OverrideBuildCmd=0
 BuildCmd=
 
 [Unit43]
-FileName=..\..\uCNC\src\hal\tools\tools\laser_ppi.c
+FileName=..\..\uCNC\src\hal\tools\tools\laser_pwm.c
 CompileCpp=1
 Folder=
 Compile=1
@@ -421,7 +411,7 @@ OverrideBuildCmd=0
 BuildCmd=
 
 [Unit44]
-FileName=..\..\uCNC\src\hal\tools\tools\laser_pwm.c
+FileName=..\..\uCNC\src\hal\tools\tools\pen_servo.c
 CompileCpp=1
 Folder=
 Compile=1
@@ -431,7 +421,7 @@ OverrideBuildCmd=0
 BuildCmd=
 
 [Unit45]
-FileName=..\..\uCNC\src\hal\tools\tools\pen_servo.c
+FileName=..\..\uCNC\src\hal\tools\tools\plasma_thc.c
 CompileCpp=1
 Folder=
 Compile=1
@@ -441,7 +431,7 @@ OverrideBuildCmd=0
 BuildCmd=
 
 [Unit46]
-FileName=..\..\uCNC\src\hal\tools\tools\plasma_thc.c
+FileName=..\..\uCNC\src\hal\tools\tools\spindle_besc.c
 CompileCpp=1
 Folder=
 Compile=1
@@ -451,7 +441,7 @@ OverrideBuildCmd=0
 BuildCmd=
 
 [Unit47]
-FileName=..\..\uCNC\src\hal\tools\tools\spindle_besc.c
+FileName=..\..\uCNC\src\hal\tools\tools\spindle_pwm.c
 CompileCpp=1
 Folder=
 Compile=1
@@ -461,7 +451,7 @@ OverrideBuildCmd=0
 BuildCmd=
 
 [Unit48]
-FileName=..\..\uCNC\src\hal\tools\tools\spindle_pwm.c
+FileName=..\..\uCNC\src\hal\tools\tools\spindle_relay.c
 CompileCpp=1
 Folder=
 Compile=1
@@ -471,7 +461,7 @@ OverrideBuildCmd=0
 BuildCmd=
 
 [Unit49]
-FileName=..\..\uCNC\src\hal\tools\tools\spindle_relay.c
+FileName=..\..\uCNC\src\hal\tools\tools\vfd_modbus.c
 CompileCpp=1
 Folder=
 Compile=1
@@ -481,7 +471,7 @@ OverrideBuildCmd=0
 BuildCmd=
 
 [Unit50]
-FileName=..\..\uCNC\src\hal\tools\tools\vfd_modbus.c
+FileName=..\..\uCNC\src\hal\tools\tools\vfd_pwm.c
 CompileCpp=1
 Folder=
 Compile=1
@@ -491,7 +481,7 @@ OverrideBuildCmd=0
 BuildCmd=
 
 [Unit51]
-FileName=..\..\uCNC\src\hal\tools\tools\vfd_pwm.c
+FileName=..\..\uCNC\src\hal\mcus\mcu.c
 CompileCpp=1
 Folder=
 Compile=1
@@ -501,7 +491,7 @@ OverrideBuildCmd=0
 BuildCmd=
 
 [Unit52]
-FileName=..\..\uCNC\src\hal\mcus\mcu.c
+FileName=..\..\uCNC\src\hal\mcus\mcu.h
 CompileCpp=1
 Folder=
 Compile=1
@@ -511,7 +501,7 @@ OverrideBuildCmd=0
 BuildCmd=
 
 [Unit53]
-FileName=..\..\uCNC\src\hal\mcus\mcu.h
+FileName=..\..\uCNC\src\hal\mcus\mcudefs.h
 CompileCpp=1
 Folder=
 Compile=1
@@ -521,7 +511,7 @@ OverrideBuildCmd=0
 BuildCmd=
 
 [Unit54]
-FileName=..\..\uCNC\src\hal\mcus\mcudefs.h
+FileName=..\..\uCNC\src\hal\mcus\mcus.h
 CompileCpp=1
 Folder=
 Compile=1
@@ -531,7 +521,7 @@ OverrideBuildCmd=0
 BuildCmd=
 
 [Unit55]
-FileName=..\..\uCNC\src\hal\mcus\mcus.h
+FileName=..\..\uCNC\src\hal\mcus\virtual\mcumap_virtual.h
 CompileCpp=1
 Folder=
 Compile=1
@@ -541,7 +531,7 @@ OverrideBuildCmd=0
 BuildCmd=
 
 [Unit56]
-FileName=..\..\uCNC\src\hal\mcus\virtual\mcumap_virtual.h
+FileName=..\..\uCNC\src\hal\kinematics\kinematic.c
 CompileCpp=1
 Folder=
 Compile=1
@@ -551,7 +541,7 @@ OverrideBuildCmd=0
 BuildCmd=
 
 [Unit57]
-FileName=..\..\uCNC\src\hal\kinematics\kinematic.c
+FileName=..\..\uCNC\src\hal\kinematics\kinematic.h
 CompileCpp=1
 Folder=
 Compile=1
@@ -561,7 +551,7 @@ OverrideBuildCmd=0
 BuildCmd=
 
 [Unit58]
-FileName=..\..\uCNC\src\hal\kinematics\kinematic.h
+FileName=..\..\uCNC\src\hal\kinematics\kinematic_cartesian.c
 CompileCpp=1
 Folder=
 Compile=1
@@ -571,7 +561,7 @@ OverrideBuildCmd=0
 BuildCmd=
 
 [Unit59]
-FileName=..\..\uCNC\src\hal\kinematics\kinematic_cartesian.c
+FileName=..\..\uCNC\src\hal\kinematics\kinematic_cartesian.h
 CompileCpp=1
 Folder=
 Compile=1
@@ -581,7 +571,7 @@ OverrideBuildCmd=0
 BuildCmd=
 
 [Unit60]
-FileName=..\..\uCNC\src\hal\kinematics\kinematic_cartesian.h
+FileName=..\..\uCNC\src\hal\kinematics\kinematic_corexy.c
 CompileCpp=1
 Folder=
 Compile=1
@@ -591,7 +581,7 @@ OverrideBuildCmd=0
 BuildCmd=
 
 [Unit61]
-FileName=..\..\uCNC\src\hal\kinematics\kinematic_corexy.c
+FileName=..\..\uCNC\src\hal\kinematics\kinematic_corexy.h
 CompileCpp=1
 Folder=
 Compile=1
@@ -601,7 +591,7 @@ OverrideBuildCmd=0
 BuildCmd=
 
 [Unit62]
-FileName=..\..\uCNC\src\hal\kinematics\kinematic_corexy.h
+FileName=..\..\uCNC\src\hal\kinematics\kinematic_delta.c
 CompileCpp=1
 Folder=
 Compile=1
@@ -611,7 +601,7 @@ OverrideBuildCmd=0
 BuildCmd=
 
 [Unit63]
-FileName=..\..\uCNC\src\hal\kinematics\kinematic_delta.c
+FileName=..\..\uCNC\src\hal\kinematics\kinematic_delta.h
 CompileCpp=1
 Folder=
 Compile=1
@@ -621,7 +611,7 @@ OverrideBuildCmd=0
 BuildCmd=
 
 [Unit64]
-FileName=..\..\uCNC\src\hal\kinematics\kinematic_delta.h
+FileName=..\..\uCNC\src\hal\kinematics\kinematic_linear_delta.c
 CompileCpp=1
 Folder=
 Compile=1
@@ -631,7 +621,7 @@ OverrideBuildCmd=0
 BuildCmd=
 
 [Unit65]
-FileName=..\..\uCNC\src\hal\kinematics\kinematic_linear_delta.c
+FileName=..\..\uCNC\src\hal\kinematics\kinematic_linear_delta.h
 CompileCpp=1
 Folder=
 Compile=1
@@ -641,7 +631,7 @@ OverrideBuildCmd=0
 BuildCmd=
 
 [Unit66]
-FileName=..\..\uCNC\src\hal\kinematics\kinematic_linear_delta.h
+FileName=..\..\uCNC\src\hal\kinematics\kinematic_scara.c
 CompileCpp=1
 Folder=
 Compile=1
@@ -651,7 +641,7 @@ OverrideBuildCmd=0
 BuildCmd=
 
 [Unit67]
-FileName=..\..\uCNC\src\hal\kinematics\kinematic_scara.c
+FileName=..\..\uCNC\src\hal\kinematics\kinematic_scara.h
 CompileCpp=1
 Folder=
 Compile=1
@@ -661,7 +651,7 @@ OverrideBuildCmd=0
 BuildCmd=
 
 [Unit68]
-FileName=..\..\uCNC\src\hal\kinematics\kinematic_scara.h
+FileName=..\..\uCNC\src\hal\kinematics\kinematicdefs.h
 CompileCpp=1
 Folder=
 Compile=1
@@ -671,7 +661,7 @@ OverrideBuildCmd=0
 BuildCmd=
 
 [Unit69]
-FileName=..\..\uCNC\src\hal\kinematics\kinematicdefs.h
+FileName=..\..\uCNC\src\hal\kinematics\kinematics.h
 CompileCpp=1
 Folder=
 Compile=1
@@ -681,7 +671,7 @@ OverrideBuildCmd=0
 BuildCmd=
 
 [Unit70]
-FileName=..\..\uCNC\src\hal\kinematics\kinematics.h
+FileName=..\..\uCNC\src\hal\boards\boarddefs.h
 CompileCpp=1
 Folder=
 Compile=1
@@ -691,7 +681,7 @@ OverrideBuildCmd=0
 BuildCmd=
 
 [Unit71]
-FileName=..\..\uCNC\src\hal\boards\boarddefs.h
+FileName=..\..\uCNC\src\core\interpolator.c
 CompileCpp=1
 Folder=
 Compile=1
@@ -701,7 +691,7 @@ OverrideBuildCmd=0
 BuildCmd=
 
 [Unit72]
-FileName=..\..\uCNC\src\core\interpolator.c
+FileName=..\..\uCNC\src\core\interpolator.h
 CompileCpp=1
 Folder=
 Compile=1
@@ -711,7 +701,7 @@ OverrideBuildCmd=0
 BuildCmd=
 
 [Unit73]
-FileName=..\..\uCNC\src\core\interpolator.h
+FileName=..\..\uCNC\src\core\io_control.c
 CompileCpp=1
 Folder=
 Compile=1
@@ -721,7 +711,7 @@ OverrideBuildCmd=0
 BuildCmd=
 
 [Unit74]
-FileName=..\..\uCNC\src\core\io_control.c
+FileName=..\..\uCNC\src\core\io_control.h
 CompileCpp=1
 Folder=
 Compile=1
@@ -731,7 +721,7 @@ OverrideBuildCmd=0
 BuildCmd=
 
 [Unit75]
-FileName=..\..\uCNC\src\core\io_control.h
+FileName=..\..\uCNC\src\core\motion_control.c
 CompileCpp=1
 Folder=
 Compile=1
@@ -741,7 +731,7 @@ OverrideBuildCmd=0
 BuildCmd=
 
 [Unit76]
-FileName=..\..\uCNC\src\core\motion_control.c
+FileName=..\..\uCNC\src\core\motion_control.h
 CompileCpp=1
 Folder=
 Compile=1
@@ -751,7 +741,7 @@ OverrideBuildCmd=0
 BuildCmd=
 
 [Unit77]
-FileName=..\..\uCNC\src\core\motion_control.h
+FileName=..\..\uCNC\src\core\parser.c
 CompileCpp=1
 Folder=
 Compile=1
@@ -761,7 +751,7 @@ OverrideBuildCmd=0
 BuildCmd=
 
 [Unit78]
-FileName=..\..\uCNC\src\core\parser.c
+FileName=..\..\uCNC\src\core\parser.h
 CompileCpp=1
 Folder=
 Compile=1
@@ -771,7 +761,7 @@ OverrideBuildCmd=0
 BuildCmd=
 
 [Unit79]
-FileName=..\..\uCNC\src\core\parser.h
+FileName=..\..\uCNC\src\core\planner.c
 CompileCpp=1
 Folder=
 Compile=1
@@ -781,7 +771,7 @@ OverrideBuildCmd=0
 BuildCmd=
 
 [Unit80]
-FileName=..\..\uCNC\src\core\planner.c
+FileName=..\..\uCNC\src\core\planner.h
 CompileCpp=1
 Folder=
 Compile=1
@@ -791,7 +781,7 @@ OverrideBuildCmd=0
 BuildCmd=
 
 [Unit81]
-FileName=..\..\uCNC\src\core\planner.h
+FileName=..\..\uCNC\src\utils.c
 CompileCpp=1
 Folder=
 Compile=1
@@ -801,7 +791,7 @@ OverrideBuildCmd=0
 BuildCmd=
 
 [Unit82]
-FileName=..\..\uCNC\src\utils.c
+FileName=..\..\uCNC\src\interface\grbl_print.c
 CompileCpp=1
 Folder=
 Compile=1
@@ -811,7 +801,7 @@ OverrideBuildCmd=0
 BuildCmd=
 
 [Unit84]
-FileName=..\..\uCNC\src\interface\grbl_protocol.c
+FileName=..\..\uCNC\src\interface\grbl_settings.c
 CompileCpp=1
 Folder=
 Compile=1
@@ -821,7 +811,7 @@ OverrideBuildCmd=0
 BuildCmd=
 
 [Unit85]
-FileName=..\..\uCNC\src\interface\grbl_settings.c
+FileName=..\..\uCNC\src\interface\grbl_stream.c
 Folder=
 Compile=1
 Link=1
@@ -831,7 +821,7 @@ BuildCmd=
 CompileCpp=1
 
 [Unit86]
-FileName=..\..\uCNC\src\interface\grbl_stream.c
+FileName=..\..\uCNC\src\interface\grbl_print.h
 CompileCpp=1
 Folder=
 Compile=1
@@ -841,7 +831,7 @@ OverrideBuildCmd=0
 BuildCmd=
 
 [Unit89]
-FileName=..\..\uCNC\src\interface\grbl_settings.h
+FileName=..\..\uCNC\src\interface\grbl_stream.h
 CompileCpp=1
 Folder=
 Compile=1
@@ -851,7 +841,7 @@ OverrideBuildCmd=0
 BuildCmd=
 
 [Unit90]
-FileName=..\..\uCNC\src\interface\grbl_stream.h
+FileName=..\..\uCNC\src\interface\serial_compatibility.h
 CompileCpp=1
 Folder=
 Compile=1
@@ -861,7 +851,7 @@ OverrideBuildCmd=0
 BuildCmd=
 
 [Unit91]
-FileName=..\..\uCNC\src\interface\serial_compatibility.h
+FileName=..\..\uCNC\src\modules\file_system.c
 CompileCpp=1
 Folder=
 Compile=1
@@ -871,7 +861,7 @@ OverrideBuildCmd=0
 BuildCmd=
 
 [Unit93]
-FileName=..\..\uCNC\src\modules\file_system.h
+FileName=..\..\uCNC\src\core\parser_expr.c
 CompileCpp=1
 Folder=
 Compile=1
@@ -881,7 +871,7 @@ OverrideBuildCmd=0
 BuildCmd=
 
 [Unit96]
-FileName=..\..\uCNC\src\modules\m66\parser_m66.c
+FileName=..\..\uCNC\src\modules\shift_register.h
 CompileCpp=1
 Folder=
 Compile=1
@@ -901,7 +891,7 @@ OverrideBuildCmd=0
 BuildCmd=
 
 [Unit87]
-FileName=..\..\uCNC\src\interface\grbl_print.h
+FileName=..\..\uCNC\src\interface\grbl_protocol.h
 CompileCpp=1
 Folder=
 Compile=1
@@ -911,7 +901,7 @@ OverrideBuildCmd=0
 BuildCmd=
 
 [Unit88]
-FileName=..\..\uCNC\src\interface\grbl_protocol.h
+FileName=..\..\uCNC\src\interface\grbl_settings.h
 CompileCpp=1
 Folder=
 Compile=1
@@ -921,7 +911,7 @@ OverrideBuildCmd=0
 BuildCmd=
 
 [Unit36]
-FileName=..\..\uCNC\src\modules\language\language_en.h
+FileName=..\..\uCNC\src\interface\defaults.h
 CompileCpp=1
 Folder=
 Compile=1
@@ -931,7 +921,7 @@ OverrideBuildCmd=0
 BuildCmd=
 
 [Unit37]
-FileName=..\..\uCNC\src\interface\defaults.h
+FileName=..\..\uCNC\src\interface\grbl_interface.h
 CompileCpp=1
 Folder=
 Compile=1
@@ -941,7 +931,7 @@ OverrideBuildCmd=0
 BuildCmd=
 
 [Unit41]
-FileName=..\..\uCNC\src\hal\tools\tool.h
+FileName=..\..\uCNC\src\hal\tools\tool_helper.h
 CompileCpp=1
 Folder=
 Compile=1
@@ -951,7 +941,7 @@ OverrideBuildCmd=0
 BuildCmd=
 
 [Unit39]
-FileName=..\..\uCNC\src\hal\io_hal.h
+FileName=..\..\uCNC\src\hal\tools\tool.c
 CompileCpp=1
 Folder=
 Compile=1
@@ -961,7 +951,7 @@ OverrideBuildCmd=0
 BuildCmd=
 
 [Unit40]
-FileName=..\..\uCNC\src\hal\tools\tool.c
+FileName=..\..\uCNC\src\hal\tools\tool.h
 CompileCpp=1
 Folder=
 Compile=1
@@ -971,7 +961,7 @@ OverrideBuildCmd=0
 BuildCmd=
 
 [Unit95]
-FileName=..\..\uCNC\src\modules\system_menu.h
+FileName=..\..\uCNC\src\modules\shift_register.c
 CompileCpp=1
 Folder=
 Compile=1
@@ -1011,7 +1001,7 @@ OverrideBuildCmd=0
 BuildCmd=
 
 [Unit83]
-FileName=..\..\uCNC\src\interface\grbl_print.c
+FileName=..\..\uCNC\src\interface\grbl_protocol.c
 CompileCpp=1
 Folder=
 Compile=1
@@ -1021,7 +1011,7 @@ OverrideBuildCmd=0
 BuildCmd=
 
 [Unit92]
-FileName=..\..\uCNC\src\modules\file_system.c
+FileName=..\..\uCNC\src\modules\file_system.h
 CompileCpp=1
 Folder=
 Compile=1
@@ -1031,7 +1021,17 @@ OverrideBuildCmd=0
 BuildCmd=
 
 [Unit94]
-FileName=..\..\uCNC\src\core\parser_expr.c
+FileName=..\..\uCNC\src\modules\ic74hc165.h
+CompileCpp=1
+Folder=
+Compile=1
+Link=1
+Priority=1000
+OverrideBuildCmd=0
+BuildCmd=
+
+[Unit23]
+FileName=..\..\uCNC\src\modules\ic74hc595.h
 CompileCpp=1
 Folder=
 Compile=1
diff --git a/uCNC/src/hal/mcus/virtual/mcumap_virtual.h b/uCNC/src/hal/mcus/virtual/mcumap_virtual.h
index 4614338e2..4fab754ac 100644
--- a/uCNC/src/hal/mcus/virtual/mcumap_virtual.h
+++ b/uCNC/src/hal/mcus/virtual/mcumap_virtual.h
@@ -36,6 +36,8 @@
 #define rom_memcpy memcpy
 #define rom_read_byte *
 
+#define MCU_WEAK __attribute__((weak,weakref))
+
 /* 7.18.2.1  Limits of exact-width integer types */
 #define INT8_MIN (-128)
 #define INT16_MIN (-32768)
@@ -258,6 +260,8 @@
 #define DIO77 77
 #define DOUT31 78
 #define DIO78 78
+
+#ifndef EMULATE_74HC165
 #define LIMIT_X 100
 #define DIO100 100
 #define LIMIT_Y 101
@@ -286,38 +290,6 @@
 #define DIO112 112
 #define CS_RES 113
 #define DIO113 113
-#define ANALOG0 114
-#define DIO114 114
-#define ANALOG1 115
-#define DIO115 115
-#define ANALOG2 116
-#define DIO116 116
-#define ANALOG3 117
-#define DIO117 117
-#define ANALOG4 118
-#define DIO118 118
-#define ANALOG5 119
-#define DIO119 119
-#define ANALOG6 120
-#define DIO120 120
-#define ANALOG7 121
-#define DIO121 121
-#define ANALOG8 122
-#define DIO122 122
-#define ANALOG9 123
-#define DIO123 123
-#define ANALOG10 124
-#define DIO124 124
-#define ANALOG11 125
-#define DIO125 125
-#define ANALOG12 126
-#define DIO126 126
-#define ANALOG13 127
-#define DIO127 127
-#define ANALOG14 128
-#define DIO128 128
-#define ANALOG15 129
-#define DIO129 129
 #define DIN0 130
 #define DIO130 130
 #define DIN1 131
@@ -334,6 +306,30 @@
 #define DIO136 136
 #define DIN7 137
 #define DIO137 137
+#else
+#define IC74HC165_COUNT 4
+#define LIMIT_X_IO_OFFSET 0
+#define LIMIT_Y_IO_OFFSET 1
+#define LIMIT_Z_IO_OFFSET 2
+#define LIMIT_A_IO_OFFSET 3
+#define LIMIT_B_IO_OFFSET 4
+#define LIMIT_C_IO_OFFSET 5
+#define PROBE_IO_OFFSET 6
+#define ESTOP_IO_OFFSET 7
+#define SAFETY_DOOR_IO_OFFSET 8
+#define FHOLD_IO_OFFSET 9
+#define CS_RES_IO_OFFSET 10
+#define DIN0_IO_OFFSET 11
+#define DIN1_IO_OFFSET 12
+#define DIN2_IO_OFFSET 13
+#define DIN3_IO_OFFSET 14
+#define DIN4_IO_OFFSET 15
+#define DIN5_IO_OFFSET 16
+#define DIN6_IO_OFFSET 17
+#define DIN7_IO_OFFSET 18
+// #define PWM0_IO_OFFSET 24
+#endif
+
 #define DIN8 138
 #define DIO138 138
 #define DIN9 139
@@ -382,6 +378,38 @@
 #define DIO160 160
 #define DIN31 161
 #define DIO161 161
+#define ANALOG0 114
+#define DIO114 114
+#define ANALOG1 115
+#define DIO115 115
+#define ANALOG2 116
+#define DIO116 116
+#define ANALOG3 117
+#define DIO117 117
+#define ANALOG4 118
+#define DIO118 118
+#define ANALOG5 119
+#define DIO119 119
+#define ANALOG6 120
+#define DIO120 120
+#define ANALOG7 121
+#define DIO121 121
+#define ANALOG8 122
+#define DIO122 122
+#define ANALOG9 123
+#define DIO123 123
+#define ANALOG10 124
+#define DIO124 124
+#define ANALOG11 125
+#define DIO125 125
+#define ANALOG12 126
+#define DIO126 126
+#define ANALOG13 127
+#define DIO127 127
+#define ANALOG14 128
+#define DIO128 128
+#define ANALOG15 129
+#define DIO129 129
 #define TX 200
 #define DIO200 200
 #define RX 201
diff --git a/uCNC/src/modules/shift_register.c b/uCNC/src/modules/shift_register.c
index eef0b1d00..31da7e8e1 100644
--- a/uCNC/src/modules/shift_register.c
+++ b/uCNC/src/modules/shift_register.c
@@ -71,83 +71,86 @@ volatile uint8_t ic74hc165_io_pins[IC74HC165_COUNT];
 
 DECL_MUTEX(shifter_running);
 
-MCU_CALLBACK void __attribute__((weak)) shift_register_io_pins(void)
+MCU_CALLBACK void /*__attribute__((weak))*/ shift_register_io_pins(void)
 {
-	MUTEX_LOCK_OR_EXIT(shifter_running);
+	MUTEX_INIT(shifter_running);
 
-	uint8_t pins[SHIFT_REGISTER_BYTES];
+	MUTEX_TAKE(shifter_running)
+	{
+		uint8_t pins[SHIFT_REGISTER_BYTES];
 
 #if (IC74HC165_COUNT > 0)
-	mcu_clear_output(IC74HC165_LOAD);
-	memset(pins, 0, IC74HC165_COUNT);
+		mcu_clear_output(IC74HC165_LOAD);
+		memset(pins, 0, IC74HC165_COUNT);
 #endif
-	__ATOMIC__
-	{
+		__ATOMIC__
+		{
 #if (IC74HC595_COUNT > 0)
-		memcpy(pins, (const void *)ic74hc595_io_pins, IC74HC595_COUNT);
+			memcpy(pins, (const void *)ic74hc595_io_pins, IC74HC595_COUNT);
 #endif
-	}
+		}
 #if (IC74HC165_COUNT > 0)
-	mcu_delay_us(5);
-	mcu_set_output(IC74HC165_LOAD);
-#endif
-	mcu_clear_output(IC74HC595_LATCH);
-	
-	/**
-	 * shift bytes
-	 */
-	for (uint8_t i = SHIFT_REGISTER_BYTES; i != 0;)
-	{
-		i--;
+		mcu_delay_us(5);
+		mcu_set_output(IC74HC165_LOAD);
+#endif
+		mcu_clear_output(IC74HC595_LATCH);
+
+		/**
+		 * shift bytes
+		 */
+		for (uint8_t i = SHIFT_REGISTER_BYTES; i != 0;)
+		{
+			i--;
 #if (defined(SHIFT_REGISTER_USE_HW_SPI) && defined(MCU_HAS_SPI))
-		mcu_spi_xmit(pins[i]);
+			mcu_spi_xmit(pins[i]);
 #else
-		uint8_t pinbyte = pins[i];
-		for (uint8_t j = 0x80; j != 0; j >>= 1)
-		{
+			uint8_t pinbyte = pins[i];
+			for (uint8_t j = 0x80; j != 0; j >>= 1)
+			{
 #if (SHIFT_REGISTER_DELAY_CYCLES)
-			shift_register_delay();
+				shift_register_delay();
 #endif
-			mcu_clear_output(SHIFT_REGISTER_CLK);
+				mcu_clear_output(SHIFT_REGISTER_CLK);
 #if (IC74HC595_COUNT > 0)
-			// write
-			if (pinbyte & j)
-			{
-				mcu_set_output(SHIFT_REGISTER_SDO);
-			}
-			else
-			{
-				mcu_clear_output(SHIFT_REGISTER_SDO);
-			}
+				// write
+				if (pinbyte & j)
+				{
+					mcu_set_output(SHIFT_REGISTER_SDO);
+				}
+				else
+				{
+					mcu_clear_output(SHIFT_REGISTER_SDO);
+				}
 #endif
 // read
 #if (IC74HC165_COUNT > 0)
-			if (mcu_get_input(SHIFT_REGISTER_SDI))
-			{
-				pinbyte |= j;
-			}
-			else
-			{
-				pinbyte &= ~j;
+				if (mcu_get_input(SHIFT_REGISTER_SDI))
+				{
+					pinbyte |= j;
+				}
+				else
+				{
+					pinbyte &= ~j;
+				}
+#endif
+				mcu_set_output(SHIFT_REGISTER_CLK);
 			}
-#endif
-			mcu_set_output(SHIFT_REGISTER_CLK);
-		}
 
 #if (IC74HC165_COUNT > 0)
-		pins[i] = pinbyte;
+			pins[i] = pinbyte;
 #endif
 
 #endif
-	}
+		}
 #if (SHIFT_REGISTER_DELAY_CYCLES)
-	shift_register_delay();
+		shift_register_delay();
 #endif
 #if (IC74HC165_COUNT > 0)
-	memcpy(ic74hc165_io_pins, (const void *)pins, IC74HC165_COUNT);
+		memcpy((void *)ic74hc165_io_pins, (const void *)pins, IC74HC165_COUNT);
 #endif
-	mcu_set_output(IC74HC595_LATCH);
-	mcu_set_output(SHIFT_REGISTER_CLK);
+		mcu_set_output(IC74HC595_LATCH);
+		mcu_set_output(SHIFT_REGISTER_CLK);
+	}
 }
 #else
 MCU_CALLBACK void __attribute__((weak)) shift_register_io_pins(void)
diff --git a/uCNC/src/utils.h b/uCNC/src/utils.h
index a86f5c24a..96af9bd28 100644
--- a/uCNC/src/utils.h
+++ b/uCNC/src/utils.h
@@ -229,14 +229,20 @@ extern "C"
 #endif
 
 #ifndef DECL_MUTEX
-#define MUTEX_CLEANUP(name) \
-	static void name##_mutex_cleanup(uint8_t *m) { name##_mutex_lock = *m; }
+#define MUTEX_CLEANUP(name)                    \
+	static void name##_mutex_cleanup(uint8_t *m) \
+	{                                            \
+		if (!*m /*can unlock*/)                    \
+		{                                          \
+			name##_mutex_lock = 0;                   \
+		}                                          \
+	}
 #define DECL_MUTEX(name)                     \
 	static volatile uint8_t name##_mutex_lock; \
 	MUTEX_CLEANUP(name)
-#define MUTEX_RELEASE(name) name##_mutex_lock = 0
-#define MUTEX_LOCK_OR_EXIT(name)                                                    \
-	uint8_t __attribute__((__cleanup__(name##_mutex_cleanup))) name##_mutex_temp = 0; \
+#define MUTEX_INIT(name) uint8_t __attribute__((__cleanup__(name##_mutex_cleanup))) name##_mutex_temp = 0
+#define MUTEX_RELEASE(name) if(!name##_mutex_temp /*has the lock*/) name##_mutex_lock = 0
+#define MUTEX_TAKE(name)                                                            \
 	__ATOMIC__                                                                        \
 	{                                                                                 \
 		name##_mutex_temp = name##_mutex_lock;                                          \
@@ -244,13 +250,10 @@ extern "C"
 		{                                                                               \
 			name##_mutex_lock = 1;                                                        \
 		}                                                                               \
-		else                                                                            \
-		{                                                                               \
-			return;                                                                       \
-		}                                                                               \
-	}
-#define MUTEX_WAIT(name, timeout_us)                                           \
-	uint8_t __attribute__((__cleanup__(name##_mutex_cleanup))) name##_mutex_temp = 0; \
+	}                                                                                 \
+	if (!name##_mutex_temp /*the lock was aquired*/)
+
+#define MUTEX_WAIT(name, timeout_us)                                                \
 	__TIMEOUT_US__(timeout_us)                                                        \
 	{                                                                                 \
 		__ATOMIC__                                                                      \
@@ -259,17 +262,11 @@ extern "C"
 			if (!name##_mutex_temp)                                                       \
 			{                                                                             \
 				name##_mutex_lock = 1;                                                      \
-			}                                                                             \
-			else                                                                          \
-			{                                                                             \
-				return;                                                                     \
+				break;                                                                      \
 			}                                                                             \
 		}                                                                               \
-		if (!name##_mutex_temp)                                                         \
-		{                                                                               \
-			break;                                                                        \
-		}                                                                               \
-	}
+	}                                                                                 \
+	if (!name##_mutex_temp && timeout_us != 0 /*the lock was aquired in time*/)
 #endif
 
 #define __STRGIFY__(s) #s