diff --git a/Firmware/Marlin.h b/Firmware/Marlin.h
index 8050ab54..8d6eed16 100644
--- a/Firmware/Marlin.h
+++ b/Firmware/Marlin.h
@@ -214,7 +214,7 @@ void FlushSerialRequestResend();
 void ClearToSend();
 
 void get_coordinates();
-void prepare_move(uint8_t sdlen = 0);
+void prepare_move();
 void kill(const char *full_screen_message = NULL, unsigned char id = 0);
 void Stop();
 
diff --git a/Firmware/Marlin_main.cpp b/Firmware/Marlin_main.cpp
index 5d183f17..b1ba7ef5 100644
--- a/Firmware/Marlin_main.cpp
+++ b/Firmware/Marlin_main.cpp
@@ -441,6 +441,8 @@ unsigned long _usb_timer = 0;
 
 static uint8_t tmp_extruder;
 
+bool extruder_under_pressure = true;
+
 
 bool Stopped=false;
 
@@ -1150,8 +1152,18 @@ void loop()
     #else
       process_commands();
     #endif //SDSUPPORT
+
       if (! cmdbuffer_front_already_processed)
-          cmdqueue_pop_front();
+	  {
+		cli();
+    	uint8_t sdlen = 0;
+		if (CMDBUFFER_CURRENT_TYPE == CMDBUFFER_CURRENT_TYPE_SDCARD)
+			sdlen = cmdbuffer[bufindr + 1];
+	    cmdqueue_pop_front();
+		if (sdlen)
+			planner_add_sd_length(sdlen);
+		sei();
+	  }
       cmdbuffer_front_already_processed = false;
   }
 }
@@ -1983,10 +1995,7 @@ void process_commands()
 
             }
           #endif //FWRETRACT
-		uint8_t sdlen = 0;
-		if (CMDBUFFER_CURRENT_TYPE == CMDBUFFER_CURRENT_TYPE_SDCARD)
-			sdlen = cmdbuffer[bufindr + 1];
-        prepare_move(sdlen);
+        prepare_move();
         //ClearToSend();
       }
       break;
@@ -5759,7 +5768,7 @@ void clamp_to_software_endstops(float target[3])
 }
 
 #ifdef MESH_BED_LEVELING
-    void mesh_plan_buffer_line(const float &x, const float &y, const float &z, const float &e, const float &feed_rate, const uint8_t extruder, uint8_t sdlen) {
+    void mesh_plan_buffer_line(const float &x, const float &y, const float &z, const float &e, const float &feed_rate, const uint8_t extruder) {
         float dx = x - current_position[X_AXIS];
         float dy = y - current_position[Y_AXIS];
         float dz = z - current_position[Z_AXIS];
@@ -5793,20 +5802,20 @@ void clamp_to_software_endstops(float target[3])
     }
 #endif  // MESH_BED_LEVELING
     
-void prepare_move(uint8_t sdlen)
+void prepare_move()
 {
   clamp_to_software_endstops(destination);
   previous_millis_cmd = millis();
 
   // Do not use feedmultiply for E or Z only moves
   if( (current_position[X_AXIS] == destination [X_AXIS]) && (current_position[Y_AXIS] == destination [Y_AXIS])) {
-      plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder, sdlen);
+      plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
   }
   else {
 #ifdef MESH_BED_LEVELING
-    mesh_plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate*feedmultiply*(1./(60.f*100.f)), active_extruder, sdlen);
+    mesh_plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate*feedmultiply*(1./(60.f*100.f)), active_extruder);
 #else
-     plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate*feedmultiply*(1./(60.f*100.f)), active_extruder, sdlen);
+     plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate*feedmultiply*(1./(60.f*100.f)), active_extruder);
 #endif
   }
 
@@ -6880,16 +6889,21 @@ void restore_print_from_eeprom() {
 ////////////////////////////////////////////////////////////////////////////////
 // new save/restore printing
 
+extern uint32_t sdpos_atomic;
+
 bool saved_printing = false;
 uint32_t saved_sdpos = 0;
 uint32_t saved_pos[4] = {0, 0, 0, 0};
+float saved_feedrate2 = 0;
+uint8_t saved_active_extruder = 0;
+bool saved_extruder_under_pressure = false;
+
 
 void stop_and_save_print_to_ram()
 {
 	if (saved_printing) return;
 	cli();
-	uint32_t sdpos = card.get_sdpos();
-	saved_sdpos = sdpos;
+	saved_sdpos = sdpos_atomic;
 	uint16_t sdlen_planner = planner_calc_sd_length();
 	saved_sdpos -= sdlen_planner;
 	uint16_t sdlen_cmdqueue = cmdqueue_calc_sd_length();
@@ -6898,14 +6912,19 @@ void stop_and_save_print_to_ram()
 //	babystep_reset();
 	for (int axis = X_AXIS; axis <= E_AXIS; axis++)
 		saved_pos[axis] = current_position[axis];
+	saved_feedrate2 = feedrate;
+	saved_active_extruder = active_extruder;
+	saved_extruder_under_pressure = extruder_under_pressure;
 	cmdqueue_reset();
 	card.sdprinting = false;
 //	card.closefile();
 	saved_printing = true;
 	sei();
-	plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS] + 10, current_position[E_AXIS], homing_feedrate[Z_AXIS], active_extruder);
+	float extruder_move = 0;
+	if (extruder_under_pressure) extruder_move -= 0.8;
+	plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS] + 10, current_position[E_AXIS] + extruder_move, homing_feedrate[Z_AXIS], active_extruder);
     st_synchronize();
-	MYSERIAL.print("SDPOS="); MYSERIAL.println(sdpos, DEC);
+	MYSERIAL.print("SDPOS="); MYSERIAL.println(sdpos_atomic, DEC);
 	MYSERIAL.print("SDLEN_PLAN="); MYSERIAL.println(sdlen_planner, DEC);
 	MYSERIAL.print("SDLEN_CMDQ="); MYSERIAL.println(sdlen_cmdqueue, DEC);
 
@@ -6915,8 +6934,14 @@ void restore_print_from_ram_and_continue()
 {
 	if (!saved_printing) return;
 //	babystep_apply();
-	plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], homing_feedrate[Z_AXIS], active_extruder);
+	for (int axis = X_AXIS; axis <= E_AXIS; axis++)
+	    current_position[axis] = st_get_position_mm(axis);
+	active_extruder = saved_active_extruder;
+	float extruder_move = 0;
+	if (saved_extruder_under_pressure) extruder_move += 0.8;
+	plan_buffer_line(saved_pos[X_AXIS], saved_pos[Y_AXIS], saved_pos[Z_AXIS], current_position[E_AXIS] + extruder_move, homing_feedrate[Z_AXIS], active_extruder);
     st_synchronize();
+	feedrate = saved_feedrate2;
 	card.setIndex(saved_sdpos);
 	card.sdprinting = true;
 	saved_printing = false;
diff --git a/Firmware/cmdqueue.cpp b/Firmware/cmdqueue.cpp
index cd0a86b9..252af7f7 100644
--- a/Firmware/cmdqueue.cpp
+++ b/Firmware/cmdqueue.cpp
@@ -29,6 +29,8 @@ long gcode_N = 0;
 long gcode_LastN = 0;
 long Stopped_gcode_LastN = 0;
 
+uint32_t sdpos_atomic = 0;
+
 
 // Pop the currently processed command from the queue.
 // It is expected, that there is at least one command in the queue.
@@ -572,7 +574,10 @@ void get_command()
         return; //if empty line
       }
       cmdbuffer[bufindw+serial_count+CMDHDRSIZE] = 0; //terminate string
-      cmdbuffer[bufindw] = CMDBUFFER_CURRENT_TYPE_SDCARD;
+	  uint8_t len = strlen(cmdbuffer+bufindw+CMDHDRSIZE) + (1 + CMDHDRSIZE);
+
+	  cli();
+	  cmdbuffer[bufindw] = CMDBUFFER_CURRENT_TYPE_SDCARD;
       cmdbuffer[bufindw+1] = sd_count;
 /*	  SERIAL_ECHOPGM("SD cmd(");
 	  MYSERIAL.print(sd_count, DEC);
@@ -583,10 +588,13 @@ void get_command()
       ++ buflen;
 //	  SERIAL_ECHOPGM("buflen:");
 //	  MYSERIAL.print(buflen);
-      bufindw += strlen(cmdbuffer+bufindw+CMDHDRSIZE) + (1 + CMDHDRSIZE);
+      bufindw += len;
+	  sdpos_atomic = card.get_sdpos();
       if (bufindw == sizeof(cmdbuffer))
           bufindw = 0;
-      comment_mode = false; //for new command
+	  sei();
+
+	  comment_mode = false; //for new command
       serial_count = 0; //clear buffer
       // The following line will reserve buffer space if available.
       if (! cmdqueue_could_enqueue_back(MAX_CMD_SIZE-1))
diff --git a/Firmware/planner.cpp b/Firmware/planner.cpp
index 025027a5..e5eb41db 100644
--- a/Firmware/planner.cpp
+++ b/Firmware/planner.cpp
@@ -593,7 +593,7 @@ float junction_deviation = 0.1;
 // Add a new linear movement to the buffer. steps_x, _y and _z is the absolute position in 
 // mm. Microseconds specify how many microseconds the move should take to perform. To aid acceleration
 // calculation the caller must also provide the physical length of the line in millimeters.
-void plan_buffer_line(float x, float y, float z, const float &e, float feed_rate, const uint8_t &extruder, uint8_t sdlen)
+void plan_buffer_line(float x, float y, float z, const float &e, float feed_rate, const uint8_t &extruder)
 {
     // Calculate the buffer head after we push this byte
   int next_buffer_head = next_block_index(block_buffer_head);
@@ -724,7 +724,7 @@ void plan_buffer_line(float x, float y, float z, const float &e, float feed_rate
   block_t *block = &block_buffer[block_buffer_head];
 
   // Set sdlen for calculating sd position
-  block->sdlen = sdlen;
+  block->sdlen = 0;
 
   // Mark block as not busy (Not executed by the stepper interrupt, could be still tinkered with.)
   block->busy = false;
diff --git a/Firmware/planner.h b/Firmware/planner.h
index 5fa06888..0dd831bb 100644
--- a/Firmware/planner.h
+++ b/Firmware/planner.h
@@ -113,12 +113,12 @@ void plan_init();
 // millimaters. Feed rate specifies the speed of the motion.
 
 #ifdef ENABLE_AUTO_BED_LEVELING
-void plan_buffer_line(float x, float y, float z, const float &e, float feed_rate, const uint8_t &extruder, uint8_t sdlen = 0);
+void plan_buffer_line(float x, float y, float z, const float &e, float feed_rate, const uint8_t &extruder);
 
 // Get the position applying the bed level matrix if enabled
 vector_3 plan_get_position();
 #else
-void plan_buffer_line(float x, float y, float z, const float &e, float feed_rate, const uint8_t &extruder, uint8_t sdlen = 0);
+void plan_buffer_line(float x, float y, float z, const float &e, float feed_rate, const uint8_t &extruder);
 //void plan_buffer_line(const float &x, const float &y, const float &z, const float &e, float feed_rate, const uint8_t &extruder);
 #endif // ENABLE_AUTO_BED_LEVELING