diff --git a/Marlin/Marlin.pde b/Marlin/Marlin.pde
index 07716fd35..b122e58d8 100644
--- a/Marlin/Marlin.pde
+++ b/Marlin/Marlin.pde
@@ -509,11 +509,9 @@ bool code_seen(char code)
     plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder); \
     \
     current_position[LETTER##_AXIS] = (LETTER##_HOME_DIR == -1) ? 0 : LETTER##_MAX_LENGTH;\
-    plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);\
     destination[LETTER##_AXIS] = current_position[LETTER##_AXIS];\
     feedrate = 0.0;\
     st_synchronize();\
-    plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);\
     endstops_hit_on_purpose();\
   }
 
@@ -567,7 +565,7 @@ void process_commands()
       feedrate = 0.0;
       home_all_axis = !((code_seen(axis_codes[0])) || (code_seen(axis_codes[1])) || (code_seen(axis_codes[2])));
       #ifdef QUICK_HOME
-      if( code_seen(axis_codes[0]) && code_seen(axis_codes[1]) )  //first diagonal move
+      if( code_seen(axis_codes[X_AXIS]) && code_seen(axis_codes[Y_AXIS]) )  //first diagonal move
       {
         current_position[X_AXIS] = 0;current_position[Y_AXIS] = 0;  
 
@@ -585,10 +583,10 @@ void process_commands()
         destination[Y_AXIS] = current_position[Y_AXIS];
         feedrate = 0.0;
         st_synchronize();
-        plan_set_position(0, 0, current_position[Z_AXIS], current_position[E_AXIS]);
-        current_position[X_AXIS] = 0;current_position[Y_AXIS] = 0;
         endstops_hit_on_purpose();
       }
+      else
+      {
       #endif
       
       if((home_all_axis) || (code_seen(axis_codes[X_AXIS]))) 
@@ -599,7 +597,11 @@ void process_commands()
       if((home_all_axis) || (code_seen(axis_codes[Y_AXIS]))) {
        HOMEAXIS(Y);
       }
-
+      
+      #ifdef QUICK_HOME
+      }
+      #endif
+      
       if((home_all_axis) || (code_seen(axis_codes[Z_AXIS]))) {
         HOMEAXIS(Z);
       }
@@ -616,6 +618,8 @@ void process_commands()
       if(code_seen(axis_codes[Z_AXIS])) {
         current_position[2]=code_value()+add_homeing[2];
       }
+      plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
+      
       #ifdef ENDSTOPS_ONLY_FOR_HOMING
         enable_endstops(false);
       #endif
@@ -1254,19 +1258,6 @@ void get_arc_coordinates()
 
 void prepare_move()
 {
-  
-  if (min_software_endstops) {
-    if (destination[X_AXIS] < 0) destination[X_AXIS] = 0.0;
-    if (destination[Y_AXIS] < 0) destination[Y_AXIS] = 0.0;
-    if (destination[Z_AXIS] < 0) destination[Z_AXIS] = 0.0;
-  }
-
-  if (max_software_endstops) {
-    if (destination[X_AXIS] > X_MAX_LENGTH) destination[X_AXIS] = X_MAX_LENGTH;
-    if (destination[Y_AXIS] > Y_MAX_LENGTH) destination[Y_AXIS] = Y_MAX_LENGTH;
-    if (destination[Z_AXIS] > Z_MAX_LENGTH) destination[Z_AXIS] = Z_MAX_LENGTH;
-  }
-
   plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate*feedmultiply/60/100.0, active_extruder);
   for(int8_t i=0; i < NUM_AXIS; i++) {
     current_position[i] = destination[i];
diff --git a/Marlin/planner.cpp b/Marlin/planner.cpp
index b895b95e5..b631eb45a 100644
--- a/Marlin/planner.cpp
+++ b/Marlin/planner.cpp
@@ -439,11 +439,24 @@ 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(const float &x, const float &y, const float &z, const float &e,  float feed_rate, const uint8_t &extruder)
+void plan_buffer_line(float &x, float &y, float &z, float &e, float feed_rate, uint8_t &extruder)
 {
   // Calculate the buffer head after we push this byte
   int next_buffer_head = next_block_index(block_buffer_head);
 
+  
+  if (min_software_endstops) {
+    if (x < 0) x = 0;
+    if (y < 0) y = 0;
+    if (z < 0) z = 0;
+  }
+
+  if (max_software_endstops) {
+    if (x > X_MAX_LENGTH) x = X_MAX_LENGTH;
+    if (y > Y_MAX_LENGTH) y = Y_MAX_LENGTH;
+    if (z > Z_MAX_LENGTH) z = Z_MAX_LENGTH;
+  }
+  
   // If the buffer is full: good! That means we are well ahead of the robot. 
   // Rest here until there is room in the buffer.
   while(block_buffer_tail == next_buffer_head) { 
@@ -451,7 +464,7 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa
     manage_inactivity(1); 
     LCD_STATUS;
   }
-
+  
   // The target position of the tool in absolute steps
   // Calculate target position in absolute steps
   //this should be done after the wait, because otherwise a M92 code within the gcode disrupts this calculation somehow
@@ -461,6 +474,8 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa
   target[Z_AXIS] = lround(z*axis_steps_per_unit[Z_AXIS]);     
   target[E_AXIS] = lround(e*axis_steps_per_unit[E_AXIS]);
   
+
+  
   #ifdef PREVENT_DANGEROUS_EXTRUDE
     if(target[E_AXIS]!=position[E_AXIS])
     if(degHotend(active_extruder)<EXTRUDE_MINTEMP && !allow_cold_extrude)
diff --git a/Marlin/planner.h b/Marlin/planner.h
index ac9719fbc..c90581bfd 100644
--- a/Marlin/planner.h
+++ b/Marlin/planner.h
@@ -67,7 +67,7 @@ void plan_init();
 
 // Add a new linear movement to the buffer. x, y and z is the signed, absolute target position in 
 // millimaters. Feed rate specifies the speed of the motion.
-void plan_buffer_line(const float &x, const float &y, const float &z, const float &e, float feed_rate, const uint8_t &extruder);
+void plan_buffer_line(float &x, float &y, float &z, float &e, float feed_rate, uint8_t &extruder);
 
 // Set position. Used for G92 instructions.
 void plan_set_position(const float &x, const float &y, const float &z, const float &e);