diff --git a/Marlin/Marlin.pde b/Marlin/Marlin.pde
index 3d906cc0ac..0ff3464b5e 100644
--- a/Marlin/Marlin.pde
+++ b/Marlin/Marlin.pde
@@ -513,20 +513,22 @@ bool code_seen(char code)
     destination[LETTER##_AXIS] = 1.5 * LETTER##_MAX_LENGTH * LETTER##_HOME_DIR; \
     feedrate = homing_feedrate[LETTER##_AXIS]; \
     plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder); \
+    st_synchronize();\
     \
     current_position[LETTER##_AXIS] = 0;\
     plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);\
     destination[LETTER##_AXIS] = -LETTER##_HOME_RETRACT_MM * LETTER##_HOME_DIR;\
     plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder); \
+    st_synchronize();\
     \
     destination[LETTER##_AXIS] = 2*LETTER##_HOME_RETRACT_MM * LETTER##_HOME_DIR;\
     feedrate = homing_feedrate[LETTER##_AXIS]/2 ;  \
     plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder); \
+    st_synchronize();\
     \
     current_position[LETTER##_AXIS] = (LETTER##_HOME_DIR == -1) ? LETTER##_HOME_POS : LETTER##_MAX_LENGTH;\
     destination[LETTER##_AXIS] = current_position[LETTER##_AXIS];\
     feedrate = 0.0;\
-    st_synchronize();\
     endstops_hit_on_purpose();\
   }
 
@@ -589,13 +591,15 @@ void process_commands()
         feedrate = homing_feedrate[X_AXIS]; 
         if(homing_feedrate[Y_AXIS]<feedrate)
           feedrate =homing_feedrate[Y_AXIS]; 
-        prepare_move(); 
+        plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder); \
+        st_synchronize();
     
         current_position[X_AXIS] = (X_HOME_DIR == -1) ? X_HOME_POS : X_MAX_LENGTH;
         current_position[Y_AXIS] = (Y_HOME_DIR == -1) ? Y_HOME_POS : Y_MAX_LENGTH;
         plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
         destination[X_AXIS] = current_position[X_AXIS];
         destination[Y_AXIS] = current_position[Y_AXIS];
+        plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder); \
         feedrate = 0.0;
         st_synchronize();
         endstops_hit_on_purpose();
@@ -1273,6 +1277,18 @@ void get_arc_coordinates()
 
 void prepare_move()
 {
+  if (min_software_endstops) {
+    if (destination[X_AXIS] < X_HOME_POS) destination[X_AXIS] = X_HOME_POS;
+    if (destination[Y_AXIS] < Y_HOME_POS) destination[Y_AXIS] = Y_HOME_POS;
+    if (destination[Z_AXIS] < Z_HOME_POS) destination[Z_AXIS] = Z_HOME_POS;
+  }
+
+  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/motion_control.cpp b/Marlin/motion_control.cpp
index a24b2b782c..67619b86f3 100644
--- a/Marlin/motion_control.cpp
+++ b/Marlin/motion_control.cpp
@@ -122,6 +122,18 @@ void mc_arc(float *position, float *target, float *offset, uint8_t axis_0, uint8
     arc_target[axis_1] = center_axis1 + r_axis1;
     arc_target[axis_linear] += linear_per_segment;
     arc_target[E_AXIS] += extruder_per_segment;
+
+    if (min_software_endstops) {
+      if (arc_target[X_AXIS] < X_HOME_POS) arc_target[X_AXIS] = X_HOME_POS;
+      if (arc_target[Y_AXIS] < Y_HOME_POS) arc_target[Y_AXIS] = Y_HOME_POS;
+      if (arc_target[Z_AXIS] < Z_HOME_POS) arc_target[Z_AXIS] = Z_HOME_POS;
+    }
+
+    if (max_software_endstops) {
+      if (arc_target[X_AXIS] > X_MAX_LENGTH) arc_target[X_AXIS] = X_MAX_LENGTH;
+      if (arc_target[Y_AXIS] > Y_MAX_LENGTH) arc_target[Y_AXIS] = Y_MAX_LENGTH;
+      if (arc_target[Z_AXIS] > Z_MAX_LENGTH) arc_target[Z_AXIS] = Z_MAX_LENGTH;
+    }
     plan_buffer_line(arc_target[X_AXIS], arc_target[Y_AXIS], arc_target[Z_AXIS], arc_target[E_AXIS], feed_rate, extruder);
     
   }
diff --git a/Marlin/planner.cpp b/Marlin/planner.cpp
index d78e1d9991..00fead1288 100644
--- a/Marlin/planner.cpp
+++ b/Marlin/planner.cpp
@@ -446,19 +446,6 @@ void plan_buffer_line(float &x, float &y, float &z, float &e, float feed_rate, u
   // 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 < X_HOME_POS) x = X_HOME_POS;
-    if (y < Y_HOME_POS) y = Y_HOME_POS;
-    if (z < Z_HOME_POS) z = Z_HOME_POS;
-  }
-
-  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) { 
diff --git a/Marlin/temperature.cpp b/Marlin/temperature.cpp
index 069674be90..1155d88cf4 100644
--- a/Marlin/temperature.cpp
+++ b/Marlin/temperature.cpp
@@ -851,18 +851,18 @@ ISR(TIMER0_COMPB_vect)
     for(unsigned char e = 0; e < EXTRUDERS; e++) {
        if(current_raw[e] >= maxttemp[e]) {
           target_raw[e] = 0;
+          max_temp_error(e);
           #ifndef BOGUS_TEMPERATURE_FAILSAFE_OVERRIDE
           {
-            max_temp_error(e);
             kill();;
           }
           #endif
        }
        if(current_raw[e] <= minttemp[e]) {
           target_raw[e] = 0;
+          min_temp_error(e);
           #ifndef BOGUS_TEMPERATURE_FAILSAFE_OVERRIDE
           {
-            min_temp_error(e);
             kill();
           }
           #endif