diff --git a/grbl/.vscode/settings.json b/grbl/.vscode/settings.json
new file mode 100644
index 0000000..2143081
--- /dev/null
+++ b/grbl/.vscode/settings.json
@@ -0,0 +1,5 @@
+{
+    "files.associations": {
+        "typeinfo": "c"
+    }
+}
\ No newline at end of file
diff --git a/grbl/coolant_control.c b/grbl/coolant_control.c
index 1eebfc0..034e7b1 100644
--- a/grbl/coolant_control.c
+++ b/grbl/coolant_control.c
@@ -23,35 +23,37 @@
 
 void coolant_init()
 {
-  COOLANT_FLOOD_DDR |= (1 << COOLANT_FLOOD_BIT); // Configure as output pin
-  #ifdef ENABLE_M7
-    COOLANT_MIST_DDR |= (1 << COOLANT_MIST_BIT);
-  #endif
-  coolant_stop();
+printf("%s:%s:%d Not supported yet..\n",__FILE__,__FUNCTION__,__LINE__);
+//  COOLANT_FLOOD_DDR |= (1 << COOLANT_FLOOD_BIT); // Configure as output pin
+//  #ifdef ENABLE_M7
+//    COOLANT_MIST_DDR |= (1 << COOLANT_MIST_BIT);
+//  #endif
+//  coolant_stop();
 }
 
 
 // Returns current coolant output state. Overrides may alter it from programmed state.
 uint8_t coolant_get_state()
 {
-  uint8_t cl_state = COOLANT_STATE_DISABLE;
-  #ifdef INVERT_COOLANT_FLOOD_PIN
-    if (bit_isfalse(COOLANT_FLOOD_PORT,(1 << COOLANT_FLOOD_BIT))) {
-  #else
-    if (bit_istrue(COOLANT_FLOOD_PORT,(1 << COOLANT_FLOOD_BIT))) {
-  #endif
-    cl_state |= COOLANT_STATE_FLOOD;
-  }
-  #ifdef ENABLE_M7
-    #ifdef INVERT_COOLANT_MIST_PIN
-      if (bit_isfalse(COOLANT_MIST_PORT,(1 << COOLANT_MIST_BIT))) {
-    #else
-      if (bit_istrue(COOLANT_MIST_PORT,(1 << COOLANT_MIST_BIT))) {
-    #endif
-      cl_state |= COOLANT_STATE_MIST;
-    }
-  #endif
-  return(cl_state);
+  printf("%s:%s:%d Not supported yet..\n",__FILE__,__FUNCTION__,__LINE__);
+//  uint8_t cl_state = COOLANT_STATE_DISABLE;
+//  #ifdef INVERT_COOLANT_FLOOD_PIN
+//    if (bit_isfalse(COOLANT_FLOOD_PORT,(1 << COOLANT_FLOOD_BIT))) {
+//  #else
+//    if (bit_istrue(COOLANT_FLOOD_PORT,(1 << COOLANT_FLOOD_BIT))) {
+//  #endif
+//    cl_state |= COOLANT_STATE_FLOOD;
+//  }
+//  #ifdef ENABLE_M7
+//    #ifdef INVERT_COOLANT_MIST_PIN
+//      if (bit_isfalse(COOLANT_MIST_PORT,(1 << COOLANT_MIST_BIT))) {
+//    #else
+//      if (bit_istrue(COOLANT_MIST_PORT,(1 << COOLANT_MIST_BIT))) {
+//    #endif
+//      cl_state |= COOLANT_STATE_MIST;
+//    }
+//  #endif
+//  return(cl_state);
 }
 
 
@@ -59,18 +61,20 @@ uint8_t coolant_get_state()
 // an interrupt-level. No report flag set, but only called by routines that don't need it.
 void coolant_stop()
 {
-  #ifdef INVERT_COOLANT_FLOOD_PIN
-    COOLANT_FLOOD_PORT |= (1 << COOLANT_FLOOD_BIT);
-  #else
-    COOLANT_FLOOD_PORT &= ~(1 << COOLANT_FLOOD_BIT);
-  #endif
-  #ifdef ENABLE_M7
-    #ifdef INVERT_COOLANT_MIST_PIN
-      COOLANT_MIST_PORT |= (1 << COOLANT_MIST_BIT);
-    #else
-      COOLANT_MIST_PORT &= ~(1 << COOLANT_MIST_BIT);
-    #endif
-  #endif
+  printf("%s:%s:%d Not supported yet..\n",__FILE__,__FUNCTION__,__LINE__);
+
+//  #ifdef INVERT_COOLANT_FLOOD_PIN
+//    COOLANT_FLOOD_PORT |= (1 << COOLANT_FLOOD_BIT);
+//  #else
+//    COOLANT_FLOOD_PORT &= ~(1 << COOLANT_FLOOD_BIT);
+//  #endif
+//  #ifdef ENABLE_M7
+//    #ifdef INVERT_COOLANT_MIST_PIN
+//      COOLANT_MIST_PORT |= (1 << COOLANT_MIST_BIT);
+//    #else
+//      COOLANT_MIST_PORT &= ~(1 << COOLANT_MIST_BIT);
+//    #endif
+//  #endif
 }
 
 
@@ -80,39 +84,41 @@ void coolant_stop()
 // parser program end, and g-code parser coolant_sync().
 void coolant_set_state(uint8_t mode)
 {
-  if (sys.abort) { return; } // Block during abort.  
-  
-	if (mode & COOLANT_FLOOD_ENABLE) {
-		#ifdef INVERT_COOLANT_FLOOD_PIN
-			COOLANT_FLOOD_PORT &= ~(1 << COOLANT_FLOOD_BIT);
-		#else
-			COOLANT_FLOOD_PORT |= (1 << COOLANT_FLOOD_BIT);
-		#endif
-	} else {
-	  #ifdef INVERT_COOLANT_FLOOD_PIN
-			COOLANT_FLOOD_PORT |= (1 << COOLANT_FLOOD_BIT);
-		#else
-			COOLANT_FLOOD_PORT &= ~(1 << COOLANT_FLOOD_BIT);
-		#endif
-	}
-  
-	#ifdef ENABLE_M7
-		if (mode & COOLANT_MIST_ENABLE) {
-			#ifdef INVERT_COOLANT_MIST_PIN
-				COOLANT_MIST_PORT &= ~(1 << COOLANT_MIST_BIT);
-			#else
-				COOLANT_MIST_PORT |= (1 << COOLANT_MIST_BIT);
-			#endif
-		} else {
-			#ifdef INVERT_COOLANT_MIST_PIN
-				COOLANT_MIST_PORT |= (1 << COOLANT_MIST_BIT);
-			#else
-				COOLANT_MIST_PORT &= ~(1 << COOLANT_MIST_BIT);
-			#endif
-		}
-	#endif
-	
-  sys.report_ovr_counter = 0; // Set to report change immediately
+  printf("%s:%s:%d Not supported yet..\n",__FILE__,__FUNCTION__,__LINE__);
+
+//  if (sys.abort) { return; } // Block during abort.  
+//  
+//	if (mode & COOLANT_FLOOD_ENABLE) {
+//		#ifdef INVERT_COOLANT_FLOOD_PIN
+//			COOLANT_FLOOD_PORT &= ~(1 << COOLANT_FLOOD_BIT);
+//		#else
+//			COOLANT_FLOOD_PORT |= (1 << COOLANT_FLOOD_BIT);
+//		#endif
+//	} else {
+//	  #ifdef INVERT_COOLANT_FLOOD_PIN
+//			COOLANT_FLOOD_PORT |= (1 << COOLANT_FLOOD_BIT);
+//		#else
+//			COOLANT_FLOOD_PORT &= ~(1 << COOLANT_FLOOD_BIT);
+//		#endif
+//	}
+//  
+//	#ifdef ENABLE_M7
+//		if (mode & COOLANT_MIST_ENABLE) {
+//			#ifdef INVERT_COOLANT_MIST_PIN
+//				COOLANT_MIST_PORT &= ~(1 << COOLANT_MIST_BIT);
+//			#else
+//				COOLANT_MIST_PORT |= (1 << COOLANT_MIST_BIT);
+//			#endif
+//		} else {
+//			#ifdef INVERT_COOLANT_MIST_PIN
+//				COOLANT_MIST_PORT |= (1 << COOLANT_MIST_BIT);
+//			#else
+//				COOLANT_MIST_PORT &= ~(1 << COOLANT_MIST_BIT);
+//			#endif
+//		}
+//	#endif
+//	
+//  sys.report_ovr_counter = 0; // Set to report change immediately
 }
 
 
@@ -120,7 +126,8 @@ void coolant_set_state(uint8_t mode)
 // if an abort or check-mode is active.
 void coolant_sync(uint8_t mode)
 {
-  if (sys.state == STATE_CHECK_MODE) { return; }
-  protocol_buffer_synchronize(); // Ensure coolant turns on when specified in program.
-  coolant_set_state(mode);
+  printf("%s:%s:%d Not supported yet..\n",__FILE__,__FUNCTION__,__LINE__);
+  //if (sys.state == STATE_CHECK_MODE) { return; }
+  //protocol_buffer_synchronize(); // Ensure coolant turns on when specified in program.
+  //coolant_set_state(mode);
 }
diff --git a/grbl/eeprom.c b/grbl/eeprom.c
index 02caf20..e6da28d 100644
--- a/grbl/eeprom.c
+++ b/grbl/eeprom.c
@@ -21,21 +21,82 @@
 *                         $Revision: 1.6 $
 *                         $Date: Friday, February 11, 2005 07:16:44 UTC $
 ****************************************************************************/
-#include <avr/io.h>
-#include <avr/interrupt.h>
+//#include <avr/io.h>
+//#include <avr/interrupt.h>
+
+
+// ecmc added
+#include <stdlib.h>
+#include <string.h>
+#define EEPROM_DUMMY_FILE ./ecmc_grbl_eeprom.txt
+#define EEPROM_MEM_SIZE 512
+char buffer[EEPROM_MEM_SIZE];
+
 
 /* These EEPROM bits have different names on different devices. */
-#ifndef EEPE
-		#define EEPE  EEWE  //!< EEPROM program/write enable.
-		#define EEMPE EEMWE //!< EEPROM master program/write enable.
-#endif
+//#ifndef EEPE
+//		#define EEPE  EEWE  //!< EEPROM program/write enable.
+//		#define EEMPE EEMWE //!< EEPROM master program/write enable.
+//#endif
 
 /* These two are unfortunately not defined in the device include files. */
-#define EEPM1 5 //!< EEPROM Programming Mode Bit 1.
-#define EEPM0 4 //!< EEPROM Programming Mode Bit 0.
+//#define EEPM1 5 //!< EEPROM Programming Mode Bit 1.
+//#define EEPM0 4 //!< EEPROM Programming Mode Bit 0.
 
 /* Define to reduce code size. */
-#define EEPROM_IGNORE_SELFPROG //!< Remove SPM flag polling.
+//#define EEPROM_IGNORE_SELFPROG //!< Remove SPM flag polling.
+
+
+// Init file
+void ecmc_init_file() {
+  printf("%s:%s:%d EEPROM simulated by file..\n",__FILE__,__FUNCTION__,__LINE__);
+  memset(&buffer[0],0,EEPROM_MEM_SIZE);
+  ecmc_mem_to_file();
+}
+
+// Read file to buffer[]
+unsigned char  ecmc_file_to_mem()
+{
+  printf("%s:%s:%d EEPROM simulated by file..\n",__FILE__,__FUNCTION__,__LINE__);
+
+  FILE* fh = fopen(EEPROM_DUMMY_FILE, "rd");
+
+  if (fh == NULL)
+    {
+        printf("something went wrong and file could not be opened");
+        return 1;
+    }
+    unsigned char c = 0;    
+    for (int i = 0, i < EEPROM_MEM_SIZE ; i++) { 
+         // Get the characters
+        buffer[i] = fgetc(fh);
+    }
+   
+    fclose(fh);
+    return 0;
+}
+
+// Write buffer[] to file
+unsigned char void ecmc_mem_to_file()
+{
+  printf("%s:%s:%d EEPROM simulated by file..\n",__FILE__,__FUNCTION__,__LINE__);
+
+  FILE* fh = fopen(EEPROM_DUMMY_FILE, "w");
+
+  if (fh == NULL)
+    {
+        printf("something went wrong and file could not be opened");
+        return 1;
+    }    
+    for (int i = 0, i < EEPROM_MEM_SIZE ; i++) { 
+         // Get the characters
+		fputc (buffer[i], fh);     
+    }
+   
+    fclose(fh);
+    return 0;
+}
+
 
 /*! \brief  Read byte from EEPROM.
  *
@@ -48,10 +109,15 @@
  */
 unsigned char eeprom_get_char( unsigned int addr )
 {
-	do {} while( EECR & (1<<EEPE) ); // Wait for completion of previous write.
-	EEAR = addr; // Set EEPROM address register.
-	EECR = (1<<EERE); // Start EEPROM read operation.
-	return EEDR; // Return the byte read from EEPROM.
+  printf("%s:%s:%d EEPROM simulated by file..\n",__FILE__,__FUNCTION__,__LINE__);
+
+  ecmc_file_to_mem();
+  return buffer[addr];
+
+  //do {} while( EECR & (1<<EEPE) ); // Wait for completion of previous write.
+  //EEAR = addr; // Set EEPROM address register.
+  //EECR = (1<<EERE); // Start EEPROM read operation.
+  //return EEDR; // Return the byte read from EEPROM.
 }
 
 /*! \brief  Write byte to EEPROM.
@@ -73,61 +139,68 @@ unsigned char eeprom_get_char( unsigned int addr )
  */
 void eeprom_put_char( unsigned int addr, unsigned char new_value )
 {
-	char old_value; // Old EEPROM value.
-	char diff_mask; // Difference mask, i.e. old value XOR new value.
+    printf("%s:%s:%d EEPROM simulated by file..\n",__FILE__,__FUNCTION__,__LINE__);
+    
+    ecmc_file_to_mem();
+    buffer[addr] = new_value;
+	ecmc_mem_to_file();
 
-	cli(); // Ensure atomic operation for the write operation.
-	
-	do {} while( EECR & (1<<EEPE) ); // Wait for completion of previous write.
-	#ifndef EEPROM_IGNORE_SELFPROG
-	do {} while( SPMCSR & (1<<SELFPRGEN) ); // Wait for completion of SPM.
-	#endif
-	
-	EEAR = addr; // Set EEPROM address register.
-	EECR = (1<<EERE); // Start EEPROM read operation.
-	old_value = EEDR; // Get old EEPROM value.
-	diff_mask = old_value ^ new_value; // Get bit differences.
-	
-	// Check if any bits are changed to '1' in the new value.
-	if( diff_mask & new_value ) {
-		// Now we know that _some_ bits need to be erased to '1'.
-		
-		// Check if any bits in the new value are '0'.
-		if( new_value != 0xff ) {
-			// Now we know that some bits need to be programmed to '0' also.
-			
-			EEDR = new_value; // Set EEPROM data register.
-			EECR = (1<<EEMPE) | // Set Master Write Enable bit...
-			       (0<<EEPM1) | (0<<EEPM0); // ...and Erase+Write mode.
-			EECR |= (1<<EEPE);  // Start Erase+Write operation.
-		} else {
-			// Now we know that all bits should be erased.
-
-			EECR = (1<<EEMPE) | // Set Master Write Enable bit...
-			       (1<<EEPM0);  // ...and Erase-only mode.
-			EECR |= (1<<EEPE);  // Start Erase-only operation.
-		}
-	} else {
-		// Now we know that _no_ bits need to be erased to '1'.
-		
-		// Check if any bits are changed from '1' in the old value.
-		if( diff_mask ) {
-			// Now we know that _some_ bits need to the programmed to '0'.
-			
-			EEDR = new_value;   // Set EEPROM data register.
-			EECR = (1<<EEMPE) | // Set Master Write Enable bit...
-			       (1<<EEPM1);  // ...and Write-only mode.
-			EECR |= (1<<EEPE);  // Start Write-only operation.
-		}
-	}
-	
-	sei(); // Restore interrupt flag state.
+	//char old_value; // Old EEPROM value.
+	//char diff_mask; // Difference mask, i.e. old value XOR new value.
+//
+	//cli(); // Ensure atomic operation for the write operation.
+	//
+	//do {} while( EECR & (1<<EEPE) ); // Wait for completion of previous write.
+	//#ifndef EEPROM_IGNORE_SELFPROG
+	//do {} while( SPMCSR & (1<<SELFPRGEN) ); // Wait for completion of SPM.
+	//#endif
+	//
+	//EEAR = addr; // Set EEPROM address register.
+	//EECR = (1<<EERE); // Start EEPROM read operation.
+	//old_value = EEDR; // Get old EEPROM value.
+	//diff_mask = old_value ^ new_value; // Get bit differences.
+	//
+	//// Check if any bits are changed to '1' in the new value.
+	//if( diff_mask & new_value ) {
+	//	// Now we know that _some_ bits need to be erased to '1'.
+	//	
+	//	// Check if any bits in the new value are '0'.
+	//	if( new_value != 0xff ) {
+	//		// Now we know that some bits need to be programmed to '0' also.
+	//		
+	//		EEDR = new_value; // Set EEPROM data register.
+	//		EECR = (1<<EEMPE) | // Set Master Write Enable bit...
+	//		       (0<<EEPM1) | (0<<EEPM0); // ...and Erase+Write mode.
+	//		EECR |= (1<<EEPE);  // Start Erase+Write operation.
+	//	} else {
+	//		// Now we know that all bits should be erased.
+//
+	//		EECR = (1<<EEMPE) | // Set Master Write Enable bit...
+	//		       (1<<EEPM0);  // ...and Erase-only mode.
+	//		EECR |= (1<<EEPE);  // Start Erase-only operation.
+	//	}
+	//} else {
+	//	// Now we know that _no_ bits need to be erased to '1'.
+	//	
+	//	// Check if any bits are changed from '1' in the old value.
+	//	if( diff_mask ) {
+	//		// Now we know that _some_ bits need to the programmed to '0'.
+	//		
+	//		EEDR = new_value;   // Set EEPROM data register.
+	//		EECR = (1<<EEMPE) | // Set Master Write Enable bit...
+	//		       (1<<EEPM1);  // ...and Write-only mode.
+	//		EECR |= (1<<EEPE);  // Start Write-only operation.
+	//	}
+	//}
+	//
+	//sei(); // Restore interrupt flag state.
 }
 
 // Extensions added as part of Grbl 
 
 
 void memcpy_to_eeprom_with_checksum(unsigned int destination, char *source, unsigned int size) {
+  printf("%s:%s:%d EEPROM simulated by file..\n",__FILE__,__FUNCTION__,__LINE__);
   unsigned char checksum = 0;
   for(; size > 0; size--) { 
     checksum = (checksum << 1) || (checksum >> 7);
@@ -138,6 +211,7 @@ void memcpy_to_eeprom_with_checksum(unsigned int destination, char *source, unsi
 }
 
 int memcpy_from_eeprom_with_checksum(char *destination, unsigned int source, unsigned int size) {
+  printf("%s:%s:%d EEPROM simulated by file..\n",__FILE__,__FUNCTION__,__LINE__);
   unsigned char data, checksum = 0;
   for(; size > 0; size--) { 
     data = eeprom_get_char(source++);
diff --git a/grbl/eeprom.h b/grbl/eeprom.h
index c9718a2..6760262 100644
--- a/grbl/eeprom.h
+++ b/grbl/eeprom.h
@@ -21,6 +21,9 @@
 #ifndef eeprom_h
 #define eeprom_h
 
+//Added for ecmc
+void ecmc_init_file();
+
 unsigned char eeprom_get_char(unsigned int addr);
 void eeprom_put_char(unsigned int addr, unsigned char new_value);
 void memcpy_to_eeprom_with_checksum(unsigned int destination, char *source, unsigned int size);
diff --git a/grbl/grbl.h b/grbl/grbl.h
index dd8e781..3f75e4f 100644
--- a/grbl/grbl.h
+++ b/grbl/grbl.h
@@ -26,11 +26,11 @@
 #define GRBL_VERSION_BUILD "20190830"
 
 // Define standard libraries used by Grbl.
-#include <avr/io.h>
-#include <avr/pgmspace.h>
-#include <avr/interrupt.h>
-#include <avr/wdt.h>
-#include <util/delay.h>
+//#include <avr/io.h>
+//#include <avr/pgmspace.h>
+//#include <avr/interrupt.h>
+//#include <avr/wdt.h>
+//#include <util/delay.h>
 #include <math.h>
 #include <inttypes.h>
 #include <string.h>
diff --git a/grbl/limits.c b/grbl/limits.c
index 1348c14..931f430 100644
--- a/grbl/limits.c
+++ b/grbl/limits.c
@@ -20,7 +20,7 @@
 */
 
 #include "grbl.h"
-
+#include "ecmcMotion.h"
 
 // Homing axis search distance multiplier. Computed by this value times the cycle travel.
 #ifndef HOMING_AXIS_SEARCH_SCALAR
@@ -40,34 +40,37 @@
 
 void limits_init()
 {
-  LIMIT_DDR &= ~(LIMIT_MASK); // Set as input pins
+  printf("%s:%s:%d Not supported yet..\n",__FILE__,__FUNCTION__,__LINE__);
 
-  #ifdef DISABLE_LIMIT_PIN_PULL_UP
-    LIMIT_PORT &= ~(LIMIT_MASK); // Normal low operation. Requires external pull-down.
-  #else
-    LIMIT_PORT |= (LIMIT_MASK);  // Enable internal pull-up resistors. Normal high operation.
-  #endif
-
-  if (bit_istrue(settings.flags,BITFLAG_HARD_LIMIT_ENABLE)) {
-    LIMIT_PCMSK |= LIMIT_MASK; // Enable specific pins of the Pin Change Interrupt
-    PCICR |= (1 << LIMIT_INT); // Enable Pin Change Interrupt
-  } else {
-    limits_disable();
-  }
-
-  #ifdef ENABLE_SOFTWARE_DEBOUNCE
-    MCUSR &= ~(1<<WDRF);
-    WDTCSR |= (1<<WDCE) | (1<<WDE);
-    WDTCSR = (1<<WDP0); // Set time-out at ~32msec.
-  #endif
+  //LIMIT_DDR &= ~(LIMIT_MASK); // Set as input pins
+//
+  //#ifdef DISABLE_LIMIT_PIN_PULL_UP
+  //  LIMIT_PORT &= ~(LIMIT_MASK); // Normal low operation. Requires external pull-down.
+  //#else
+  //  LIMIT_PORT |= (LIMIT_MASK);  // Enable internal pull-up resistors. Normal high operation.
+  //#endif
+//
+  //if (bit_istrue(settings.flags,BITFLAG_HARD_LIMIT_ENABLE)) {
+  //  LIMIT_PCMSK |= LIMIT_MASK; // Enable specific pins of the Pin Change Interrupt
+  //  PCICR |= (1 << LIMIT_INT); // Enable Pin Change Interrupt
+  //} else {
+  //  limits_disable();
+  //}
+//
+  //#ifdef ENABLE_SOFTWARE_DEBOUNCE
+  //  MCUSR &= ~(1<<WDRF);
+  //  WDTCSR |= (1<<WDCE) | (1<<WDE);
+  //  WDTCSR = (1<<WDP0); // Set time-out at ~32msec.
+  //#endif
 }
 
 
 // Disables hard limits.
 void limits_disable()
 {
-  LIMIT_PCMSK &= ~LIMIT_MASK;  // Disable specific pins of the Pin Change Interrupt
-  PCICR &= ~(1 << LIMIT_INT);  // Disable Pin Change Interrupt
+  printf("%s:%s:%d: Not supported yet..\n",__FILE__,__FUNCTION__,__LINE__);
+  //LIMIT_PCMSK &= ~LIMIT_MASK;  // Disable specific pins of the Pin Change Interrupt
+  //PCICR &= ~(1 << LIMIT_INT);  // Disable Pin Change Interrupt
 }
 
 
@@ -76,22 +79,27 @@ void limits_disable()
 // number in bit position, i.e. Z_AXIS is (1<<2) or bit 2, and Y_AXIS is (1<<1) or bit 1.
 uint8_t limits_get_state()
 {
-  uint8_t limit_state = 0;
-  uint8_t pin = (LIMIT_PIN & LIMIT_MASK);
-  #ifdef INVERT_LIMIT_PIN_MASK
-    pin ^= INVERT_LIMIT_PIN_MASK;
-  #endif
-  if (bit_isfalse(settings.flags,BITFLAG_INVERT_LIMIT_PINS)) { pin ^= LIMIT_MASK; }
-  if (pin) {
-    uint8_t idx;
-    for (idx=0; idx<N_AXIS; idx++) {
-      if (pin & get_limit_pin_mask(idx)) { limit_state |= (1 << idx); }
-    }
-    #ifdef ENABLE_DUAL_AXIS
-      if (pin & (1<<DUAL_LIMIT_BIT)) { limit_state |= (1 << N_AXIS); }
-    #endif
-  }
-  return(limit_state);
+  printf("%s:%s:%d: Not supported yet..\n",__FILE__,__FUNCTION__,__LINE__);
+  // ecmc comment: Use getAxisAtHardFwd() and getAxisAtHardBwd here in ecmcMotion.h
+  return 0;
+
+  
+  //uint8_t limit_state = 0;
+  //uint8_t pin = (LIMIT_PIN & LIMIT_MASK);
+  //#ifdef INVERT_LIMIT_PIN_MASK
+  //  pin ^= INVERT_LIMIT_PIN_MASK;
+  //#endif
+  //if (bit_isfalse(settings.flags,BITFLAG_INVERT_LIMIT_PINS)) { pin ^= LIMIT_MASK; }
+  //if (pin) {
+  //  uint8_t idx;
+  //  for (idx=0; idx<N_AXIS; idx++) {
+  //    if (pin & get_limit_pin_mask(idx)) { limit_state |= (1 << idx); }
+  //  }
+  //  #ifdef ENABLE_DUAL_AXIS
+  //    if (pin & (1<<DUAL_LIMIT_BIT)) { limit_state |= (1 << N_AXIS); }
+  //  #endif
+  //}
+  //return(limit_state);
 }
 
 
@@ -106,46 +114,47 @@ uint8_t limits_get_state()
 // homing cycles and will not respond correctly. Upon user request or need, there may be a
 // special pinout for an e-stop, but it is generally recommended to just directly connect
 // your e-stop switch to the Arduino reset pin, since it is the most correct way to do this.
-#ifndef ENABLE_SOFTWARE_DEBOUNCE
-  ISR(LIMIT_INT_vect) // DEFAULT: Limit pin change interrupt process.
-  {
-    // Ignore limit switches if already in an alarm state or in-process of executing an alarm.
-    // When in the alarm state, Grbl should have been reset or will force a reset, so any pending
-    // moves in the planner and serial buffers are all cleared and newly sent blocks will be
-    // locked out until a homing cycle or a kill lock command. Allows the user to disable the hard
-    // limit setting if their limits are constantly triggering after a reset and move their axes.
-    if (sys.state != STATE_ALARM) {
-      if (!(sys_rt_exec_alarm)) {
-        #ifdef HARD_LIMIT_FORCE_STATE_CHECK
-          // Check limit pin state.
-          if (limits_get_state()) {
-            mc_reset(); // Initiate system kill.
-            system_set_exec_alarm(EXEC_ALARM_HARD_LIMIT); // Indicate hard limit critical event
-          }
-        #else
-          mc_reset(); // Initiate system kill.
-          system_set_exec_alarm(EXEC_ALARM_HARD_LIMIT); // Indicate hard limit critical event
-        #endif
-      }
-    }
-  }
-#else // OPTIONAL: Software debounce limit pin routine.
-  // Upon limit pin change, enable watchdog timer to create a short delay. 
-  ISR(LIMIT_INT_vect) { if (!(WDTCSR & (1<<WDIE))) { WDTCSR |= (1<<WDIE); } }
-  ISR(WDT_vect) // Watchdog timer ISR
-  {
-    WDTCSR &= ~(1<<WDIE); // Disable watchdog timer. 
-    if (sys.state != STATE_ALARM) {  // Ignore if already in alarm state. 
-      if (!(sys_rt_exec_alarm)) {
-        // Check limit pin state. 
-        if (limits_get_state()) {
-          mc_reset(); // Initiate system kill.
-          system_set_exec_alarm(EXEC_ALARM_HARD_LIMIT); // Indicate hard limit critical event
-        }
-      }  
-    }
-  }
-#endif
+
+//#ifndef ENABLE_SOFTWARE_DEBOUNCE
+//  ISR(LIMIT_INT_vect) // DEFAULT: Limit pin change interrupt process.
+//  {
+//    // Ignore limit switches if already in an alarm state or in-process of executing an alarm.
+//    // When in the alarm state, Grbl should have been reset or will force a reset, so any pending
+//    // moves in the planner and serial buffers are all cleared and newly sent blocks will be
+//    // locked out until a homing cycle or a kill lock command. Allows the user to disable the hard
+//    // limit setting if their limits are constantly triggering after a reset and move their axes.
+//    if (sys.state != STATE_ALARM) {
+//      if (!(sys_rt_exec_alarm)) {
+//        #ifdef HARD_LIMIT_FORCE_STATE_CHECK
+//          // Check limit pin state.
+//          if (limits_get_state()) {
+//            mc_reset(); // Initiate system kill.
+//            system_set_exec_alarm(EXEC_ALARM_HARD_LIMIT); // Indicate hard limit critical event
+//          }
+//        #else
+//          mc_reset(); // Initiate system kill.
+//          system_set_exec_alarm(EXEC_ALARM_HARD_LIMIT); // Indicate hard limit critical event
+//        #endif
+//      }
+//    }
+//  }
+//#else // OPTIONAL: Software debounce limit pin routine.
+//  // Upon limit pin change, enable watchdog timer to create a short delay. 
+//  ISR(LIMIT_INT_vect) { if (!(WDTCSR & (1<<WDIE))) { WDTCSR |= (1<<WDIE); } }
+//  ISR(WDT_vect) // Watchdog timer ISR
+//  {
+//    WDTCSR &= ~(1<<WDIE); // Disable watchdog timer. 
+//    if (sys.state != STATE_ALARM) {  // Ignore if already in alarm state. 
+//      if (!(sys_rt_exec_alarm)) {
+//        // Check limit pin state. 
+//        if (limits_get_state()) {
+//          mc_reset(); // Initiate system kill.
+//          system_set_exec_alarm(EXEC_ALARM_HARD_LIMIT); // Indicate hard limit critical event
+//        }
+//      }  
+//    }
+//  }
+//#endif
 
 // Homes the specified cycle axes, sets the machine position, and performs a pull-off motion after
 // completing. Homing is a special motion case, which involves rapid uncontrolled stops to locate
@@ -156,253 +165,255 @@ uint8_t limits_get_state()
 // TODO: Move limit pin-specific calls to a general function for portability.
 void limits_go_home(uint8_t cycle_mask)
 {
-  if (sys.abort) { return; } // Block if system reset has been issued.
-
-  // Initialize plan data struct for homing motion. Spindle and coolant are disabled.
-  plan_line_data_t plan_data;
-  plan_line_data_t *pl_data = &plan_data;
-  memset(pl_data,0,sizeof(plan_line_data_t));
-  pl_data->condition = (PL_COND_FLAG_SYSTEM_MOTION|PL_COND_FLAG_NO_FEED_OVERRIDE);
-  #ifdef USE_LINE_NUMBERS
-    pl_data->line_number = HOMING_CYCLE_LINE_NUMBER;
-  #endif
-
-  // Initialize variables used for homing computations.
-  uint8_t n_cycle = (2*N_HOMING_LOCATE_CYCLE+1);
-  uint8_t step_pin[N_AXIS];
-  #ifdef ENABLE_DUAL_AXIS
-    uint8_t step_pin_dual;
-    uint8_t dual_axis_async_check;
-    int32_t dual_trigger_position;
-    #if (DUAL_AXIS_SELECT == X_AXIS)
-      float fail_distance = (-DUAL_AXIS_HOMING_FAIL_AXIS_LENGTH_PERCENT/100.0)*settings.max_travel[Y_AXIS];
-    #else
-      float fail_distance = (-DUAL_AXIS_HOMING_FAIL_AXIS_LENGTH_PERCENT/100.0)*settings.max_travel[X_AXIS];
-    #endif
-    fail_distance = min(fail_distance, DUAL_AXIS_HOMING_FAIL_DISTANCE_MAX);
-    fail_distance = max(fail_distance, DUAL_AXIS_HOMING_FAIL_DISTANCE_MIN);
-    int32_t dual_fail_distance = trunc(fail_distance*settings.steps_per_mm[DUAL_AXIS_SELECT]);
-    // int32_t dual_fail_distance = trunc((DUAL_AXIS_HOMING_TRIGGER_FAIL_DISTANCE)*settings.steps_per_mm[DUAL_AXIS_SELECT]);
-  #endif
-  float target[N_AXIS];
-  float max_travel = 0.0;
-  uint8_t idx;
-  for (idx=0; idx<N_AXIS; idx++) {
-    // Initialize step pin masks
-    step_pin[idx] = get_step_pin_mask(idx);
-    #ifdef COREXY
-      if ((idx==A_MOTOR)||(idx==B_MOTOR)) { step_pin[idx] = (get_step_pin_mask(X_AXIS)|get_step_pin_mask(Y_AXIS)); }
-    #endif
-
-    if (bit_istrue(cycle_mask,bit(idx))) {
-      // Set target based on max_travel setting. Ensure homing switches engaged with search scalar.
-      // NOTE: settings.max_travel[] is stored as a negative value.
-      max_travel = max(max_travel,(-HOMING_AXIS_SEARCH_SCALAR)*settings.max_travel[idx]);
-    }
-  }
-  #ifdef ENABLE_DUAL_AXIS
-    step_pin_dual = (1<<DUAL_STEP_BIT);
-  #endif
-
-  // Set search mode with approach at seek rate to quickly engage the specified cycle_mask limit switches.
-  bool approach = true;
-  float homing_rate = settings.homing_seek_rate;
-
-  uint8_t limit_state, axislock, n_active_axis;
-  do {
-
-    system_convert_array_steps_to_mpos(target,sys_position);
-
-    // Initialize and declare variables needed for homing routine.
-    axislock = 0;
-    #ifdef ENABLE_DUAL_AXIS
-      sys.homing_axis_lock_dual = 0;
-      dual_trigger_position = 0;
-      dual_axis_async_check = DUAL_AXIS_CHECK_DISABLE;
-    #endif
-    n_active_axis = 0;
-    for (idx=0; idx<N_AXIS; idx++) {
-      // Set target location for active axes and setup computation for homing rate.
-      if (bit_istrue(cycle_mask,bit(idx))) {
-        n_active_axis++;
-        #ifdef COREXY
-          if (idx == X_AXIS) {
-            int32_t axis_position = system_convert_corexy_to_y_axis_steps(sys_position);
-            sys_position[A_MOTOR] = axis_position;
-            sys_position[B_MOTOR] = -axis_position;
-          } else if (idx == Y_AXIS) {
-            int32_t axis_position = system_convert_corexy_to_x_axis_steps(sys_position);
-            sys_position[A_MOTOR] = sys_position[B_MOTOR] = axis_position;
-          } else {
-            sys_position[Z_AXIS] = 0;
-          }
-        #else
-          sys_position[idx] = 0;
-        #endif
-        // Set target direction based on cycle mask and homing cycle approach state.
-        // NOTE: This happens to compile smaller than any other implementation tried.
-        if (bit_istrue(settings.homing_dir_mask,bit(idx))) {
-          if (approach) { target[idx] = -max_travel; }
-          else { target[idx] = max_travel; }
-        } else {
-          if (approach) { target[idx] = max_travel; }
-          else { target[idx] = -max_travel; }
-        }
-        // Apply axislock to the step port pins active in this cycle.
-        axislock |= step_pin[idx];
-        #ifdef ENABLE_DUAL_AXIS
-          if (idx == DUAL_AXIS_SELECT) { sys.homing_axis_lock_dual = step_pin_dual; }
-        #endif
-      }
-
-    }
-    homing_rate *= sqrt(n_active_axis); // [sqrt(N_AXIS)] Adjust so individual axes all move at homing rate.
-    sys.homing_axis_lock = axislock;
-
-    // Perform homing cycle. Planner buffer should be empty, as required to initiate the homing cycle.
-    pl_data->feed_rate = homing_rate; // Set current homing rate.
-    plan_buffer_line(target, pl_data); // Bypass mc_line(). Directly plan homing motion.
-
-    sys.step_control = STEP_CONTROL_EXECUTE_SYS_MOTION; // Set to execute homing motion and clear existing flags.
-    st_prep_buffer(); // Prep and fill segment buffer from newly planned block.
-    st_wake_up(); // Initiate motion
-    do {
-      if (approach) {
-        // Check limit state. Lock out cycle axes when they change.
-        limit_state = limits_get_state();
-        for (idx=0; idx<N_AXIS; idx++) {
-          if (axislock & step_pin[idx]) {
-            if (limit_state & (1 << idx)) {
-              #ifdef COREXY
-                if (idx==Z_AXIS) { axislock &= ~(step_pin[Z_AXIS]); }
-                else { axislock &= ~(step_pin[A_MOTOR]|step_pin[B_MOTOR]); }
-              #else
-                axislock &= ~(step_pin[idx]);
-                #ifdef ENABLE_DUAL_AXIS
-                  if (idx == DUAL_AXIS_SELECT) { dual_axis_async_check |= DUAL_AXIS_CHECK_TRIGGER_1; }
-                #endif
-              #endif
-            }
-          }
-        }
-        sys.homing_axis_lock = axislock;
-        #ifdef ENABLE_DUAL_AXIS
-          if (sys.homing_axis_lock_dual) { // NOTE: Only true when homing dual axis.
-            if (limit_state & (1 << N_AXIS)) { 
-              sys.homing_axis_lock_dual = 0;
-              dual_axis_async_check |= DUAL_AXIS_CHECK_TRIGGER_2;
-            }
-          }
-          
-          // When first dual axis limit triggers, record position and begin checking distance until other limit triggers. Bail upon failure.
-          if (dual_axis_async_check) {
-            if (dual_axis_async_check & DUAL_AXIS_CHECK_ENABLE) {
-              if (( dual_axis_async_check &  (DUAL_AXIS_CHECK_TRIGGER_1 | DUAL_AXIS_CHECK_TRIGGER_2)) == (DUAL_AXIS_CHECK_TRIGGER_1 | DUAL_AXIS_CHECK_TRIGGER_2)) {
-                dual_axis_async_check = DUAL_AXIS_CHECK_DISABLE;
-              } else {
-                if (abs(dual_trigger_position - sys_position[DUAL_AXIS_SELECT]) > dual_fail_distance) {
-                  system_set_exec_alarm(EXEC_ALARM_HOMING_FAIL_DUAL_APPROACH);
-                  mc_reset();
-                  protocol_execute_realtime();
-                  return;
-                }
-              }
-            } else {
-              dual_axis_async_check |= DUAL_AXIS_CHECK_ENABLE;
-              dual_trigger_position = sys_position[DUAL_AXIS_SELECT];
-            }
-          }
-        #endif
-      }
-
-      st_prep_buffer(); // Check and prep segment buffer. NOTE: Should take no longer than 200us.
-
-      // Exit routines: No time to run protocol_execute_realtime() in this loop.
-      if (sys_rt_exec_state & (EXEC_SAFETY_DOOR | EXEC_RESET | EXEC_CYCLE_STOP)) {
-        uint8_t rt_exec = sys_rt_exec_state;
-        // Homing failure condition: Reset issued during cycle.
-        if (rt_exec & EXEC_RESET) { system_set_exec_alarm(EXEC_ALARM_HOMING_FAIL_RESET); }
-        // Homing failure condition: Safety door was opened.
-        if (rt_exec & EXEC_SAFETY_DOOR) { system_set_exec_alarm(EXEC_ALARM_HOMING_FAIL_DOOR); }
-        // Homing failure condition: Limit switch still engaged after pull-off motion
-        if (!approach && (limits_get_state() & cycle_mask)) { system_set_exec_alarm(EXEC_ALARM_HOMING_FAIL_PULLOFF); }
-        // Homing failure condition: Limit switch not found during approach.
-        if (approach && (rt_exec & EXEC_CYCLE_STOP)) { system_set_exec_alarm(EXEC_ALARM_HOMING_FAIL_APPROACH); }
-        if (sys_rt_exec_alarm) {
-          mc_reset(); // Stop motors, if they are running.
-          protocol_execute_realtime();
-          return;
-        } else {
-          // Pull-off motion complete. Disable CYCLE_STOP from executing.
-          system_clear_exec_state_flag(EXEC_CYCLE_STOP);
-          break;
-        }
-      }
-
-    #ifdef ENABLE_DUAL_AXIS
-      } while ((STEP_MASK & axislock) || (sys.homing_axis_lock_dual));
-    #else
-      } while (STEP_MASK & axislock);
-    #endif
-
-    st_reset(); // Immediately force kill steppers and reset step segment buffer.
-    delay_ms(settings.homing_debounce_delay); // Delay to allow transient dynamics to dissipate.
-
-    // Reverse direction and reset homing rate for locate cycle(s).
-    approach = !approach;
-
-    // After first cycle, homing enters locating phase. Shorten search to pull-off distance.
-    if (approach) {
-      max_travel = settings.homing_pulloff*HOMING_AXIS_LOCATE_SCALAR;
-      homing_rate = settings.homing_feed_rate;
-    } else {
-      max_travel = settings.homing_pulloff;
-      homing_rate = settings.homing_seek_rate;
-    }
-
-  } while (n_cycle-- > 0);
-
-  // The active cycle axes should now be homed and machine limits have been located. By
-  // default, Grbl defines machine space as all negative, as do most CNCs. Since limit switches
-  // can be on either side of an axes, check and set axes machine zero appropriately. Also,
-  // set up pull-off maneuver from axes limit switches that have been homed. This provides
-  // some initial clearance off the switches and should also help prevent them from falsely
-  // triggering when hard limits are enabled or when more than one axes shares a limit pin.
-  int32_t set_axis_position;
-  // Set machine positions for homed limit switches. Don't update non-homed axes.
-  for (idx=0; idx<N_AXIS; idx++) {
-    // NOTE: settings.max_travel[] is stored as a negative value.
-    if (cycle_mask & bit(idx)) {
-      #ifdef HOMING_FORCE_SET_ORIGIN
-        set_axis_position = 0;
-      #else
-        if ( bit_istrue(settings.homing_dir_mask,bit(idx)) ) {
-          set_axis_position = lround((settings.max_travel[idx]+settings.homing_pulloff)*settings.steps_per_mm[idx]);
-        } else {
-          set_axis_position = lround(-settings.homing_pulloff*settings.steps_per_mm[idx]);
-        }
-      #endif
-
-      #ifdef COREXY
-        if (idx==X_AXIS) {
-          int32_t off_axis_position = system_convert_corexy_to_y_axis_steps(sys_position);
-          sys_position[A_MOTOR] = set_axis_position + off_axis_position;
-          sys_position[B_MOTOR] = set_axis_position - off_axis_position;
-        } else if (idx==Y_AXIS) {
-          int32_t off_axis_position = system_convert_corexy_to_x_axis_steps(sys_position);
-          sys_position[A_MOTOR] = off_axis_position + set_axis_position;
-          sys_position[B_MOTOR] = off_axis_position - set_axis_position;
-        } else {
-          sys_position[idx] = set_axis_position;
-        }
-      #else
-        sys_position[idx] = set_axis_position;
-      #endif
-
-    }
-  }
-  sys.step_control = STEP_CONTROL_NORMAL_OP; // Return step control to normal operation.
-}
+  printf("%s:%s:%d Not supported yet..\n",__FILE__,__FUNCTION__,__LINE__);
+  
+//  if (sys.abort) { return; } // Block if system reset has been issued.
+//
+//  // Initialize plan data struct for homing motion. Spindle and coolant are disabled.
+//  plan_line_data_t plan_data;
+//  plan_line_data_t *pl_data = &plan_data;
+//  memset(pl_data,0,sizeof(plan_line_data_t));
+//  pl_data->condition = (PL_COND_FLAG_SYSTEM_MOTION|PL_COND_FLAG_NO_FEED_OVERRIDE);
+//  #ifdef USE_LINE_NUMBERS
+//    pl_data->line_number = HOMING_CYCLE_LINE_NUMBER;
+//  #endif
+//
+//  // Initialize variables used for homing computations.
+//  uint8_t n_cycle = (2*N_HOMING_LOCATE_CYCLE+1);
+//  uint8_t step_pin[N_AXIS];
+//  #ifdef ENABLE_DUAL_AXIS
+//    uint8_t step_pin_dual;
+//    uint8_t dual_axis_async_check;
+//    int32_t dual_trigger_position;
+//    #if (DUAL_AXIS_SELECT == X_AXIS)
+//      float fail_distance = (-DUAL_AXIS_HOMING_FAIL_AXIS_LENGTH_PERCENT/100.0)*settings.max_travel[Y_AXIS];
+//    #else
+//      float fail_distance = (-DUAL_AXIS_HOMING_FAIL_AXIS_LENGTH_PERCENT/100.0)*settings.max_travel[X_AXIS];
+//    #endif
+//    fail_distance = min(fail_distance, DUAL_AXIS_HOMING_FAIL_DISTANCE_MAX);
+//    fail_distance = max(fail_distance, DUAL_AXIS_HOMING_FAIL_DISTANCE_MIN);
+//    int32_t dual_fail_distance = trunc(fail_distance*settings.steps_per_mm[DUAL_AXIS_SELECT]);
+//    // int32_t dual_fail_distance = trunc((DUAL_AXIS_HOMING_TRIGGER_FAIL_DISTANCE)*settings.steps_per_mm[DUAL_AXIS_SELECT]);
+//  #endif
+//  float target[N_AXIS];
+//  float max_travel = 0.0;
+//  uint8_t idx;
+//  for (idx=0; idx<N_AXIS; idx++) {
+//    // Initialize step pin masks
+//    step_pin[idx] = get_step_pin_mask(idx);
+//    #ifdef COREXY
+//      if ((idx==A_MOTOR)||(idx==B_MOTOR)) { step_pin[idx] = (get_step_pin_mask(X_AXIS)|get_step_pin_mask(Y_AXIS)); }
+//    #endif
+//
+//    if (bit_istrue(cycle_mask,bit(idx))) {
+//      // Set target based on max_travel setting. Ensure homing switches engaged with search scalar.
+//      // NOTE: settings.max_travel[] is stored as a negative value.
+//      max_travel = max(max_travel,(-HOMING_AXIS_SEARCH_SCALAR)*settings.max_travel[idx]);
+//    }
+//  }
+//  #ifdef ENABLE_DUAL_AXIS
+//    step_pin_dual = (1<<DUAL_STEP_BIT);
+//  #endif
+//
+//  // Set search mode with approach at seek rate to quickly engage the specified cycle_mask limit switches.
+//  bool approach = true;
+//  float homing_rate = settings.homing_seek_rate;
+//
+//  uint8_t limit_state, axislock, n_active_axis;
+//  do {
+//
+//    system_convert_array_steps_to_mpos(target,sys_position);
+//
+//    // Initialize and declare variables needed for homing routine.
+//    axislock = 0;
+//    #ifdef ENABLE_DUAL_AXIS
+//      sys.homing_axis_lock_dual = 0;
+//      dual_trigger_position = 0;
+//      dual_axis_async_check = DUAL_AXIS_CHECK_DISABLE;
+//    #endif
+//    n_active_axis = 0;
+//    for (idx=0; idx<N_AXIS; idx++) {
+//      // Set target location for active axes and setup computation for homing rate.
+//      if (bit_istrue(cycle_mask,bit(idx))) {
+//        n_active_axis++;
+//        #ifdef COREXY
+//          if (idx == X_AXIS) {
+//            int32_t axis_position = system_convert_corexy_to_y_axis_steps(sys_position);
+//            sys_position[A_MOTOR] = axis_position;
+//            sys_position[B_MOTOR] = -axis_position;
+//          } else if (idx == Y_AXIS) {
+//            int32_t axis_position = system_convert_corexy_to_x_axis_steps(sys_position);
+//            sys_position[A_MOTOR] = sys_position[B_MOTOR] = axis_position;
+//          } else {
+//            sys_position[Z_AXIS] = 0;
+//          }
+//        #else
+//          sys_position[idx] = 0;
+//        #endif
+//        // Set target direction based on cycle mask and homing cycle approach state.
+//        // NOTE: This happens to compile smaller than any other implementation tried.
+//        if (bit_istrue(settings.homing_dir_mask,bit(idx))) {
+//          if (approach) { target[idx] = -max_travel; }
+//          else { target[idx] = max_travel; }
+//        } else {
+//          if (approach) { target[idx] = max_travel; }
+//          else { target[idx] = -max_travel; }
+//        }
+//        // Apply axislock to the step port pins active in this cycle.
+//        axislock |= step_pin[idx];
+//        #ifdef ENABLE_DUAL_AXIS
+//          if (idx == DUAL_AXIS_SELECT) { sys.homing_axis_lock_dual = step_pin_dual; }
+//        #endif
+//      }
+//
+//    }
+//    homing_rate *= sqrt(n_active_axis); // [sqrt(N_AXIS)] Adjust so individual axes all move at homing rate.
+//    sys.homing_axis_lock = axislock;
+//
+//    // Perform homing cycle. Planner buffer should be empty, as required to initiate the homing cycle.
+//    pl_data->feed_rate = homing_rate; // Set current homing rate.
+//    plan_buffer_line(target, pl_data); // Bypass mc_line(). Directly plan homing motion.
+//
+//    sys.step_control = STEP_CONTROL_EXECUTE_SYS_MOTION; // Set to execute homing motion and clear existing flags.
+//    st_prep_buffer(); // Prep and fill segment buffer from newly planned block.
+//    st_wake_up(); // Initiate motion
+//    do {
+//      if (approach) {
+//        // Check limit state. Lock out cycle axes when they change.
+//        limit_state = limits_get_state();
+//        for (idx=0; idx<N_AXIS; idx++) {
+//          if (axislock & step_pin[idx]) {
+//            if (limit_state & (1 << idx)) {
+//              #ifdef COREXY
+//                if (idx==Z_AXIS) { axislock &= ~(step_pin[Z_AXIS]); }
+//                else { axislock &= ~(step_pin[A_MOTOR]|step_pin[B_MOTOR]); }
+//              #else
+//                axislock &= ~(step_pin[idx]);
+//                #ifdef ENABLE_DUAL_AXIS
+//                  if (idx == DUAL_AXIS_SELECT) { dual_axis_async_check |= DUAL_AXIS_CHECK_TRIGGER_1; }
+//                #endif
+//              #endif
+//            }
+//          }
+//        }
+//        sys.homing_axis_lock = axislock;
+//        #ifdef ENABLE_DUAL_AXIS
+//          if (sys.homing_axis_lock_dual) { // NOTE: Only true when homing dual axis.
+//            if (limit_state & (1 << N_AXIS)) { 
+//              sys.homing_axis_lock_dual = 0;
+//              dual_axis_async_check |= DUAL_AXIS_CHECK_TRIGGER_2;
+//            }
+//          }
+//          
+//          // When first dual axis limit triggers, record position and begin checking distance until other limit triggers. Bail upon failure.
+//          if (dual_axis_async_check) {
+//            if (dual_axis_async_check & DUAL_AXIS_CHECK_ENABLE) {
+//              if (( dual_axis_async_check &  (DUAL_AXIS_CHECK_TRIGGER_1 | DUAL_AXIS_CHECK_TRIGGER_2)) == (DUAL_AXIS_CHECK_TRIGGER_1 | DUAL_AXIS_CHECK_TRIGGER_2)) {
+//                dual_axis_async_check = DUAL_AXIS_CHECK_DISABLE;
+//              } else {
+//                if (abs(dual_trigger_position - sys_position[DUAL_AXIS_SELECT]) > dual_fail_distance) {
+//                  system_set_exec_alarm(EXEC_ALARM_HOMING_FAIL_DUAL_APPROACH);
+//                  mc_reset();
+//                  protocol_execute_realtime();
+//                  return;
+//                }
+//              }
+//            } else {
+//              dual_axis_async_check |= DUAL_AXIS_CHECK_ENABLE;
+//              dual_trigger_position = sys_position[DUAL_AXIS_SELECT];
+//            }
+//          }
+//        #endif
+//      }
+//
+//      st_prep_buffer(); // Check and prep segment buffer. NOTE: Should take no longer than 200us.
+//
+//      // Exit routines: No time to run protocol_execute_realtime() in this loop.
+//      if (sys_rt_exec_state & (EXEC_SAFETY_DOOR | EXEC_RESET | EXEC_CYCLE_STOP)) {
+//        uint8_t rt_exec = sys_rt_exec_state;
+//        // Homing failure condition: Reset issued during cycle.
+//        if (rt_exec & EXEC_RESET) { system_set_exec_alarm(EXEC_ALARM_HOMING_FAIL_RESET); }
+//        // Homing failure condition: Safety door was opened.
+//        if (rt_exec & EXEC_SAFETY_DOOR) { system_set_exec_alarm(EXEC_ALARM_HOMING_FAIL_DOOR); }
+//        // Homing failure condition: Limit switch still engaged after pull-off motion
+//        if (!approach && (limits_get_state() & cycle_mask)) { system_set_exec_alarm(EXEC_ALARM_HOMING_FAIL_PULLOFF); }
+//        // Homing failure condition: Limit switch not found during approach.
+//        if (approach && (rt_exec & EXEC_CYCLE_STOP)) { system_set_exec_alarm(EXEC_ALARM_HOMING_FAIL_APPROACH); }
+//        if (sys_rt_exec_alarm) {
+//          mc_reset(); // Stop motors, if they are running.
+//          protocol_execute_realtime();
+//          return;
+//        } else {
+//          // Pull-off motion complete. Disable CYCLE_STOP from executing.
+//          system_clear_exec_state_flag(EXEC_CYCLE_STOP);
+//          break;
+//        }
+//      }
+//
+//    #ifdef ENABLE_DUAL_AXIS
+//      } while ((STEP_MASK & axislock) || (sys.homing_axis_lock_dual));
+//    #else
+//      } while (STEP_MASK & axislock);
+//    #endif
+//
+//    st_reset(); // Immediately force kill steppers and reset step segment buffer.
+//    delay_ms(settings.homing_debounce_delay); // Delay to allow transient dynamics to dissipate.
+//
+//    // Reverse direction and reset homing rate for locate cycle(s).
+//    approach = !approach;
+//
+//    // After first cycle, homing enters locating phase. Shorten search to pull-off distance.
+//    if (approach) {
+//      max_travel = settings.homing_pulloff*HOMING_AXIS_LOCATE_SCALAR;
+//      homing_rate = settings.homing_feed_rate;
+//    } else {
+//      max_travel = settings.homing_pulloff;
+//      homing_rate = settings.homing_seek_rate;
+//    }
+//
+//  } while (n_cycle-- > 0);
+//
+//  // The active cycle axes should now be homed and machine limits have been located. By
+//  // default, Grbl defines machine space as all negative, as do most CNCs. Since limit switches
+//  // can be on either side of an axes, check and set axes machine zero appropriately. Also,
+//  // set up pull-off maneuver from axes limit switches that have been homed. This provides
+//  // some initial clearance off the switches and should also help prevent them from falsely
+//  // triggering when hard limits are enabled or when more than one axes shares a limit pin.
+//  int32_t set_axis_position;
+//  // Set machine positions for homed limit switches. Don't update non-homed axes.
+//  for (idx=0; idx<N_AXIS; idx++) {
+//    // NOTE: settings.max_travel[] is stored as a negative value.
+//    if (cycle_mask & bit(idx)) {
+//      #ifdef HOMING_FORCE_SET_ORIGIN
+//        set_axis_position = 0;
+//      #else
+//        if ( bit_istrue(settings.homing_dir_mask,bit(idx)) ) {
+//          set_axis_position = lround((settings.max_travel[idx]+settings.homing_pulloff)*settings.steps_per_mm[idx]);
+//        } else {
+//          set_axis_position = lround(-settings.homing_pulloff*settings.steps_per_mm[idx]);
+//        }
+//      #endif
+//
+//      #ifdef COREXY
+//        if (idx==X_AXIS) {
+//          int32_t off_axis_position = system_convert_corexy_to_y_axis_steps(sys_position);
+//          sys_position[A_MOTOR] = set_axis_position + off_axis_position;
+//          sys_position[B_MOTOR] = set_axis_position - off_axis_position;
+//        } else if (idx==Y_AXIS) {
+//          int32_t off_axis_position = system_convert_corexy_to_x_axis_steps(sys_position);
+//          sys_position[A_MOTOR] = off_axis_position + set_axis_position;
+//          sys_position[B_MOTOR] = off_axis_position - set_axis_position;
+//        } else {
+//          sys_position[idx] = set_axis_position;
+//        }
+//      #else
+//        sys_position[idx] = set_axis_position;
+//      #endif
+//
+//    }
+//  }
+//  sys.step_control = STEP_CONTROL_NORMAL_OP; // Return step control to normal operation.
+//}
 
 
 // Performs a soft limit check. Called from mc_line() only. Assumes the machine has been homed,
@@ -410,6 +421,8 @@ void limits_go_home(uint8_t cycle_mask)
 // NOTE: Used by jogging to limit travel within soft-limit volume.
 void limits_soft_check(float *target)
 {
+  printf("%s:%s:%d:\n",__FILE__,__FUNCTION__,__LINE__);
+
   if (system_check_travel_limits(target)) {
     sys.soft_limit = true;
     // Force feed hold if cycle is active. All buffered blocks are guaranteed to be within
diff --git a/grbl/motion_control.c b/grbl/motion_control.c
index 6e11e35..2544803 100644
--- a/grbl/motion_control.c
+++ b/grbl/motion_control.c
@@ -31,6 +31,8 @@
 // in the planner and to let backlash compensation or canned cycle integration simple and direct.
 void mc_line(float *target, plan_line_data_t *pl_data)
 {
+  printf("%s:%s:%d\n",__FILE__,__FUNCTION__,__LINE__);
+
   // If enabled, check for soft limit violations. Placed here all line motions are picked up
   // from everywhere in Grbl.
   if (bit_istrue(settings.flags,BITFLAG_SOFT_LIMIT_ENABLE)) {
@@ -87,6 +89,8 @@ void mc_line(float *target, plan_line_data_t *pl_data)
 void mc_arc(float *target, plan_line_data_t *pl_data, float *position, float *offset, float radius,
   uint8_t axis_0, uint8_t axis_1, uint8_t axis_linear, uint8_t is_clockwise_arc)
 {
+  printf("%s:%s:%d\n",__FILE__,__FUNCTION__,__LINE__);
+
   float center_axis0 = position[axis_0] + offset[axis_0];
   float center_axis1 = position[axis_1] + offset[axis_1];
   float r_axis0 = -offset[axis_0];  // Radius vector from center to current location
@@ -194,6 +198,8 @@ void mc_arc(float *target, plan_line_data_t *pl_data, float *position, float *of
 // Execute dwell in seconds.
 void mc_dwell(float seconds)
 {
+  printf("%s:%s:%d\n",__FILE__,__FUNCTION__,__LINE__);
+
   if (sys.state == STATE_CHECK_MODE) { return; }
   protocol_buffer_synchronize();
   delay_sec(seconds, DELAY_MODE_DWELL);
@@ -205,49 +211,51 @@ void mc_dwell(float seconds)
 // executing the homing cycle. This prevents incorrect buffered plans after homing.
 void mc_homing_cycle(uint8_t cycle_mask)
 {
-  // Check and abort homing cycle, if hard limits are already enabled. Helps prevent problems
-  // with machines with limits wired on both ends of travel to one limit pin.
-  // TODO: Move the pin-specific LIMIT_PIN call to limits.c as a function.
-  #ifdef LIMITS_TWO_SWITCHES_ON_AXES
-    if (limits_get_state()) {
-      mc_reset(); // Issue system reset and ensure spindle and coolant are shutdown.
-      system_set_exec_alarm(EXEC_ALARM_HARD_LIMIT);
-      return;
-    }
-  #endif
+  printf("%s:%s:%d Not supported yet..\n",__FILE__,__FUNCTION__,__LINE__);
 
-  limits_disable(); // Disable hard limits pin change register for cycle duration
-
-  // -------------------------------------------------------------------------------------
-  // Perform homing routine. NOTE: Special motion case. Only system reset works.
-  
-  #ifdef HOMING_SINGLE_AXIS_COMMANDS
-    if (cycle_mask) { limits_go_home(cycle_mask); } // Perform homing cycle based on mask.
-    else
-  #endif
-  {
-    // Search to engage all axes limit switches at faster homing seek rate.
-    limits_go_home(HOMING_CYCLE_0);  // Homing cycle 0
-    #ifdef HOMING_CYCLE_1
-      limits_go_home(HOMING_CYCLE_1);  // Homing cycle 1
-    #endif
-    #ifdef HOMING_CYCLE_2
-      limits_go_home(HOMING_CYCLE_2);  // Homing cycle 2
-    #endif
-  }
-
-  protocol_execute_realtime(); // Check for reset and set system abort.
-  if (sys.abort) { return; } // Did not complete. Alarm state set by mc_alarm.
-
-  // Homing cycle complete! Setup system for normal operation.
-  // -------------------------------------------------------------------------------------
-
-  // Sync gcode parser and planner positions to homed position.
-  gc_sync_position();
-  plan_sync_position();
-
-  // If hard limits feature enabled, re-enable hard limits pin change register after homing cycle.
-  limits_init();
+//  // Check and abort homing cycle, if hard limits are already enabled. Helps prevent problems
+//  // with machines with limits wired on both ends of travel to one limit pin.
+//  // TODO: Move the pin-specific LIMIT_PIN call to limits.c as a function.
+//  #ifdef LIMITS_TWO_SWITCHES_ON_AXES
+//    if (limits_get_state()) {
+//      mc_reset(); // Issue system reset and ensure spindle and coolant are shutdown.
+//      system_set_exec_alarm(EXEC_ALARM_HARD_LIMIT);
+//      return;
+//    }
+//  #endif
+//
+//  limits_disable(); // Disable hard limits pin change register for cycle duration
+//
+//  // -------------------------------------------------------------------------------------
+//  // Perform homing routine. NOTE: Special motion case. Only system reset works.
+//  
+//  #ifdef HOMING_SINGLE_AXIS_COMMANDS
+//    if (cycle_mask) { limits_go_home(cycle_mask); } // Perform homing cycle based on mask.
+//    else
+//  #endif
+//  {
+//    // Search to engage all axes limit switches at faster homing seek rate.
+//    limits_go_home(HOMING_CYCLE_0);  // Homing cycle 0
+//    #ifdef HOMING_CYCLE_1
+//      limits_go_home(HOMING_CYCLE_1);  // Homing cycle 1
+//    #endif
+//    #ifdef HOMING_CYCLE_2
+//      limits_go_home(HOMING_CYCLE_2);  // Homing cycle 2
+//    #endif
+//  }
+//
+//  protocol_execute_realtime(); // Check for reset and set system abort.
+//  if (sys.abort) { return; } // Did not complete. Alarm state set by mc_alarm.
+//
+//  // Homing cycle complete! Setup system for normal operation.
+//  // -------------------------------------------------------------------------------------
+//
+//  // Sync gcode parser and planner positions to homed position.
+//  gc_sync_position();
+//  plan_sync_position();
+//
+//  // If hard limits feature enabled, re-enable hard limits pin change register after homing cycle.
+//  limits_init();
 }
 
 
@@ -255,66 +263,69 @@ void mc_homing_cycle(uint8_t cycle_mask)
 // NOTE: Upon probe failure, the program will be stopped and placed into ALARM state.
 uint8_t mc_probe_cycle(float *target, plan_line_data_t *pl_data, uint8_t parser_flags)
 {
-  // TODO: Need to update this cycle so it obeys a non-auto cycle start.
-  if (sys.state == STATE_CHECK_MODE) { return(GC_PROBE_CHECK_MODE); }
+  printf("%s:%s:%d Not supported yet..\n",__FILE__,__FUNCTION__,__LINE__);
+  return 0;
 
-  // Finish all queued commands and empty planner buffer before starting probe cycle.
-  protocol_buffer_synchronize();
-  if (sys.abort) { return(GC_PROBE_ABORT); } // Return if system reset has been issued.
-
-  // Initialize probing control variables
-  uint8_t is_probe_away = bit_istrue(parser_flags,GC_PARSER_PROBE_IS_AWAY);
-  uint8_t is_no_error = bit_istrue(parser_flags,GC_PARSER_PROBE_IS_NO_ERROR);
-  sys.probe_succeeded = false; // Re-initialize probe history before beginning cycle.
-  probe_configure_invert_mask(is_probe_away);
-
-  // After syncing, check if probe is already triggered. If so, halt and issue alarm.
-  // NOTE: This probe initialization error applies to all probing cycles.
-  if ( probe_get_state() ) { // Check probe pin state.
-    system_set_exec_alarm(EXEC_ALARM_PROBE_FAIL_INITIAL);
-    protocol_execute_realtime();
-    probe_configure_invert_mask(false); // Re-initialize invert mask before returning.
-    return(GC_PROBE_FAIL_INIT); // Nothing else to do but bail.
-  }
-
-  // Setup and queue probing motion. Auto cycle-start should not start the cycle.
-  mc_line(target, pl_data);
-
-  // Activate the probing state monitor in the stepper module.
-  sys_probe_state = PROBE_ACTIVE;
-
-  // Perform probing cycle. Wait here until probe is triggered or motion completes.
-  system_set_exec_state_flag(EXEC_CYCLE_START);
-  do {
-    protocol_execute_realtime();
-    if (sys.abort) { return(GC_PROBE_ABORT); } // Check for system abort
-  } while (sys.state != STATE_IDLE);
-
-  // Probing cycle complete!
-
-  // Set state variables and error out, if the probe failed and cycle with error is enabled.
-  if (sys_probe_state == PROBE_ACTIVE) {
-    if (is_no_error) { memcpy(sys_probe_position, sys_position, sizeof(sys_position)); }
-    else { system_set_exec_alarm(EXEC_ALARM_PROBE_FAIL_CONTACT); }
-  } else {
-    sys.probe_succeeded = true; // Indicate to system the probing cycle completed successfully.
-  }
-  sys_probe_state = PROBE_OFF; // Ensure probe state monitor is disabled.
-  probe_configure_invert_mask(false); // Re-initialize invert mask.
-  protocol_execute_realtime();   // Check and execute run-time commands
-
-  // Reset the stepper and planner buffers to remove the remainder of the probe motion.
-  st_reset(); // Reset step segment buffer.
-  plan_reset(); // Reset planner buffer. Zero planner positions. Ensure probing motion is cleared.
-  plan_sync_position(); // Sync planner position to current machine position.
-
-  #ifdef MESSAGE_PROBE_COORDINATES
-    // All done! Output the probe position as message.
-    report_probe_parameters();
-  #endif
-
-  if (sys.probe_succeeded) { return(GC_PROBE_FOUND); } // Successful probe cycle.
-  else { return(GC_PROBE_FAIL_END); } // Failed to trigger probe within travel. With or without error.
+  //// TODO: Need to update this cycle so it obeys a non-auto cycle start.
+  //if (sys.state == STATE_CHECK_MODE) { return(GC_PROBE_CHECK_MODE); }
+//
+  //// Finish all queued commands and empty planner buffer before starting probe cycle.
+  //protocol_buffer_synchronize();
+  //if (sys.abort) { return(GC_PROBE_ABORT); } // Return if system reset has been issued.
+//
+  //// Initialize probing control variables
+  //uint8_t is_probe_away = bit_istrue(parser_flags,GC_PARSER_PROBE_IS_AWAY);
+  //uint8_t is_no_error = bit_istrue(parser_flags,GC_PARSER_PROBE_IS_NO_ERROR);
+  //sys.probe_succeeded = false; // Re-initialize probe history before beginning cycle.
+  //probe_configure_invert_mask(is_probe_away);
+//
+  //// After syncing, check if probe is already triggered. If so, halt and issue alarm.
+  //// NOTE: This probe initialization error applies to all probing cycles.
+  //if ( probe_get_state() ) { // Check probe pin state.
+  //  system_set_exec_alarm(EXEC_ALARM_PROBE_FAIL_INITIAL);
+  //  protocol_execute_realtime();
+  //  probe_configure_invert_mask(false); // Re-initialize invert mask before returning.
+  //  return(GC_PROBE_FAIL_INIT); // Nothing else to do but bail.
+  //}
+//
+  //// Setup and queue probing motion. Auto cycle-start should not start the cycle.
+  //mc_line(target, pl_data);
+//
+  //// Activate the probing state monitor in the stepper module.
+  //sys_probe_state = PROBE_ACTIVE;
+//
+  //// Perform probing cycle. Wait here until probe is triggered or motion completes.
+  //system_set_exec_state_flag(EXEC_CYCLE_START);
+  //do {
+  //  protocol_execute_realtime();
+  //  if (sys.abort) { return(GC_PROBE_ABORT); } // Check for system abort
+  //} while (sys.state != STATE_IDLE);
+//
+  //// Probing cycle complete!
+//
+  //// Set state variables and error out, if the probe failed and cycle with error is enabled.
+  //if (sys_probe_state == PROBE_ACTIVE) {
+  //  if (is_no_error) { memcpy(sys_probe_position, sys_position, sizeof(sys_position)); }
+  //  else { system_set_exec_alarm(EXEC_ALARM_PROBE_FAIL_CONTACT); }
+  //} else {
+  //  sys.probe_succeeded = true; // Indicate to system the probing cycle completed successfully.
+  //}
+  //sys_probe_state = PROBE_OFF; // Ensure probe state monitor is disabled.
+  //probe_configure_invert_mask(false); // Re-initialize invert mask.
+  //protocol_execute_realtime();   // Check and execute run-time commands
+//
+  //// Reset the stepper and planner buffers to remove the remainder of the probe motion.
+  //st_reset(); // Reset step segment buffer.
+  //plan_reset(); // Reset planner buffer. Zero planner positions. Ensure probing motion is cleared.
+  //plan_sync_position(); // Sync planner position to current machine position.
+//
+  //#ifdef MESSAGE_PROBE_COORDINATES
+  //  // All done! Output the probe position as message.
+  //  report_probe_parameters();
+  //#endif
+//
+  //if (sys.probe_succeeded) { return(GC_PROBE_FOUND); } // Successful probe cycle.
+  //else { return(GC_PROBE_FAIL_END); } // Failed to trigger probe within travel. With or without error.
 }
 
 
@@ -323,6 +334,8 @@ uint8_t mc_probe_cycle(float *target, plan_line_data_t *pl_data, uint8_t parser_
 #ifdef PARKING_ENABLE
   void mc_parking_motion(float *parking_target, plan_line_data_t *pl_data)
   {
+    printf("%s:%s:%d\n",__FILE__,__FUNCTION__,__LINE__);
+
     if (sys.abort) { return; } // Block during abort.
 
     uint8_t plan_status = plan_buffer_line(parking_target, pl_data);
@@ -350,6 +363,8 @@ uint8_t mc_probe_cycle(float *target, plan_line_data_t *pl_data, uint8_t parser_
 #ifdef ENABLE_PARKING_OVERRIDE_CONTROL
   void mc_override_ctrl_update(uint8_t override_state)
   {
+    printf("%s:%s:%d\n",__FILE__,__FUNCTION__,__LINE__);
+
     // Finish all queued commands before altering override control state
     protocol_buffer_synchronize();
     if (sys.abort) { return; }
@@ -365,6 +380,8 @@ uint8_t mc_probe_cycle(float *target, plan_line_data_t *pl_data, uint8_t parser_
 // realtime abort command and hard limits. So, keep to a minimum.
 void mc_reset()
 {
+  printf("%s:%s:%d\n",__FILE__,__FUNCTION__,__LINE__);
+
   // Only this function can set the system reset. Helps prevent multiple kill calls.
   if (bit_isfalse(sys_rt_exec_state, EXEC_RESET)) {
     system_set_exec_state_flag(EXEC_RESET);
diff --git a/grbl/nuts_bolts.c b/grbl/nuts_bolts.c
index 9d89a8d..6df70f4 100644
--- a/grbl/nuts_bolts.c
+++ b/grbl/nuts_bolts.c
@@ -24,6 +24,10 @@
 
 #define MAX_INT_DIGITS 8 // Maximum number of digits in int32 (and float)
 
+//added for ecmc
+#include <time.h>
+CLOCK_MONOTONIC
+
 
 // Extracts a floating point value from a string. The following code is based loosely on
 // the avr-libc strtod() function by Michael Stumpf and Dmitry Xmelkov and many freely
@@ -111,18 +115,21 @@ uint8_t read_float(char *line, uint8_t *char_counter, float *float_ptr)
 // Non-blocking delay function used for general operation and suspend features.
 void delay_sec(float seconds, uint8_t mode)
 {
- 	uint16_t i = ceil(1000/DWELL_TIME_STEP*seconds);
-	while (i-- > 0) {
-		if (sys.abort) { return; }
-		if (mode == DELAY_MODE_DWELL) {
-			protocol_execute_realtime();
-		} else { // DELAY_MODE_SYS_SUSPEND
-		  // Execute rt_system() only to avoid nesting suspend loops.
-		  protocol_exec_rt_system();
-		  if (sys.suspend & SUSPEND_RESTART_RETRACT) { return; } // Bail, if safety door reopens.
-		}
-		_delay_ms(DWELL_TIME_STEP); // Delay DWELL_TIME_STEP increment
-	}
+
+  clock_nanosleep(CLOCK_MONOTONIC,0,1E9);
+
+ 	//uint16_t i = ceil(1000/DWELL_TIME_STEP*seconds);
+	//while (i-- > 0) {
+	//	if (sys.abort) { return; }
+	//	if (mode == DELAY_MODE_DWELL) {
+	//		protocol_execute_realtime();
+	//	} else { // DELAY_MODE_SYS_SUSPEND
+	//	  // Execute rt_system() only to avoid nesting suspend loops.
+	//	  protocol_exec_rt_system();
+	//	  if (sys.suspend & SUSPEND_RESTART_RETRACT) { return; } // Bail, if safety door reopens.
+	//	}
+	//	_delay_ms(DWELL_TIME_STEP); // Delay DWELL_TIME_STEP increment
+	//}
 }
 
 
@@ -130,7 +137,8 @@ void delay_sec(float seconds, uint8_t mode)
 // which only accepts constants in future compiler releases.
 void delay_ms(uint16_t ms)
 {
-  while ( ms-- ) { _delay_ms(1); }
+  clock_nanosleep(CLOCK_MONOTONIC,0,1E6);
+  //while ( ms-- ) { _delay_ms(1); }
 }
 
 
@@ -139,21 +147,23 @@ void delay_ms(uint16_t ms)
 // efficiently with larger delays, as the counter adds parasitic time in each iteration.
 void delay_us(uint32_t us)
 {
-  while (us) {
-    if (us < 10) {
-      _delay_us(1);
-      us--;
-    } else if (us < 100) {
-      _delay_us(10);
-      us -= 10;
-    } else if (us < 1000) {
-      _delay_us(100);
-      us -= 100;
-    } else {
-      _delay_ms(1);
-      us -= 1000;
-    }
-  }
+  clock_nanosleep(CLOCK_MONOTONIC,0,1E3);
+  
+  //while (us) {
+  //  if (us < 10) {
+  //    _delay_us(1);
+  //    us--;
+  //  } else if (us < 100) {
+  //    _delay_us(10);
+  //    us -= 10;
+  //  } else if (us < 1000) {
+  //    _delay_us(100);
+  //    us -= 100;
+  //  } else {
+  //    _delay_ms(1);
+  //    us -= 1000;
+  //  }
+  //}
 }
 
 
diff --git a/grbl/print.c b/grbl/print.c
index 771e399..61e28aa 100644
--- a/grbl/print.c
+++ b/grbl/print.c
@@ -24,17 +24,20 @@
 
 void printString(const char *s)
 {
-  while (*s)
-    serial_write(*s++);
+  printf("%s:%s:%d:%s\n",__FILE__,__FUNCTION__,__LINE__,s);
+   
+  //while (*s)
+  //  serial_write(*s++);
 }
 
 
 // Print a string stored in PGM-memory
 void printPgmString(const char *s)
 {
-  char c;
-  while ((c = pgm_read_byte_near(s++)))
-    serial_write(c);
+  printf("%s:%s:%d: Not supported\n",__FILE__,__FUNCTION__,__LINE__,s);
+//  char c;
+//  while ((c = pgm_read_byte_near(s++)))
+//    serial_write(c);
 }
 
 
@@ -63,24 +66,30 @@ void printPgmString(const char *s)
 // Prints an uint8 variable in base 10.
 void print_uint8_base10(uint8_t n)
 {
-  uint8_t digit_a = 0;
-  uint8_t digit_b = 0;
-  if (n >= 100) { // 100-255
-    digit_a = '0' + n % 10;
-    n /= 10;
-  }
-  if (n >= 10) { // 10-99
-    digit_b = '0' + n % 10;
-    n /= 10;
-  }
-  serial_write('0' + n);
-  if (digit_b) { serial_write(digit_b); }
-  if (digit_a) { serial_write(digit_a); }
+  printf("%s:%s:%d:%d\n",__FILE__,__FUNCTION__,__LINE__,n);
+  
+  //uint8_t digit_a = 0;
+  //uint8_t digit_b = 0;
+  //if (n >= 100) { // 100-255
+  //  digit_a = '0' + n % 10;
+  //  n /= 10;
+  //}
+  //if (n >= 10) { // 10-99
+  //  digit_b = '0' + n % 10;
+  //  n /= 10;
+  //}
+  //serial_write('0' + n);
+  //if (digit_b) { serial_write(digit_b); }
+  //if (digit_a) { serial_write(digit_a); }
 }
 
 
 // Prints an uint8 variable in base 2 with desired number of desired digits.
 void print_uint8_base2_ndigit(uint8_t n, uint8_t digits) {
+
+
+  printf("%s:%s:%d:\n",__FILE__,__FUNCTION__,__LINE__,n);
+
   unsigned char buf[digits];
   uint8_t i = 0;
 
@@ -89,39 +98,46 @@ void print_uint8_base2_ndigit(uint8_t n, uint8_t digits) {
       n /= 2;
   }
 
-  for (; i > 0; i--)
-      serial_write('0' + buf[i - 1]);
+  for (; i > 0; i--) {
+      //serial_write('0' + buf[i - 1]);
+      printf("%c", '0' + buf[i - 1]);
+  }
+  printf("\n");
 }
 
 
 void print_uint32_base10(uint32_t n)
 {
-  if (n == 0) {
-    serial_write('0');
-    return;
-  }
+  printf("%s:%s:%d:%d\n",__FILE__,__FUNCTION__,__LINE__,n);
 
-  unsigned char buf[10];
-  uint8_t i = 0;
-
-  while (n > 0) {
-    buf[i++] = n % 10;
-    n /= 10;
-  }
-
-  for (; i > 0; i--)
-    serial_write('0' + buf[i-1]);
+//  if (n == 0) {
+//    serial_write('0');
+//    return;
+//  }
+//
+//  unsigned char buf[10];
+//  uint8_t i = 0;
+//
+//  while (n > 0) {
+//    buf[i++] = n % 10;
+//    n /= 10;
+//  }
+//
+//  for (; i > 0; i--)
+//    serial_write('0' + buf[i-1]);
 }
 
 
 void printInteger(long n)
 {
-  if (n < 0) {
-    serial_write('-');
-    print_uint32_base10(-n);
-  } else {
-    print_uint32_base10(n);
-  }
+  printf("%s:%s:%d:%d\n",__FILE__,__FUNCTION__,__LINE__,n);
+
+  //if (n < 0) {
+  //  serial_write('-');
+  //  print_uint32_base10(-n);
+  //} else {
+  //  print_uint32_base10(n);
+  //}
 }
 
 
@@ -132,39 +148,41 @@ void printInteger(long n)
 // techniques are actually just slightly slower. Found this out the hard way.
 void printFloat(float n, uint8_t decimal_places)
 {
-  if (n < 0) {
-    serial_write('-');
-    n = -n;
-  }
+  printf("%s:%s:%d:%f\n",__FILE__,__FUNCTION__,__LINE__,n);
 
-  uint8_t decimals = decimal_places;
-  while (decimals >= 2) { // Quickly convert values expected to be E0 to E-4.
-    n *= 100;
-    decimals -= 2;
-  }
-  if (decimals) { n *= 10; }
-  n += 0.5; // Add rounding factor. Ensures carryover through entire value.
-
-  // Generate digits backwards and store in string.
-  unsigned char buf[13];
-  uint8_t i = 0;
-  uint32_t a = (long)n;
-  while(a > 0) {
-    buf[i++] = (a % 10) + '0'; // Get digit
-    a /= 10;
-  }
-  while (i < decimal_places) {
-     buf[i++] = '0'; // Fill in zeros to decimal point for (n < 1)
-  }
-  if (i == decimal_places) { // Fill in leading zero, if needed.
-    buf[i++] = '0';
-  }
-
-  // Print the generated string.
-  for (; i > 0; i--) {
-    if (i == decimal_places) { serial_write('.'); } // Insert decimal point in right place.
-    serial_write(buf[i-1]);
-  }
+//  if (n < 0) {
+//    serial_write('-');
+//    n = -n;
+//  }
+//
+//  uint8_t decimals = decimal_places;
+//  while (decimals >= 2) { // Quickly convert values expected to be E0 to E-4.
+//    n *= 100;
+//    decimals -= 2;
+//  }
+//  if (decimals) { n *= 10; }
+//  n += 0.5; // Add rounding factor. Ensures carryover through entire value.
+//
+//  // Generate digits backwards and store in string.
+//  unsigned char buf[13];
+//  uint8_t i = 0;
+//  uint32_t a = (long)n;
+//  while(a > 0) {
+//    buf[i++] = (a % 10) + '0'; // Get digit
+//    a /= 10;
+//  }
+//  while (i < decimal_places) {
+//     buf[i++] = '0'; // Fill in zeros to decimal point for (n < 1)
+//  }
+//  if (i == decimal_places) { // Fill in leading zero, if needed.
+//    buf[i++] = '0';
+//  }
+//
+//  // Print the generated string.
+//  for (; i > 0; i--) {
+//    if (i == decimal_places) { serial_write('.'); } // Insert decimal point in right place.
+//    //serial_write(buf[i-1]);
+//  }
 }
 
 
diff --git a/grbl/probe.c b/grbl/probe.c
index 60c9073..7da453e 100644
--- a/grbl/probe.c
+++ b/grbl/probe.c
@@ -28,13 +28,15 @@ uint8_t probe_invert_mask;
 // Probe pin initialization routine.
 void probe_init()
 {
-  PROBE_DDR &= ~(PROBE_MASK); // Configure as input pins
-  #ifdef DISABLE_PROBE_PIN_PULL_UP
-    PROBE_PORT &= ~(PROBE_MASK); // Normal low operation. Requires external pull-down.
-  #else
-    PROBE_PORT |= PROBE_MASK;    // Enable internal pull-up resistors. Normal high operation.
-  #endif
-  probe_configure_invert_mask(false); // Initialize invert mask.
+  printf("%s:%s:%d Not supported yet..\n",__FILE__,__FUNCTION__,__LINE__);
+
+//  PROBE_DDR &= ~(PROBE_MASK); // Configure as input pins
+//  #ifdef DISABLE_PROBE_PIN_PULL_UP
+//    PROBE_PORT &= ~(PROBE_MASK); // Normal low operation. Requires external pull-down.
+//  #else
+//    PROBE_PORT |= PROBE_MASK;    // Enable internal pull-up resistors. Normal high operation.
+//  #endif
+//  probe_configure_invert_mask(false); // Initialize invert mask.
 }
 
 
@@ -43,14 +45,21 @@ void probe_init()
 // and the probing cycle modes for toward-workpiece/away-from-workpiece.
 void probe_configure_invert_mask(uint8_t is_probe_away)
 {
-  probe_invert_mask = 0; // Initialize as zero.
-  if (bit_isfalse(settings.flags,BITFLAG_INVERT_PROBE_PIN)) { probe_invert_mask ^= PROBE_MASK; }
-  if (is_probe_away) { probe_invert_mask ^= PROBE_MASK; }
+  printf("%s:%s:%d Not supported yet..\n",__FILE__,__FUNCTION__,__LINE__);
+
+  //probe_invert_mask = 0; // Initialize as zero.
+  //if (bit_isfalse(settings.flags,BITFLAG_INVERT_PROBE_PIN)) { probe_invert_mask ^= PROBE_MASK; }
+  //if (is_probe_away) { probe_invert_mask ^= PROBE_MASK; }
 }
 
 
 // Returns the probe pin state. Triggered = true. Called by gcode parser and probe state monitor.
-uint8_t probe_get_state() { return((PROBE_PIN & PROBE_MASK) ^ probe_invert_mask); }
+uint8_t probe_get_state() 
+{
+  printf("%s:%s:%d Not supported yet..\n",__FILE__,__FUNCTION__,__LINE__);
+  return 0; 
+  //return((PROBE_PIN & PROBE_MASK) ^ probe_invert_mask); 
+}
 
 
 // Monitors probe pin state and records the system position when detected. Called by the
@@ -58,6 +67,7 @@ uint8_t probe_get_state() { return((PROBE_PIN & PROBE_MASK) ^ probe_invert_mask)
 // NOTE: This function must be extremely efficient as to not bog down the stepper ISR.
 void probe_state_monitor()
 {
+  printf("%s:%s:%d Not supported yet..\n",__FILE__,__FUNCTION__,__LINE__);
   if (probe_get_state()) {
     sys_probe_state = PROBE_OFF;
     memcpy(sys_probe_position, sys_position, sizeof(sys_position));
diff --git a/grbl/system.c b/grbl/system.c
index 55e6c40..f8d8334 100644
--- a/grbl/system.c
+++ b/grbl/system.c
@@ -120,6 +120,9 @@ void system_execute_startup(char *line)
 // the lines that are processed afterward, not necessarily real-time during a cycle,
 // since there are motions already stored in the buffer. However, this 'lag' should not
 // be an issue, since these commands are not typically used during a cycle.
+
+
+// Anders: MAIN entry point for configuration and G-CODE!!!!!
 uint8_t system_execute_line(char *line)
 {
   uint8_t char_counter = 1;