diff --git a/Marlin/planner.h b/Marlin/planner.h
index 53ac3d84470197c86f94ea7295c03c04fb4ecf65..b1e028f005a7ccc0ead32024fef14fbab5810da3 100644
--- a/Marlin/planner.h
+++ b/Marlin/planner.h
@@ -130,4 +130,14 @@ FORCE_INLINE block_t *plan_get_current_block()
   block->busy = true;
   return(block);
 }
+
+// Gets the current block. Returns NULL if buffer empty
+FORCE_INLINE bool blocks_queued() 
+{
+  if (block_buffer_head == block_buffer_tail) { 
+    return false; 
+  }
+  else
+    return true;
+}
 #endif
diff --git a/Marlin/stepper.cpp b/Marlin/stepper.cpp
index e993d73037a057126809d8bd6d65a43be2604944..2ef077346c8d0a8c3653f87a80f758ef7c094b9d 100644
--- a/Marlin/stepper.cpp
+++ b/Marlin/stepper.cpp
@@ -706,22 +706,17 @@ void st_init()
   sei();
 }
 
-#define TEMPORARY_Z_HOME_SOUND_FIX
-#ifdef TEMPORARY_Z_HOME_SOUND_FIX
-  #include <util/delay.h>
-#endif
+
+#include <util/delay.h>
 // Block until all buffered steps are executed
 void st_synchronize()
 {
-  while(plan_get_current_block()) {
+  while(current_block!=0 || blocks_queued()) {
     manage_heater();
     manage_inactivity(1);
     LCD_STATUS;
-    #ifdef TEMPORARY_Z_HOME_SOUND_FIX
-    _delay_ms(200);
-    _delay_ms(200);
-    #endif
-  }   
+    //_delay_ms(1);
+  }
 }
 
 void st_set_position(const long &x, const long &y, const long &z, const long &e)