diff --git a/Firmware/Marlin_main.cpp b/Firmware/Marlin_main.cpp
index 6f251d28f..17ff10ff6 100644
--- a/Firmware/Marlin_main.cpp
+++ b/Firmware/Marlin_main.cpp
@@ -1142,10 +1142,6 @@ void list_sec_lang_from_external_flash()
 // are initialized by the main() routine provided by the Arduino framework.
 void setup()
 {
-#ifdef W25X20CL
-  // Enter an STK500 compatible Optiboot boot loader waiting for flashing the languages to an external flash memory.
-  optiboot_w25x20cl_enter();
-#endif
     lcd_init();
 	fdev_setup_stream(lcdout, lcd_putchar, NULL, _FDEV_SETUP_WRITE); //setup lcdout stream
 
@@ -1153,6 +1149,11 @@ void setup()
 
 	lcd_splash();
 
+#ifdef W25X20CL
+  // Enter an STK500 compatible Optiboot boot loader waiting for flashing the languages to an external flash memory.
+  optiboot_w25x20cl_enter();
+#endif
+
 #if (LANG_MODE != 0) //secondary language support
 #ifdef W25X20CL
 	if (w25x20cl_init())
@@ -3187,6 +3188,13 @@ static void gcode_PRUSA_SN()
     }
 }
 
+#ifdef BACKLASH_X
+extern uint8_t st_backlash_x;
+#endif //BACKLASH_X
+#ifdef BACKLASH_Y
+extern uint8_t st_backlash_y;
+#endif //BACKLASH_Y
+
 void process_commands()
 {
 	if (!buflen) return; //empty command
@@ -3297,6 +3305,22 @@ void process_commands()
 			}
 		}
 	}
+#ifdef BACKLASH_X
+	else if (strncmp_P(CMDBUFFER_CURRENT_STRING, PSTR("BACKLASH_X"), 10) == 0)
+	{
+		uint8_t bl = (uint8_t)strtol(CMDBUFFER_CURRENT_STRING + 10, NULL, 10);
+		st_backlash_x = bl;
+		printf_P(_N("st_backlash_x = %hhd\n"), st_backlash_x);
+	}
+#endif //BACKLASH_X
+#ifdef BACKLASH_Y
+	else if (strncmp_P(CMDBUFFER_CURRENT_STRING, PSTR("BACKLASH_Y"), 10) == 0)
+	{
+		uint8_t bl = (uint8_t)strtol(CMDBUFFER_CURRENT_STRING + 10, NULL, 10);
+		st_backlash_y = bl;
+		printf_P(_N("st_backlash_y = %hhd\n"), st_backlash_y);
+	}
+#endif //BACKLASH_Y
 #endif //TMC2130
 
   else if(code_seen("PRUSA")){
diff --git a/Firmware/stepper.cpp b/Firmware/stepper.cpp
index 902ec7bca..90a49c6b3 100644
--- a/Firmware/stepper.cpp
+++ b/Firmware/stepper.cpp
@@ -407,12 +407,68 @@ ISR(TIMER1_COMPA_vect) {
   }
 }
 
+uint8_t last_dir_bits = 0;
+
+#ifdef BACKLASH_X
+uint8_t st_backlash_x = 0;
+#endif //BACKLASH_X
+#ifdef BACKLASH_Y
+uint8_t st_backlash_y = 0;
+#endif //BACKLASH_Y
+
 FORCE_INLINE void stepper_next_block()
 {
   // Anything in the buffer?
   //WRITE_NC(LOGIC_ANALYZER_CH2, true);
   current_block = plan_get_current_block();
   if (current_block != NULL) {
+#ifdef BACKLASH_X
+	if (current_block->steps_x.wide)
+	{ //X-axis movement
+		if ((current_block->direction_bits ^ last_dir_bits) & 1)
+		{
+			printf_P(PSTR("BL %d\n"), (current_block->direction_bits & 1)?st_backlash_x:-st_backlash_x);
+			if (current_block->direction_bits & 1)
+				WRITE_NC(X_DIR_PIN, INVERT_X_DIR);
+			else
+				WRITE_NC(X_DIR_PIN, !INVERT_X_DIR);
+			_delay_us(100);
+			for (uint8_t i = 0; i < st_backlash_x; i++)
+			{
+				WRITE_NC(X_STEP_PIN, !INVERT_X_STEP_PIN);
+				_delay_us(100);
+				WRITE_NC(X_STEP_PIN, INVERT_X_STEP_PIN);
+				_delay_us(900);
+			}
+		}
+		last_dir_bits &= ~1;
+		last_dir_bits |= current_block->direction_bits & 1;
+	}
+#endif
+#ifdef BACKLASH_Y
+	if (current_block->steps_y.wide)
+	{ //Y-axis movement
+		if ((current_block->direction_bits ^ last_dir_bits) & 2)
+		{
+			printf_P(PSTR("BL %d\n"), (current_block->direction_bits & 2)?st_backlash_y:-st_backlash_y);
+			if (current_block->direction_bits & 2)
+				WRITE_NC(Y_DIR_PIN, INVERT_Y_DIR);
+			else
+				WRITE_NC(Y_DIR_PIN, !INVERT_Y_DIR);
+			_delay_us(100);
+			for (uint8_t i = 0; i < st_backlash_y; i++)
+			{
+				WRITE_NC(Y_STEP_PIN, !INVERT_Y_STEP_PIN);
+				_delay_us(100);
+				WRITE_NC(Y_STEP_PIN, INVERT_Y_STEP_PIN);
+				_delay_us(900);
+			}
+		}
+		last_dir_bits &= ~2;
+		last_dir_bits |= current_block->direction_bits & 2;
+	}
+#endif
+
 #ifdef PAT9125
     fsensor_counter = 0;
     fsensor_st_block_begin(current_block);
@@ -996,6 +1052,7 @@ FORCE_INLINE void isr() {
       fsensor_st_block_chunk(current_block, fsensor_counter);
 	    fsensor_counter = 0;
 #endif //PAT9125
+
       current_block = NULL;
       plan_discard_current_block();
     }
diff --git a/Firmware/tmc2130.cpp b/Firmware/tmc2130.cpp
index bfc720c6d..10407120c 100644
--- a/Firmware/tmc2130.cpp
+++ b/Firmware/tmc2130.cpp
@@ -453,7 +453,7 @@ void tmc2130_setup_chopper(uint8_t axis, uint8_t mres, uint8_t current_h, uint8_
 	}
 	else
 	{
-		tmc2130_wr_CHOPCONF(axis, toff, hstrt, hend, fd3, 0, 0, 0, tbl, 0, 0, 0, 0, mres, intpol, 0, 0);
+		tmc2130_wr_CHOPCONF(axis, toff, hstrt, hend, fd3, 0, rndtf, chm, tbl, 0, 0, 0, 0, mres, intpol, 0, 0);
 		tmc2130_wr(axis, TMC2130_REG_IHOLD_IRUN, 0x000f0000 | (((current_r >> 1) & 0x1f) << 8) | ((current_h >> 1) & 0x1f));
 	}
 }
@@ -868,7 +868,7 @@ void tmc2130_set_wave(uint8_t axis, uint8_t amp, uint8_t fac1000)
 	if (fac1000 < TMC2130_WAVE_FAC1000_MIN) fac1000 = 0;
 	if (fac1000 > TMC2130_WAVE_FAC1000_MAX) fac1000 = TMC2130_WAVE_FAC1000_MAX;
 	float fac = 0;
-	if (fac1000) fac = (float)((uint16_t)fac1000 + 1000) / 1000; //correction factor
+	if (fac1000) fac = ((float)((uint16_t)fac1000 + 1000) / 1000); //correction factor
 	printf_P(PSTR(" factor: %s\n"), ftostr43(fac));
 	uint8_t vA = 0;                //value of currentA
 	uint8_t va = 0;                //previous vA
diff --git a/Firmware/ultralcd.cpp b/Firmware/ultralcd.cpp
index 832ecfa71..e38d80927 100644
--- a/Firmware/ultralcd.cpp
+++ b/Firmware/ultralcd.cpp
@@ -5830,7 +5830,7 @@ static void lcd_main_menu()
 
   MENU_ITEM_SUBMENU_P(_i("Support"), lcd_support_menu);////MSG_SUPPORT c=0 r=0
 
-  MENU_ITEM_SUBMENU_P(_i("W25x20CL init"), lcd_test_menu);////MSG_SUPPORT c=0 r=0
+//  MENU_ITEM_SUBMENU_P(_i("W25x20CL init"), lcd_test_menu);////MSG_SUPPORT c=0 r=0
 
   MENU_END();
 
diff --git a/Firmware/variants/1_75mm_MK3-EINSy10a-E3Dv6full.h b/Firmware/variants/1_75mm_MK3-EINSy10a-E3Dv6full.h
index 2186c4aec..8048b8297 100644
--- a/Firmware/variants/1_75mm_MK3-EINSy10a-E3Dv6full.h
+++ b/Firmware/variants/1_75mm_MK3-EINSy10a-E3Dv6full.h
@@ -135,6 +135,9 @@
 // Filament sensor
 #define PAT9125
 
+// Backlash - 
+//#define BACKLASH_X
+//#define BACKLASH_Y
 
 // Disable some commands
 #define _DISABLE_M42_M226