diff --git a/grbl/grbl.h b/grbl/grbl.h
index 3f75e4f..2af4d40 100644
--- a/grbl/grbl.h
+++ b/grbl/grbl.h
@@ -56,7 +56,7 @@
 #include "probe.h"
 #include "protocol.h"
 #include "report.h"
-#include "serial.h"
+//#include "serial.h"
 #include "spindle_control.h"
 #include "stepper.h"
 #include "jog.h"
diff --git a/grbl/spindle_control.c b/grbl/spindle_control.c
index 550b752..8b1525d 100644
--- a/grbl/spindle_control.c
+++ b/grbl/spindle_control.c
@@ -29,66 +29,72 @@
 
 void spindle_init()
 {
-  #ifdef VARIABLE_SPINDLE
-    // Configure variable spindle PWM and enable pin, if requried. On the Uno, PWM and enable are
-    // combined unless configured otherwise.
-    SPINDLE_PWM_DDR |= (1<<SPINDLE_PWM_BIT); // Configure as PWM output pin.
-    SPINDLE_TCCRA_REGISTER = SPINDLE_TCCRA_INIT_MASK; // Configure PWM output compare timer
-    SPINDLE_TCCRB_REGISTER = SPINDLE_TCCRB_INIT_MASK;
-    #ifdef USE_SPINDLE_DIR_AS_ENABLE_PIN
-      SPINDLE_ENABLE_DDR |= (1<<SPINDLE_ENABLE_BIT); // Configure as output pin.
-    #else
-      #ifndef ENABLE_DUAL_AXIS
-        SPINDLE_DIRECTION_DDR |= (1<<SPINDLE_DIRECTION_BIT); // Configure as output pin.
-      #endif
-    #endif
-    pwm_gradient = SPINDLE_PWM_RANGE/(settings.rpm_max-settings.rpm_min);
-  #else
-    SPINDLE_ENABLE_DDR |= (1<<SPINDLE_ENABLE_BIT); // Configure as output pin.
-    #ifndef ENABLE_DUAL_AXIS
-      SPINDLE_DIRECTION_DDR |= (1<<SPINDLE_DIRECTION_BIT); // Configure as output pin.
-    #endif
-  #endif
+  printf("%s:%s:%d Not supported yet..\n",__FILE__,__FUNCTION__,__LINE__);
 
-  spindle_stop();
+//  #ifdef VARIABLE_SPINDLE
+//    // Configure variable spindle PWM and enable pin, if requried. On the Uno, PWM and enable are
+//    // combined unless configured otherwise.
+//    SPINDLE_PWM_DDR |= (1<<SPINDLE_PWM_BIT); // Configure as PWM output pin.
+//    SPINDLE_TCCRA_REGISTER = SPINDLE_TCCRA_INIT_MASK; // Configure PWM output compare timer
+//    SPINDLE_TCCRB_REGISTER = SPINDLE_TCCRB_INIT_MASK;
+//    #ifdef USE_SPINDLE_DIR_AS_ENABLE_PIN
+//      SPINDLE_ENABLE_DDR |= (1<<SPINDLE_ENABLE_BIT); // Configure as output pin.
+//    #else
+//      #ifndef ENABLE_DUAL_AXIS
+//        SPINDLE_DIRECTION_DDR |= (1<<SPINDLE_DIRECTION_BIT); // Configure as output pin.
+//      #endif
+//    #endif
+//    pwm_gradient = SPINDLE_PWM_RANGE/(settings.rpm_max-settings.rpm_min);
+//  #else
+//    SPINDLE_ENABLE_DDR |= (1<<SPINDLE_ENABLE_BIT); // Configure as output pin.
+//    #ifndef ENABLE_DUAL_AXIS
+//      SPINDLE_DIRECTION_DDR |= (1<<SPINDLE_DIRECTION_BIT); // Configure as output pin.
+//    #endif
+//  #endif
+//
+//  spindle_stop();
 }
 
 
 uint8_t spindle_get_state()
 {
-  #ifdef VARIABLE_SPINDLE
-    #ifdef USE_SPINDLE_DIR_AS_ENABLE_PIN
-      // No spindle direction output pin. 
-      #ifdef INVERT_SPINDLE_ENABLE_PIN
-        if (bit_isfalse(SPINDLE_ENABLE_PORT,(1<<SPINDLE_ENABLE_BIT))) { return(SPINDLE_STATE_CW); }
-      #else
-        if (bit_istrue(SPINDLE_ENABLE_PORT,(1<<SPINDLE_ENABLE_BIT))) { return(SPINDLE_STATE_CW); }
-      #endif
-    #else
-      if (SPINDLE_TCCRA_REGISTER & (1<<SPINDLE_COMB_BIT)) { // Check if PWM is enabled.
-        #ifdef ENABLE_DUAL_AXIS
-          return(SPINDLE_STATE_CW);
-        #else
-          if (SPINDLE_DIRECTION_PORT & (1<<SPINDLE_DIRECTION_BIT)) { return(SPINDLE_STATE_CCW); }
-          else { return(SPINDLE_STATE_CW); }
-        #endif
-      }
-    #endif
-  #else
-    #ifdef INVERT_SPINDLE_ENABLE_PIN
-      if (bit_isfalse(SPINDLE_ENABLE_PORT,(1<<SPINDLE_ENABLE_BIT))) { 
-    #else
-      if (bit_istrue(SPINDLE_ENABLE_PORT,(1<<SPINDLE_ENABLE_BIT))) {
-    #endif
-      #ifdef ENABLE_DUAL_AXIS    
-        return(SPINDLE_STATE_CW);
-      #else
-        if (SPINDLE_DIRECTION_PORT & (1<<SPINDLE_DIRECTION_BIT)) { return(SPINDLE_STATE_CCW); }
-        else { return(SPINDLE_STATE_CW); }
-      #endif
-    }
-  #endif
+  printf("%s:%s:%d Not supported yet..\n",__FILE__,__FUNCTION__,__LINE__);
+  
+//
+  //#ifdef VARIABLE_SPINDLE
+  //  #ifdef USE_SPINDLE_DIR_AS_ENABLE_PIN
+  //    // No spindle direction output pin. 
+  //    #ifdef INVERT_SPINDLE_ENABLE_PIN
+  //      if (bit_isfalse(SPINDLE_ENABLE_PORT,(1<<SPINDLE_ENABLE_BIT))) { return(SPINDLE_STATE_CW); }
+  //    #else
+  //      if (bit_istrue(SPINDLE_ENABLE_PORT,(1<<SPINDLE_ENABLE_BIT))) { return(SPINDLE_STATE_CW); }
+  //    #endif
+  //  #else
+  //    if (SPINDLE_TCCRA_REGISTER & (1<<SPINDLE_COMB_BIT)) { // Check if PWM is enabled.
+  //      #ifdef ENABLE_DUAL_AXIS
+  //        return(SPINDLE_STATE_CW);
+  //      #else
+  //        if (SPINDLE_DIRECTION_PORT & (1<<SPINDLE_DIRECTION_BIT)) { return(SPINDLE_STATE_CCW); }
+  //        else { return(SPINDLE_STATE_CW); }
+  //      #endif
+  //    }
+  //  #endif
+  //#else
+  //  #ifdef INVERT_SPINDLE_ENABLE_PIN
+  //    if (bit_isfalse(SPINDLE_ENABLE_PORT,(1<<SPINDLE_ENABLE_BIT))) { 
+  //  #else
+  //    if (bit_istrue(SPINDLE_ENABLE_PORT,(1<<SPINDLE_ENABLE_BIT))) {
+  //  #endif
+  //    #ifdef ENABLE_DUAL_AXIS    
+  //      return(SPINDLE_STATE_CW);
+  //    #else
+  //      if (SPINDLE_DIRECTION_PORT & (1<<SPINDLE_DIRECTION_BIT)) { return(SPINDLE_STATE_CCW); }
+  //      else { return(SPINDLE_STATE_CW); }
+  //    #endif
+  //  }
+  //#endif
   return(SPINDLE_STATE_DISABLE);
+  
 }
 
 
@@ -97,22 +103,24 @@ uint8_t spindle_get_state()
 // Called by spindle_init(), spindle_set_speed(), spindle_set_state(), and mc_reset().
 void spindle_stop()
 {
-  #ifdef VARIABLE_SPINDLE
-    SPINDLE_TCCRA_REGISTER &= ~(1<<SPINDLE_COMB_BIT); // Disable PWM. Output voltage is zero.
-    #ifdef USE_SPINDLE_DIR_AS_ENABLE_PIN
-      #ifdef INVERT_SPINDLE_ENABLE_PIN
-        SPINDLE_ENABLE_PORT |= (1<<SPINDLE_ENABLE_BIT);  // Set pin to high
-      #else
-        SPINDLE_ENABLE_PORT &= ~(1<<SPINDLE_ENABLE_BIT); // Set pin to low
-      #endif
-    #endif
-  #else
-    #ifdef INVERT_SPINDLE_ENABLE_PIN
-      SPINDLE_ENABLE_PORT |= (1<<SPINDLE_ENABLE_BIT);  // Set pin to high
-    #else
-      SPINDLE_ENABLE_PORT &= ~(1<<SPINDLE_ENABLE_BIT); // Set pin to low
-    #endif
-  #endif
+  printf("%s:%s:%d Not supported yet..\n",__FILE__,__FUNCTION__,__LINE__);
+
+  //#ifdef VARIABLE_SPINDLE
+  //  SPINDLE_TCCRA_REGISTER &= ~(1<<SPINDLE_COMB_BIT); // Disable PWM. Output voltage is zero.
+  //  #ifdef USE_SPINDLE_DIR_AS_ENABLE_PIN
+  //    #ifdef INVERT_SPINDLE_ENABLE_PIN
+  //      SPINDLE_ENABLE_PORT |= (1<<SPINDLE_ENABLE_BIT);  // Set pin to high
+  //    #else
+  //      SPINDLE_ENABLE_PORT &= ~(1<<SPINDLE_ENABLE_BIT); // Set pin to low
+  //    #endif
+  //  #endif
+  //#else
+  //  #ifdef INVERT_SPINDLE_ENABLE_PIN
+  //    SPINDLE_ENABLE_PORT |= (1<<SPINDLE_ENABLE_BIT);  // Set pin to high
+  //  #else
+  //    SPINDLE_ENABLE_PORT &= ~(1<<SPINDLE_ENABLE_BIT); // Set pin to low
+  //  #endif
+  //#endif
 }
 
 
@@ -121,102 +129,107 @@ void spindle_stop()
   // and stepper ISR. Keep routine small and efficient.
   void spindle_set_speed(uint8_t pwm_value)
   {
-    SPINDLE_OCR_REGISTER = pwm_value; // Set PWM output level.
-    #ifdef SPINDLE_ENABLE_OFF_WITH_ZERO_SPEED
-      if (pwm_value == SPINDLE_PWM_OFF_VALUE) {
-        spindle_stop();
-      } else {
-        SPINDLE_TCCRA_REGISTER |= (1<<SPINDLE_COMB_BIT); // Ensure PWM output is enabled.
-        #ifdef INVERT_SPINDLE_ENABLE_PIN
-          SPINDLE_ENABLE_PORT &= ~(1<<SPINDLE_ENABLE_BIT);
-        #else
-          SPINDLE_ENABLE_PORT |= (1<<SPINDLE_ENABLE_BIT);
-        #endif
-      }
-    #else
-      if (pwm_value == SPINDLE_PWM_OFF_VALUE) {
-        SPINDLE_TCCRA_REGISTER &= ~(1<<SPINDLE_COMB_BIT); // Disable PWM. Output voltage is zero.
-      } else {
-        SPINDLE_TCCRA_REGISTER |= (1<<SPINDLE_COMB_BIT); // Ensure PWM output is enabled.
-      }
-    #endif
+    printf("%s:%s:%d Not supported yet..\n",__FILE__,__FUNCTION__,__LINE__);
+
+  //  SPINDLE_OCR_REGISTER = pwm_value; // Set PWM output level.
+  //  #ifdef SPINDLE_ENABLE_OFF_WITH_ZERO_SPEED
+  //    if (pwm_value == SPINDLE_PWM_OFF_VALUE) {
+  //      spindle_stop();
+  //    } else {
+  //      SPINDLE_TCCRA_REGISTER |= (1<<SPINDLE_COMB_BIT); // Ensure PWM output is enabled.
+  //      #ifdef INVERT_SPINDLE_ENABLE_PIN
+  //        SPINDLE_ENABLE_PORT &= ~(1<<SPINDLE_ENABLE_BIT);
+  //      #else
+  //        SPINDLE_ENABLE_PORT |= (1<<SPINDLE_ENABLE_BIT);
+  //      #endif
+  //    }
+  //  #else
+  //    if (pwm_value == SPINDLE_PWM_OFF_VALUE) {
+  //      SPINDLE_TCCRA_REGISTER &= ~(1<<SPINDLE_COMB_BIT); // Disable PWM. Output voltage is zero.
+  //    } else {
+  //      SPINDLE_TCCRA_REGISTER |= (1<<SPINDLE_COMB_BIT); // Ensure PWM output is enabled.
+  //    }
+  //  #endif
   }
 
 
-  #ifdef ENABLE_PIECEWISE_LINEAR_SPINDLE
+  //#ifdef ENABLE_PIECEWISE_LINEAR_SPINDLE
   
     // Called by spindle_set_state() and step segment generator. Keep routine small and efficient.
     uint8_t spindle_compute_pwm_value(float rpm) // 328p PWM register is 8-bit.
     {
-      uint8_t pwm_value;
-      rpm *= (0.010*sys.spindle_speed_ovr); // Scale by spindle speed override value.
-      // Calculate PWM register value based on rpm max/min settings and programmed rpm.
-      if ((settings.rpm_min >= settings.rpm_max) || (rpm >= RPM_MAX)) {
-        rpm = RPM_MAX;
-        pwm_value = SPINDLE_PWM_MAX_VALUE;
-      } else if (rpm <= RPM_MIN) {
-        if (rpm == 0.0) { // S0 disables spindle
-          pwm_value = SPINDLE_PWM_OFF_VALUE;
-        } else {
-          rpm = RPM_MIN;
-          pwm_value = SPINDLE_PWM_MIN_VALUE;
-        }
-      } else {
-        // Compute intermediate PWM value with linear spindle speed model via piecewise linear fit model.
-        #if (N_PIECES > 3)
-          if (rpm > RPM_POINT34) {
-            pwm_value = floor(RPM_LINE_A4*rpm - RPM_LINE_B4);
-          } else 
-        #endif
-        #if (N_PIECES > 2)
-          if (rpm > RPM_POINT23) {
-            pwm_value = floor(RPM_LINE_A3*rpm - RPM_LINE_B3);
-          } else 
-        #endif
-        #if (N_PIECES > 1)
-          if (rpm > RPM_POINT12) {
-            pwm_value = floor(RPM_LINE_A2*rpm - RPM_LINE_B2);
-          } else 
-        #endif
-        {
-          pwm_value = floor(RPM_LINE_A1*rpm - RPM_LINE_B1);
-        }
-      }
-      sys.spindle_speed = rpm;
-      return(pwm_value);
+      printf("%s:%s:%d Not supported yet..\n",__FILE__,__FUNCTION__,__LINE__);
+      return 0;
+
+//      uint8_t pwm_value;
+//      rpm *= (0.010*sys.spindle_speed_ovr); // Scale by spindle speed override value.
+//      // Calculate PWM register value based on rpm max/min settings and programmed rpm.
+//      if ((settings.rpm_min >= settings.rpm_max) || (rpm >= RPM_MAX)) {
+//        rpm = RPM_MAX;
+//        pwm_value = SPINDLE_PWM_MAX_VALUE;
+//      } else if (rpm <= RPM_MIN) {
+//        if (rpm == 0.0) { // S0 disables spindle
+//          pwm_value = SPINDLE_PWM_OFF_VALUE;
+//        } else {
+//          rpm = RPM_MIN;
+//          pwm_value = SPINDLE_PWM_MIN_VALUE;
+//        }
+//      } else {
+//        // Compute intermediate PWM value with linear spindle speed model via piecewise linear fit model.
+//        #if (N_PIECES > 3)
+//          if (rpm > RPM_POINT34) {
+//            pwm_value = floor(RPM_LINE_A4*rpm - RPM_LINE_B4);
+//          } else 
+//        #endif
+//        #if (N_PIECES > 2)
+//          if (rpm > RPM_POINT23) {
+//            pwm_value = floor(RPM_LINE_A3*rpm - RPM_LINE_B3);
+//          } else 
+//        #endif
+//        #if (N_PIECES > 1)
+//          if (rpm > RPM_POINT12) {
+//            pwm_value = floor(RPM_LINE_A2*rpm - RPM_LINE_B2);
+//          } else 
+//        #endif
+//        {
+//          pwm_value = floor(RPM_LINE_A1*rpm - RPM_LINE_B1);
+//        }
+//      }
+//      sys.spindle_speed = rpm;
+//      return(pwm_value);
+//    }
+//    
+//  #else 
+//  
+//    // Called by spindle_set_state() and step segment generator. Keep routine small and efficient.
+//    uint8_t spindle_compute_pwm_value(float rpm) // 328p PWM register is 8-bit.
+//    {
+//      uint8_t pwm_value;
+//      rpm *= (0.010*sys.spindle_speed_ovr); // Scale by spindle speed override value.
+//      // Calculate PWM register value based on rpm max/min settings and programmed rpm.
+//      if ((settings.rpm_min >= settings.rpm_max) || (rpm >= settings.rpm_max)) {
+//        // No PWM range possible. Set simple on/off spindle control pin state.
+//        sys.spindle_speed = settings.rpm_max;
+//        pwm_value = SPINDLE_PWM_MAX_VALUE;
+//      } else if (rpm <= settings.rpm_min) {
+//        if (rpm == 0.0) { // S0 disables spindle
+//          sys.spindle_speed = 0.0;
+//          pwm_value = SPINDLE_PWM_OFF_VALUE;
+//        } else { // Set minimum PWM output
+//          sys.spindle_speed = settings.rpm_min;
+//          pwm_value = SPINDLE_PWM_MIN_VALUE;
+//        }
+//      } else { 
+//        // Compute intermediate PWM value with linear spindle speed model.
+//        // NOTE: A nonlinear model could be installed here, if required, but keep it VERY light-weight.
+//        sys.spindle_speed = rpm;
+//        pwm_value = floor((rpm-settings.rpm_min)*pwm_gradient) + SPINDLE_PWM_MIN_VALUE;
+//      }
+//      return(pwm_value);
     }
     
-  #else 
-  
-    // Called by spindle_set_state() and step segment generator. Keep routine small and efficient.
-    uint8_t spindle_compute_pwm_value(float rpm) // 328p PWM register is 8-bit.
-    {
-      uint8_t pwm_value;
-      rpm *= (0.010*sys.spindle_speed_ovr); // Scale by spindle speed override value.
-      // Calculate PWM register value based on rpm max/min settings and programmed rpm.
-      if ((settings.rpm_min >= settings.rpm_max) || (rpm >= settings.rpm_max)) {
-        // No PWM range possible. Set simple on/off spindle control pin state.
-        sys.spindle_speed = settings.rpm_max;
-        pwm_value = SPINDLE_PWM_MAX_VALUE;
-      } else if (rpm <= settings.rpm_min) {
-        if (rpm == 0.0) { // S0 disables spindle
-          sys.spindle_speed = 0.0;
-          pwm_value = SPINDLE_PWM_OFF_VALUE;
-        } else { // Set minimum PWM output
-          sys.spindle_speed = settings.rpm_min;
-          pwm_value = SPINDLE_PWM_MIN_VALUE;
-        }
-      } else { 
-        // Compute intermediate PWM value with linear spindle speed model.
-        // NOTE: A nonlinear model could be installed here, if required, but keep it VERY light-weight.
-        sys.spindle_speed = rpm;
-        pwm_value = floor((rpm-settings.rpm_min)*pwm_gradient) + SPINDLE_PWM_MIN_VALUE;
-      }
-      return(pwm_value);
-    }
-    
-  #endif
-#endif
+  //#endif
+//#endif
 
 
 // Immediately sets spindle running state with direction and spindle rpm via PWM, if enabled.
@@ -228,46 +241,48 @@ void spindle_stop()
   void _spindle_set_state(uint8_t state)
 #endif
 {
-  if (sys.abort) { return; } // Block during abort.
-
-  if (state == SPINDLE_DISABLE) { // Halt or set spindle direction and rpm.
-  
-    #ifdef VARIABLE_SPINDLE
-      sys.spindle_speed = 0.0;
-    #endif
-    spindle_stop();
-  
-  } else {
-    
-    #if !defined(USE_SPINDLE_DIR_AS_ENABLE_PIN) && !defined(ENABLE_DUAL_AXIS)
-      if (state == SPINDLE_ENABLE_CW) {
-        SPINDLE_DIRECTION_PORT &= ~(1<<SPINDLE_DIRECTION_BIT);
-      } else {
-        SPINDLE_DIRECTION_PORT |= (1<<SPINDLE_DIRECTION_BIT);
-      }
-    #endif
-  
-    #ifdef VARIABLE_SPINDLE
-      // NOTE: Assumes all calls to this function is when Grbl is not moving or must remain off.
-      if (settings.flags & BITFLAG_LASER_MODE) { 
-        if (state == SPINDLE_ENABLE_CCW) { rpm = 0.0; } // TODO: May need to be rpm_min*(100/MAX_SPINDLE_SPEED_OVERRIDE);
-      }
-      spindle_set_speed(spindle_compute_pwm_value(rpm));
-    #endif
-    #if (defined(USE_SPINDLE_DIR_AS_ENABLE_PIN) && \
-        !defined(SPINDLE_ENABLE_OFF_WITH_ZERO_SPEED)) || !defined(VARIABLE_SPINDLE)
-      // NOTE: Without variable spindle, the enable bit should just turn on or off, regardless
-      // if the spindle speed value is zero, as its ignored anyhow.
-      #ifdef INVERT_SPINDLE_ENABLE_PIN
-        SPINDLE_ENABLE_PORT &= ~(1<<SPINDLE_ENABLE_BIT);
-      #else
-        SPINDLE_ENABLE_PORT |= (1<<SPINDLE_ENABLE_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 (state == SPINDLE_DISABLE) { // Halt or set spindle direction and rpm.
+  //
+  //  #ifdef VARIABLE_SPINDLE
+  //    sys.spindle_speed = 0.0;
+  //  #endif
+  //  spindle_stop();
+  //
+  //} else {
+  //  
+  //  #if !defined(USE_SPINDLE_DIR_AS_ENABLE_PIN) && !defined(ENABLE_DUAL_AXIS)
+  //    if (state == SPINDLE_ENABLE_CW) {
+  //      SPINDLE_DIRECTION_PORT &= ~(1<<SPINDLE_DIRECTION_BIT);
+  //    } else {
+  //      SPINDLE_DIRECTION_PORT |= (1<<SPINDLE_DIRECTION_BIT);
+  //    }
+  //  #endif
+  //
+  //  #ifdef VARIABLE_SPINDLE
+  //    // NOTE: Assumes all calls to this function is when Grbl is not moving or must remain off.
+  //    if (settings.flags & BITFLAG_LASER_MODE) { 
+  //      if (state == SPINDLE_ENABLE_CCW) { rpm = 0.0; } // TODO: May need to be rpm_min*(100/MAX_SPINDLE_SPEED_OVERRIDE);
+  //    }
+  //    spindle_set_speed(spindle_compute_pwm_value(rpm));
+  //  #endif
+  //  #if (defined(USE_SPINDLE_DIR_AS_ENABLE_PIN) && \
+  //      !defined(SPINDLE_ENABLE_OFF_WITH_ZERO_SPEED)) || !defined(VARIABLE_SPINDLE)
+  //    // NOTE: Without variable spindle, the enable bit should just turn on or off, regardless
+  //    // if the spindle speed value is zero, as its ignored anyhow.
+  //    #ifdef INVERT_SPINDLE_ENABLE_PIN
+  //      SPINDLE_ENABLE_PORT &= ~(1<<SPINDLE_ENABLE_BIT);
+  //    #else
+  //      SPINDLE_ENABLE_PORT |= (1<<SPINDLE_ENABLE_BIT);
+  //    #endif    
+  //  #endif
+  //
+  //}
+  //
+  //sys.report_ovr_counter = 0; // Set to report change immediately
 }
 
 
@@ -276,15 +291,17 @@ void spindle_stop()
 #ifdef VARIABLE_SPINDLE
   void spindle_sync(uint8_t state, float rpm)
   {
-    if (sys.state == STATE_CHECK_MODE) { return; }
-    protocol_buffer_synchronize(); // Empty planner buffer to ensure spindle is set when programmed.
-    spindle_set_state(state,rpm);
+    printf("%s:%s:%d Not supported yet..\n",__FILE__,__FUNCTION__,__LINE__);
+    //if (sys.state == STATE_CHECK_MODE) { return; }
+    //protocol_buffer_synchronize(); // Empty planner buffer to ensure spindle is set when programmed.
+    //spindle_set_state(state,rpm);
   }
 #else
   void _spindle_sync(uint8_t state)
   {
-    if (sys.state == STATE_CHECK_MODE) { return; }
-    protocol_buffer_synchronize(); // Empty planner buffer to ensure spindle is set when programmed.
-    _spindle_set_state(state);
+    printf("%s:%s:%d Not supported yet..\n",__FILE__,__FUNCTION__,__LINE__);
+    //if (sys.state == STATE_CHECK_MODE) { return; }
+    //protocol_buffer_synchronize(); // Empty planner buffer to ensure spindle is set when programmed.
+    //_spindle_set_state(state);
   }
 #endif
diff --git a/grbl/stepper.c b/grbl/stepper.c
index 8e0b151..f2da0ab 100644
--- a/grbl/stepper.c
+++ b/grbl/stepper.c
@@ -179,6 +179,8 @@ typedef struct {
 } st_prep_t;
 static st_prep_t prep;
 
+static bool stepperInterruptEnable = 0;
+
 
 /*    BLOCK VELOCITY PROFILE DEFINITION
           __________________________
@@ -218,6 +220,32 @@ static st_prep_t prep;
   are shown and defined in the above illustration.
 */
 
+void ecmc_grbl_main_thread();
+pthread_t tid;
+
+void ecmc_dummy_thread() {
+  while (stepperInterruptEnable) {
+    for(int i=0; i< 30;i++) {
+      ecmc_grbl_main_thread();
+    }
+    printf("%s:%s:%d Positions(x,y,x)=%lf,%lf,%lf..\n",__FILE__,__FUNCTION__,__LINE__,sys_position[X_AXIS], sys_position[Y_AXIS],sys_position[Z_AXIS] );
+    sleep(0.001);
+  }
+}
+
+void ecmc_start_dummy_thread()
+{
+    int i = 0;
+    int err;
+
+    err = pthread_create(&(tid), NULL, &ecmc_dummy_thread, NULL);
+    if (err != 0)
+        printf("\ncan't create thread :[%s]", strerror(err));
+    else
+        printf("\n Thread created successfully\n");
+    i++;
+    return 0;
+}
 
 // Stepper state initialization. Cycle should only start if the st.cycle_start flag is
 // enabled. Startup init and limits call this function but shouldn't start the cycle.
@@ -242,16 +270,22 @@ void st_wake_up()
   #endif
 
   // Enable Stepper Driver Interrupt
-  TIMSK1 |= (1<<OCIE1A);
+  //TIMSK1 |= (1<<OCIE1A);
+  stepperInterruptEnable = 1;
+  //This should be linked to ecmc execute later
+  ecmc_start_dummy_thread();
 }
 
 
+
 // Stepper shutdown
 void st_go_idle()
 {
+  stepperInterruptEnable = 0;
+
   // Disable Stepper Driver Interrupt. Allow Stepper Port Reset Interrupt to finish, if active.
-  TIMSK1 &= ~(1<<OCIE1A); // Disable Timer1 interrupt
-  TCCR1B = (TCCR1B & ~((1<<CS12) | (1<<CS11))) | (1<<CS10); // Reset clock to no prescaling.
+  //TIMSK1 &= ~(1<<OCIE1A); // Disable Timer1 interrupt
+  //TCCR1B = (TCCR1B & ~((1<<CS12) | (1<<CS11))) | (1<<CS10); // Reset clock to no prescaling.
   busy = false;
 
   // Set stepper driver idle state, disabled or enabled, depending on settings and circumstances.
@@ -316,8 +350,11 @@ void st_go_idle()
 // TODO: Replace direct updating of the int32 position counters in the ISR somehow. Perhaps use smaller
 // int8 variables and update position counters only when a segment completes. This can get complicated
 // with probing and homing cycles that require true real-time positions.
-ISR(TIMER1_COMPA_vect)
-{
+
+// call from plugin execute
+void ecmc_grbl_main_thread()
+{ 
+
   if (busy) { return; } // The busy-flag is used to avoid reentering this interrupt
 
   // Set the direction pins a couple of nanoseconds before we step the steppers
@@ -341,11 +378,11 @@ ISR(TIMER1_COMPA_vect)
 
   // Enable step pulse reset timer so that The Stepper Port Reset Interrupt can reset the signal after
   // exactly settings.pulse_microseconds microseconds, independent of the main Timer1 prescaler.
-  TCNT0 = st.step_pulse_time; // Reload Timer0 counter
-  TCCR0B = (1<<CS01); // Begin Timer0. Full speed, 1/8 prescaler
+  //TCNT0 = st.step_pulse_time; // Reload Timer0 counter
+  //TCCR0B = (1<<CS01); // Begin Timer0. Full speed, 1/8 prescaler
 
   busy = true;
-  sei(); // Re-enable interrupts to allow Stepper Port Reset Interrupt to fire on-time.
+  //sei(); // Re-enable interrupts to allow Stepper Port Reset Interrupt to fire on-time.
          // NOTE: The remaining code in this ISR will finish before returning to main program.
 
   // If there is no step segment, attempt to pop one from the stepper buffer
@@ -361,7 +398,7 @@ ISR(TIMER1_COMPA_vect)
       #endif
 
       // Initialize step segment timing per step and load number of steps to execute.
-      OCR1A = st.exec_segment->cycles_per_tick;
+      //OCR1A = st.exec_segment->cycles_per_tick;
       st.step_count = st.exec_segment->n_step; // NOTE: Can sometimes be zero when moving slow.
       // If the new segment starts a new planner block, initialize stepper variables and counters.
       // NOTE: When the segment data index changes, this indicates a new planner block.
@@ -486,30 +523,30 @@ ISR(TIMER1_COMPA_vect)
 // This interrupt is enabled by ISR_TIMER1_COMPAREA when it sets the motor port bits to execute
 // a step. This ISR resets the motor port after a short period (settings.pulse_microseconds)
 // completing one step cycle.
-ISR(TIMER0_OVF_vect)
-{
-  // Reset stepping pins (leave the direction pins)
-  STEP_PORT = (STEP_PORT & ~STEP_MASK) | (step_port_invert_mask & STEP_MASK);
-  #ifdef ENABLE_DUAL_AXIS
-    STEP_PORT_DUAL = (STEP_PORT_DUAL & ~STEP_MASK_DUAL) | (step_port_invert_mask_dual & STEP_MASK_DUAL);
-  #endif
-  TCCR0B = 0; // Disable Timer0 to prevent re-entering this interrupt when it's not needed.
-}
-#ifdef STEP_PULSE_DELAY
-  // This interrupt is used only when STEP_PULSE_DELAY is enabled. Here, the step pulse is
-  // initiated after the STEP_PULSE_DELAY time period has elapsed. The ISR TIMER2_OVF interrupt
-  // will then trigger after the appropriate settings.pulse_microseconds, as in normal operation.
-  // The new timing between direction, step pulse, and step complete events are setup in the
-  // st_wake_up() routine.
-  ISR(TIMER0_COMPA_vect)
-  {
-    STEP_PORT = st.step_bits; // Begin step pulse.
-    #ifdef ENABLE_DUAL_AXIS
-      STEP_PORT_DUAL = st.step_bits_dual;
-    #endif
-  }
-#endif
-
+//ISR(TIMER0_OVF_vect)
+//{
+//  // Reset stepping pins (leave the direction pins)
+//  STEP_PORT = (STEP_PORT & ~STEP_MASK) | (step_port_invert_mask & STEP_MASK);
+//  #ifdef ENABLE_DUAL_AXIS
+//    STEP_PORT_DUAL = (STEP_PORT_DUAL & ~STEP_MASK_DUAL) | (step_port_invert_mask_dual & STEP_MASK_DUAL);
+//  #endif
+//  TCCR0B = 0; // Disable Timer0 to prevent re-entering this interrupt when it's not needed.
+//}
+//#ifdef STEP_PULSE_DELAY
+//  // This interrupt is used only when STEP_PULSE_DELAY is enabled. Here, the step pulse is
+//  // initiated after the STEP_PULSE_DELAY time period has elapsed. The ISR TIMER2_OVF interrupt
+//  // will then trigger after the appropriate settings.pulse_microseconds, as in normal operation.
+//  // The new timing between direction, step pulse, and step complete events are setup in the
+//  // st_wake_up() routine.
+//  ISR(TIMER0_COMPA_vect)
+//  {
+//    STEP_PORT = st.step_bits; // Begin step pulse.
+//    #ifdef ENABLE_DUAL_AXIS
+//      STEP_PORT_DUAL = st.step_bits_dual;
+//    #endif
+//  }
+//#endif
+//
 
 // Generates the step and direction port invert masks used in the Stepper Interrupt Driver.
 void st_generate_step_dir_invert_masks()
@@ -575,22 +612,22 @@ void stepper_init()
     DIRECTION_DDR_DUAL |= DIRECTION_MASK_DUAL;
   #endif
 
-  // Configure Timer 1: Stepper Driver Interrupt
-  TCCR1B &= ~(1<<WGM13); // waveform generation = 0100 = CTC
-  TCCR1B |=  (1<<WGM12);
-  TCCR1A &= ~((1<<WGM11) | (1<<WGM10));
-  TCCR1A &= ~((1<<COM1A1) | (1<<COM1A0) | (1<<COM1B1) | (1<<COM1B0)); // Disconnect OC1 output
-  // TCCR1B = (TCCR1B & ~((1<<CS12) | (1<<CS11))) | (1<<CS10); // Set in st_go_idle().
-  // TIMSK1 &= ~(1<<OCIE1A);  // Set in st_go_idle().
-
-  // Configure Timer 0: Stepper Port Reset Interrupt
-  TIMSK0 &= ~((1<<OCIE0B) | (1<<OCIE0A) | (1<<TOIE0)); // Disconnect OC0 outputs and OVF interrupt.
-  TCCR0A = 0; // Normal operation
-  TCCR0B = 0; // Disable Timer0 until needed
-  TIMSK0 |= (1<<TOIE0); // Enable Timer0 overflow interrupt
-  #ifdef STEP_PULSE_DELAY
-    TIMSK0 |= (1<<OCIE0A); // Enable Timer0 Compare Match A interrupt
-  #endif
+  //// Configure Timer 1: Stepper Driver Interrupt
+  //TCCR1B &= ~(1<<WGM13); // waveform generation = 0100 = CTC
+  //TCCR1B |=  (1<<WGM12);
+  //TCCR1A &= ~((1<<WGM11) | (1<<WGM10));
+  //TCCR1A &= ~((1<<COM1A1) | (1<<COM1A0) | (1<<COM1B1) | (1<<COM1B0)); // Disconnect OC1 output
+  //// TCCR1B = (TCCR1B & ~((1<<CS12) | (1<<CS11))) | (1<<CS10); // Set in st_go_idle().
+  //// TIMSK1 &= ~(1<<OCIE1A);  // Set in st_go_idle().
+//
+  //// Configure Timer 0: Stepper Port Reset Interrupt
+  //TIMSK0 &= ~((1<<OCIE0B) | (1<<OCIE0A) | (1<<TOIE0)); // Disconnect OC0 outputs and OVF interrupt.
+  //TCCR0A = 0; // Normal operation
+  //TCCR0B = 0; // Disable Timer0 until needed
+  //TIMSK0 |= (1<<TOIE0); // Enable Timer0 overflow interrupt
+  //#ifdef STEP_PULSE_DELAY
+  //  TIMSK0 |= (1<<OCIE0A); // Enable Timer0 Compare Match A interrupt
+  //#endif
 }
 
 
diff --git a/grbl/system.c b/grbl/system.c
index f8d8334..570d6da 100644
--- a/grbl/system.c
+++ b/grbl/system.c
@@ -122,7 +122,6 @@ void system_execute_startup(char *line)
 // 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;