diff --git a/Firmware/Marlin.h b/Firmware/Marlin.h
index fb70dfed7..363407e2e 100755
--- a/Firmware/Marlin.h
+++ b/Firmware/Marlin.h
@@ -310,9 +310,9 @@ extern int8_t lcd_change_fil_state;
 extern float default_retraction;
 
 #ifdef TMC2130
-bool homeaxis(int axis, bool doError = true, uint8_t cnt = 1, uint8_t* pstep = 0);
+void homeaxis(int axis, uint8_t cnt = 1, uint8_t* pstep = 0);
 #else
-bool homeaxis(int axis, bool doError = true, uint8_t cnt = 1);
+void homeaxis(int axis, uint8_t cnt = 1);
 #endif //TMC2130
 
 
diff --git a/Firmware/Marlin_main.cpp b/Firmware/Marlin_main.cpp
index a6becaa66..6802815a9 100755
--- a/Firmware/Marlin_main.cpp
+++ b/Firmware/Marlin_main.cpp
@@ -2197,9 +2197,24 @@ bool calibrate_z_auto()
 #endif //TMC2130
 
 #ifdef TMC2130
-bool homeaxis(int axis, bool doError, uint8_t cnt, uint8_t* pstep)
+static void check_Z_crash(void)
+{
+	if (READ(Z_TMC2130_DIAG) != 0) { //Z crash
+		FORCE_HIGH_POWER_END;
+		current_position[Z_AXIS] = 0;
+		plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
+		current_position[Z_AXIS] += MESH_HOME_Z_SEARCH;
+		plan_buffer_line_curposXYZE(max_feedrate[Z_AXIS], active_extruder);
+		st_synchronize();
+		kill(_T(MSG_BED_LEVELING_FAILED_POINT_LOW));
+	}
+}
+#endif //TMC2130
+
+#ifdef TMC2130
+void homeaxis(int axis, uint8_t cnt, uint8_t* pstep)
 #else
-bool homeaxis(int axis, bool doError, uint8_t cnt)
+void homeaxis(int axis, uint8_t cnt)
 #endif //TMC2130
 {
 	bool endstops_enabled  = enable_endstops(true); //RP: endstops should be allways enabled durring homing
@@ -2312,13 +2327,7 @@ bool homeaxis(int axis, bool doError, uint8_t cnt)
         plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
         st_synchronize();
 #ifdef TMC2130
-		if (READ(Z_TMC2130_DIAG) != 0) { //Z crash
-			FORCE_HIGH_POWER_END;
-			if (doError) kill(_T(MSG_BED_LEVELING_FAILED_POINT_LOW));
-            current_position[axis] = -5; //assume that nozzle crashed into bed
-            plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
-			return 0; 
-		}
+        check_Z_crash();
 #endif //TMC2130
         current_position[axis] = 0;
         plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
@@ -2330,13 +2339,7 @@ bool homeaxis(int axis, bool doError, uint8_t cnt)
         plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
         st_synchronize();
 #ifdef TMC2130
-		if (READ(Z_TMC2130_DIAG) != 0) { //Z crash
-			FORCE_HIGH_POWER_END;
-			if (doError) kill(_T(MSG_BED_LEVELING_FAILED_POINT_LOW));
-            current_position[axis] = -5; //assume that nozzle crashed into bed
-            plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
-			return 0; 
-		}
+        check_Z_crash();
 #endif //TMC2130
         axis_is_at_home(axis);
         destination[axis] = current_position[axis];
@@ -2348,7 +2351,6 @@ bool homeaxis(int axis, bool doError, uint8_t cnt)
 #endif	
     }
     enable_endstops(endstops_enabled);
-    return 1;
 }
 
 /**/
diff --git a/Firmware/tmc2130.cpp b/Firmware/tmc2130.cpp
index 3a4e5b51e..108d00b1d 100755
--- a/Firmware/tmc2130.cpp
+++ b/Firmware/tmc2130.cpp
@@ -994,7 +994,7 @@ bool tmc2130_home_calibrate(uint8_t axis)
 	uint8_t step[16];
 	uint8_t cnt[16];
 	uint8_t val[16];
-	homeaxis(axis, true, 16, step);
+	homeaxis(axis, 16, step);
 	bubblesort_uint8(step, 16, 0);
 	printf_P(PSTR("sorted samples:\n"));
 	for (uint8_t i = 0; i < 16; i++)
diff --git a/Firmware/ultralcd.cpp b/Firmware/ultralcd.cpp
index daf9f0176..62c410727 100755
--- a/Firmware/ultralcd.cpp
+++ b/Firmware/ultralcd.cpp
@@ -168,10 +168,10 @@ static void reset_crash_det(unsigned char axis);
 static bool lcd_selfcheck_axis_sg(unsigned char axis);
 static bool lcd_selfcheck_axis(int _axis, int _travel);
 #else
-static bool lcd_selfcheck_endstops();
 static bool lcd_selfcheck_axis(int _axis, int _travel);
 static bool lcd_selfcheck_pulleys(int axis);
 #endif //TMC2130
+static bool lcd_selfcheck_endstops();
 
 static bool lcd_selfcheck_check_heater(bool _isbed);
 enum class TestScreen : uint_least8_t
@@ -7667,11 +7667,7 @@ bool lcd_selftest()
 	if (_result)
 	{
 		_progress = lcd_selftest_screen(TestScreen::FansOk, _progress, 3, true, 2000);
-#ifndef TMC2130
-		_result = lcd_selfcheck_endstops();
-#else
-		_result = true;
-#endif
+		_result = lcd_selfcheck_endstops(); //With TMC2130, only the Z probe is tested.
 	}
 
 	if (_result)
@@ -7738,7 +7734,7 @@ bool lcd_selftest()
         set_destination_to_current();
 		_progress = lcd_selftest_screen(TestScreen::AxisZ, _progress, 3, true, 1500);
 #ifdef TMC2130
-		_result = homeaxis(Z_AXIS, false);
+		homeaxis(Z_AXIS); //In case of failure, the code gets stuck in this function.
 #else
         _result = lcd_selfcheck_axis(Z_AXIS, Z_MAX_POS);
 #endif //TMC2130
@@ -8137,31 +8133,42 @@ static bool lcd_selfcheck_pulleys(int axis)
 	}
 	return(true);
 }
+#endif //not defined TMC2130
 
 
 static bool lcd_selfcheck_endstops()
 {
 	bool _result = true;
 
-	if (((READ(X_MIN_PIN) ^ X_MIN_ENDSTOP_INVERTING) == 1) ||
+	if (
+	#ifndef TMC2130
+		((READ(X_MIN_PIN) ^ X_MIN_ENDSTOP_INVERTING) == 1) ||
 		((READ(Y_MIN_PIN) ^ Y_MIN_ENDSTOP_INVERTING) == 1) ||
+	#endif //!TMC2130
 		((READ(Z_MIN_PIN) ^ Z_MIN_ENDSTOP_INVERTING) == 1))
 	{
+	#ifndef TMC2130
 		if ((READ(X_MIN_PIN) ^ X_MIN_ENDSTOP_INVERTING) == 1) current_position[0] += 10;
 		if ((READ(Y_MIN_PIN) ^ Y_MIN_ENDSTOP_INVERTING) == 1) current_position[1] += 10;
+	#endif //!TMC2130
 		if ((READ(Z_MIN_PIN) ^ Z_MIN_ENDSTOP_INVERTING) == 1) current_position[2] += 10;
 	}
 	plan_buffer_line_curposXYZE(manual_feedrate[0] / 60, active_extruder);
-	_delay(500);
+	st_synchronize();
 
-	if (((READ(X_MIN_PIN) ^ X_MIN_ENDSTOP_INVERTING) == 1) ||
+	if (
+	#ifndef TMC2130
+		((READ(X_MIN_PIN) ^ X_MIN_ENDSTOP_INVERTING) == 1) ||
 		((READ(Y_MIN_PIN) ^ Y_MIN_ENDSTOP_INVERTING) == 1) ||
+	#endif //!TMC2130
 		((READ(Z_MIN_PIN) ^ Z_MIN_ENDSTOP_INVERTING) == 1))
 	{
 		_result = false;
 		char _error[4] = "";
+	#ifndef TMC2130
 		if ((READ(X_MIN_PIN) ^ X_MIN_ENDSTOP_INVERTING) == 1) strcat(_error, "X");
 		if ((READ(Y_MIN_PIN) ^ Y_MIN_ENDSTOP_INVERTING) == 1) strcat(_error, "Y");
+	#endif //!TMC2130
 		if ((READ(Z_MIN_PIN) ^ Z_MIN_ENDSTOP_INVERTING) == 1) strcat(_error, "Z");
 		lcd_selftest_error(TestError::Endstops, _error, "");
 	}
@@ -8169,7 +8176,6 @@ static bool lcd_selfcheck_endstops()
 	manage_inactivity(true);
 	return _result;
 }
-#endif //not defined TMC2130
 
 static bool lcd_selfcheck_check_heater(bool _isbed)
 {