diff --git a/Firmware/Configuration.h b/Firmware/Configuration.h
index 3bed653b7..5959c7e91 100644
--- a/Firmware/Configuration.h
+++ b/Firmware/Configuration.h
@@ -7,8 +7,8 @@
 #define STR(x) STR_HELPER(x)
 
 // Firmware version
-#define FW_VERSION "3.5.0"
-#define FW_COMMIT_NR   1749
+#define FW_VERSION "3.5.1"
+#define FW_COMMIT_NR   1778
 // FW_VERSION_UNKNOWN means this is an unofficial build.
 // The firmware should only be checked into github with this symbol.
 #define FW_DEV_VERSION FW_VERSION_UNKNOWN
diff --git a/Firmware/Marlin_main.cpp b/Firmware/Marlin_main.cpp
index f799a2623..49e20b69d 100644
--- a/Firmware/Marlin_main.cpp
+++ b/Firmware/Marlin_main.cpp
@@ -2813,10 +2813,15 @@ bool gcode_M45(bool onlyZ, int8_t verbosity_level)
 			lcd_puts_P(_T(MSG_FIND_BED_OFFSET_AND_SKEW_LINE2));
 		}
 			
+		bool endstops_enabled  = enable_endstops(false);
+        current_position[Z_AXIS] -= 1; //move 1mm down with disabled endstop
+        plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], homing_feedrate[Z_AXIS] / 40, active_extruder);
+        st_synchronize();
+
 		// Move the print head close to the bed.
 		current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
 
-		bool endstops_enabled  = enable_endstops(true);
+		enable_endstops(true);
 #ifdef TMC2130
 		tmc2130_home_enter(Z_AXIS_MASK);
 #endif //TMC2130
@@ -6533,6 +6538,7 @@ if((eSoundMode==e_SOUND_MODE_LOUD)||(eSoundMode==e_SOUND_MODE_ONCE))
     #endif //FILAMENTCHANGEENABLE
 	case 601: //! M601 - Pause print
 	{
+		cmdqueue_pop_front(); //trick because we want skip this command (M601) after restore
 		lcd_pause_print();
 	}
 	break;
diff --git a/Firmware/mmu.cpp b/Firmware/mmu.cpp
index 1c41284ca..af8f6676e 100644
--- a/Firmware/mmu.cpp
+++ b/Firmware/mmu.cpp
@@ -57,6 +57,7 @@ int16_t mmu_buildnr = -1;
 uint32_t mmu_last_request = 0;
 uint32_t mmu_last_response = 0;
 
+uint8_t mmu_last_cmd = 0;
 uint16_t mmu_power_failures = 0;
 
 
@@ -293,6 +294,7 @@ void mmu_loop(void)
 				mmu_puts_P(PSTR("S3\n")); //send power failures request
 				mmu_state = 4; // power failures response
 			}
+			mmu_last_cmd = mmu_cmd;
 			mmu_cmd = 0;
 		}
 		else if ((mmu_last_response + 300) < millis()) //request every 300ms
@@ -350,11 +352,20 @@ void mmu_loop(void)
 #ifdef MMU_DEBUG
 			printf_P(PSTR("MMU => 'ok'\n"));
 #endif //MMU_DEBUG
+			mmu_last_cmd = 0;
 			mmu_ready = true;
 			mmu_state = 1;
 		}
 		else if ((mmu_last_request + MMU_CMD_TIMEOUT) < millis())
 		{ //resend request after timeout (5 min)
+			if (mmu_last_cmd)
+			{
+#ifdef MMU_DEBUG
+				printf_P(PSTR("MMU retry\n"));
+#endif //MMU_DEBUG
+				mmu_cmd = mmu_last_cmd;
+//				mmu_last_cmd = 0; //resend just once
+			}
 			mmu_state = 1;
 		}
 		return;
diff --git a/Firmware/ultralcd.cpp b/Firmware/ultralcd.cpp
index 6501ca244..0a59e006f 100644
--- a/Firmware/ultralcd.cpp
+++ b/Firmware/ultralcd.cpp
@@ -2409,7 +2409,7 @@ void lcd_alright() {
 
 
   enc_dif = lcd_encoder_diff;
-
+  lcd_consume_click();
   while (lcd_change_fil_state == 0) {
 
     manage_heater();
@@ -6148,6 +6148,7 @@ static void lcd_sd_updir()
 
 void lcd_print_stop()
 {
+	saved_printing = false;
 	cancel_heatup = true;
 #ifdef MESH_BED_LEVELING
 	mbl.active = false;
@@ -6181,7 +6182,7 @@ void lcd_print_stop()
 
 void lcd_sdcard_stop()
 {
-	
+
 	lcd_set_cursor(0, 0);
 	lcd_puts_P(_T(MSG_STOP_PRINT));
 	lcd_set_cursor(2, 2);