diff --git a/main.c b/main.c
index b56bf6f..829ec5a 100644
--- a/main.c
+++ b/main.c
@@ -36,7 +36,7 @@
 
 int main(void)
 {
-  sei();
+  sei(); // Enable interrupts
   
   serial_init(BAUD_RATE);
   protocol_init();        
@@ -47,8 +47,8 @@ int main(void)
   gc_init();
   limits_init();
                     
-  for(;;){
-    sleep_mode(); // Wait for it ...
+  while (1) {
+//     sleep_mode(); // Wait for it ...
     protocol_process(); // ... process the serial protocol
   }
   return 0;   /* never reached */
diff --git a/motion_control.c b/motion_control.c
index c7e5670..06b7868 100644
--- a/motion_control.c
+++ b/motion_control.c
@@ -48,9 +48,6 @@ void mc_dwell(double seconds)
 void mc_arc(double *position, double *target, double *offset, uint8_t axis_0, uint8_t axis_1, 
   uint8_t axis_linear, double feed_rate, uint8_t invert_feed_rate, double radius, uint8_t isclockwise)
 {      
-//   int acceleration_manager_was_enabled = plan_is_acceleration_manager_enabled();
-//   plan_set_acceleration_manager_enabled(false); // disable acceleration management for the duration of the arc
-
   double center_axis0 = position[axis_0] + offset[axis_0];
   double center_axis1 = position[axis_1] + offset[axis_1];
   double linear_travel = target[axis_linear] - position[axis_linear];
@@ -141,8 +138,6 @@ void mc_arc(double *position, double *target, double *offset, uint8_t axis_0, ui
   }
   // Ensure last segment arrives at target location.
   plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], feed_rate, invert_feed_rate);
-
-//   plan_set_acceleration_manager_enabled(acceleration_manager_was_enabled);
 }
 #endif
 
diff --git a/nuts_bolts.c b/nuts_bolts.c
index 0a8bf5c..8a6960c 100644
--- a/nuts_bolts.c
+++ b/nuts_bolts.c
@@ -1,3 +1,23 @@
+/*
+  nuts_bolts.c - Shared functions
+  Part of Grbl
+
+  Copyright (c) 2009-2011 Simen Svale Skogsrud
+
+  Grbl is free software: you can redistribute it and/or modify
+  it under the terms of the GNU General Public License as published by
+  the Free Software Foundation, either version 3 of the License, or
+  (at your option) any later version.
+
+  Grbl is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+  GNU General Public License for more details.
+
+  You should have received a copy of the GNU General Public License
+  along with Grbl.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
 #include "nuts_bolts.h"
 #include <stdint.h>
 #include <stdlib.h>
diff --git a/nuts_bolts.h b/nuts_bolts.h
index 37f3aa6..54b3cb8 100644
--- a/nuts_bolts.h
+++ b/nuts_bolts.h
@@ -1,5 +1,5 @@
 /*
-  motion_control.h - cartesian robot controller.
+  nuts_bolts.h - Header file for shared definitions, variables, and functions
   Part of Grbl
 
   Copyright (c) 2009-2011 Simen Svale Skogsrud
diff --git a/planner.c b/planner.c
index 6dea6b2..44913c8 100644
--- a/planner.c
+++ b/planner.c
@@ -46,12 +46,11 @@ static int32_t position[3];             // The current position of the tool in a
 static double previous_unit_vec[3];     // Unit vector of previous path line segment
 static double previous_nominal_speed;   // Nominal speed of previous path line segment
 
-static uint8_t acceleration_manager_enabled;   // Acceleration management active?
-
 
 // Returns the index of the next block in the ring buffer
 // NOTE: Removed modulo (%) operator, which uses an expensive divide and multiplication.
-static int8_t next_block_index(int8_t block_index) {
+static uint8_t next_block_index(uint8_t block_index) 
+{
   block_index++;
   if (block_index == BLOCK_BUFFER_SIZE) { block_index = 0; }
   return(block_index);
@@ -59,7 +58,8 @@ static int8_t next_block_index(int8_t block_index) {
 
 
 // Returns the index of the previous block in the ring buffer
-static int8_t prev_block_index(int8_t block_index) {
+static uint8_t prev_block_index(uint8_t block_index) 
+{
   if (block_index == 0) { block_index = BLOCK_BUFFER_SIZE; }
   block_index--;
   return(block_index);
@@ -68,7 +68,8 @@ static int8_t prev_block_index(int8_t block_index) {
 
 // Calculates the distance (not time) it takes to accelerate from initial_rate to target_rate using the 
 // given acceleration:
-static double estimate_acceleration_distance(double initial_rate, double target_rate, double acceleration) {
+static double estimate_acceleration_distance(double initial_rate, double target_rate, double acceleration) 
+{
   return( (target_rate*target_rate-initial_rate*initial_rate)/(2*acceleration) );
 }
 
@@ -86,7 +87,8 @@ static double estimate_acceleration_distance(double initial_rate, double target_
 // you started at speed initial_rate and accelerated until this point and want to end at the final_rate after
 // a total travel of distance. This can be used to compute the intersection point between acceleration and
 // deceleration in the cases where the trapezoid has no plateau (i.e. never reaches maximum speed)
-static double intersection_distance(double initial_rate, double final_rate, double acceleration, double distance) {
+static double intersection_distance(double initial_rate, double final_rate, double acceleration, double distance) 
+{
   return( (2*acceleration*distance-initial_rate*initial_rate+final_rate*final_rate)/(4*acceleration) );
 }
 
@@ -96,13 +98,15 @@ static double intersection_distance(double initial_rate, double final_rate, doub
 // NOTE: sqrt() reimplimented here from prior version due to improved planner logic. Increases speed
 // in time critical computations, i.e. arcs or rapid short lines from curves. Guaranteed to not exceed
 // BLOCK_BUFFER_SIZE calls per planner cycle.
-static double max_allowable_speed(double acceleration, double target_velocity, double distance) {
+static double max_allowable_speed(double acceleration, double target_velocity, double distance) 
+{
   return( sqrt(target_velocity*target_velocity-2*acceleration*distance) );
 }
 
 
 // The kernel called by planner_recalculate() when scanning the plan from last to first entry.
-static void planner_reverse_pass_kernel(block_t *previous, block_t *current, block_t *next) {
+static void planner_reverse_pass_kernel(block_t *previous, block_t *current, block_t *next) 
+{
   if (!current) { return; }  // Cannot operate on nothing.
   
   if (next) { 
@@ -128,8 +132,9 @@ static void planner_reverse_pass_kernel(block_t *previous, block_t *current, blo
 
 // planner_recalculate() needs to go over the current plan twice. Once in reverse and once forward. This 
 // implements the reverse pass.
-static void planner_reverse_pass() {
-  auto int8_t block_index = block_buffer_head;
+static void planner_reverse_pass() 
+{
+  uint8_t block_index = block_buffer_head;
   block_t *block[3] = {NULL, NULL, NULL};
   while(block_index != block_buffer_tail) {    
     block_index = prev_block_index( block_index );
@@ -143,7 +148,8 @@ static void planner_reverse_pass() {
 
 
 // The kernel called by planner_recalculate() when scanning the plan from first to last entry.
-static void planner_forward_pass_kernel(block_t *previous, block_t *current, block_t *next) {
+static void planner_forward_pass_kernel(block_t *previous, block_t *current, block_t *next) 
+{
   if(!previous) { return; }  // Begin planning after buffer_tail
   
   // If the previous block is an acceleration block, but it is not long enough to complete the
@@ -167,8 +173,9 @@ static void planner_forward_pass_kernel(block_t *previous, block_t *current, blo
 
 // planner_recalculate() needs to go over the current plan twice. Once in reverse and once forward. This 
 // implements the forward pass.
-static void planner_forward_pass() {
-  int8_t block_index = block_buffer_tail;
+static void planner_forward_pass() 
+{
+  uint8_t block_index = block_buffer_tail;
   block_t *block[3] = {NULL, NULL, NULL};
   
   while(block_index != block_buffer_head) {
@@ -194,8 +201,8 @@ static void planner_forward_pass() {
 // The factors represent a factor of braking and must be in the range 0.0-1.0.
 // This converts the planner parameters to the data required by the stepper controller.
 // NOTE: Final rates must be computed in terms of their respective blocks.
-static void calculate_trapezoid_for_block(block_t *block, double entry_factor, double exit_factor) {
-  
+static void calculate_trapezoid_for_block(block_t *block, double entry_factor, double exit_factor) 
+{  
   block->initial_rate = ceil(block->nominal_rate*entry_factor); // (step/min)
   block->final_rate = ceil(block->nominal_rate*exit_factor); // (step/min)
   int32_t acceleration_per_minute = block->rate_delta*ACCELERATION_TICKS_PER_SECOND*60.0; // (step/min^2)
@@ -235,8 +242,9 @@ static void calculate_trapezoid_for_block(block_t *block, double entry_factor, d
 // planner_recalculate() after updating the blocks. Any recalulate flagged junction will
 // compute the two adjacent trapezoids to the junction, since the junction speed corresponds 
 // to exit speed and entry speed of one another.
-static void planner_recalculate_trapezoids() {
-  int8_t block_index = block_buffer_tail;
+static void planner_recalculate_trapezoids() 
+{
+  uint8_t block_index = block_buffer_tail;
   block_t *current;
   block_t *next = NULL;
   
@@ -281,49 +289,41 @@ static void planner_recalculate_trapezoids() {
 // All planner computations are performed with doubles (float on Arduinos) to minimize numerical round-
 // off errors. Only when planned values are converted to stepper rate parameters, these are integers.
 
-static void planner_recalculate() {     
+static void planner_recalculate() 
+{     
   planner_reverse_pass();
   planner_forward_pass();
   planner_recalculate_trapezoids();
 }
 
-void plan_init() {
+void plan_init() 
+{
   block_buffer_head = 0;
   block_buffer_tail = 0;
-  plan_set_acceleration_manager_enabled(true);
   clear_vector(position);
   clear_vector_double(previous_unit_vec);
   previous_nominal_speed = 0.0;
 }
 
-void plan_set_acceleration_manager_enabled(uint8_t enabled) {
-  if ((!!acceleration_manager_enabled) != (!!enabled)) {
-    st_synchronize();
-    acceleration_manager_enabled = !!enabled;
-  }
-}
-
-int plan_is_acceleration_manager_enabled() {
-  return(acceleration_manager_enabled);
-}
-
-void plan_discard_current_block() {
+void plan_discard_current_block() 
+{
   if (block_buffer_head != block_buffer_tail) {
     block_buffer_tail = next_block_index( block_buffer_tail );
   }
 }
 
-block_t *plan_get_current_block() {
+block_t *plan_get_current_block() 
+{
   if (block_buffer_head == block_buffer_tail) { return(NULL); }
   return(&block_buffer[block_buffer_tail]);
 }
 
 
 // Add a new linear movement to the buffer. x, y and z is the signed, absolute target position in 
-// millimaters. Feed rate specifies the speed of the motion. If feed rate is inverted, the feed
+// millimeters. Feed rate specifies the speed of the motion. If feed rate is inverted, the feed
 // rate is taken to mean "frequency" and would complete the operation in 1/feed_rate minutes.
-void plan_buffer_line(double x, double y, double z, double feed_rate, uint8_t invert_feed_rate) {
-  
+void plan_buffer_line(double x, double y, double z, double feed_rate, uint8_t invert_feed_rate) 
+{  
   // Calculate target position in absolute steps
   int32_t target[3];
   target[X_AXIS] = lround(x*settings.steps_per_mm[X_AXIS]);
@@ -331,7 +331,7 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, uint8_t in
   target[Z_AXIS] = lround(z*settings.steps_per_mm[Z_AXIS]);     
   
   // Calculate the buffer head after we push this byte
-  int next_buffer_head = next_block_index( block_buffer_head );	
+  uint8_t next_buffer_head = next_block_index( block_buffer_head );	
   // If the buffer is full: good! That means we are well ahead of the robot. 
   // Rest here until there is room in the buffer.
   while(block_buffer_tail == next_buffer_head) { sleep_mode(); }
@@ -384,84 +384,72 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, uint8_t in
   block->rate_delta = ceil( block->step_event_count*inverse_millimeters *  
         settings.acceleration / (60 * ACCELERATION_TICKS_PER_SECOND )); // (step/min/acceleration_tick)
 
-  // Perform planner-enabled calculations
-  if (acceleration_manager_enabled) {  
-  
-    // Compute path unit vector                            
-    double unit_vec[3];
+  // Compute path unit vector                            
+  double unit_vec[3];
 
-    unit_vec[X_AXIS] = delta_mm[X_AXIS]*inverse_millimeters;
-    unit_vec[Y_AXIS] = delta_mm[Y_AXIS]*inverse_millimeters;
-    unit_vec[Z_AXIS] = delta_mm[Z_AXIS]*inverse_millimeters;  
-  
-    // Compute maximum allowable entry speed at junction by centripetal acceleration approximation.
-    // Let a circle be tangent to both previous and current path line segments, where the junction 
-    // deviation is defined as the distance from the junction to the closest edge of the circle, 
-    // colinear with the circle center. The circular segment joining the two paths represents the 
-    // path of centripetal acceleration. Solve for max velocity based on max acceleration about the
-    // radius of the circle, defined indirectly by junction deviation. This may be also viewed as 
-    // path width or max_jerk in the previous grbl version. This approach does not actually deviate 
-    // from path, but used as a robust way to compute cornering speeds, as it takes into account the
-    // nonlinearities of both the junction angle and junction velocity.
-    double vmax_junction = MINIMUM_PLANNER_SPEED; // Set default max junction speed
+  unit_vec[X_AXIS] = delta_mm[X_AXIS]*inverse_millimeters;
+  unit_vec[Y_AXIS] = delta_mm[Y_AXIS]*inverse_millimeters;
+  unit_vec[Z_AXIS] = delta_mm[Z_AXIS]*inverse_millimeters;  
 
-    // Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles.
-    if ((block_buffer_head != block_buffer_tail) && (previous_nominal_speed > 0.0)) {
-      // Compute cosine of angle between previous and current path. (prev_unit_vec is negative)
-      // NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity.
-      double cos_theta = - previous_unit_vec[X_AXIS] * unit_vec[X_AXIS] 
-                         - previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS] 
-                         - previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ;
-                           
-      // Skip and use default max junction speed for 0 degree acute junction.
-      if (cos_theta < 0.95) {
-        vmax_junction = min(previous_nominal_speed,block->nominal_speed);
-        // Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds.
-        if (cos_theta > -0.95) {
-          // Compute maximum junction velocity based on maximum acceleration and junction deviation
-          double sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive.
-          vmax_junction = min(vmax_junction,
-            sqrt(settings.acceleration * settings.junction_deviation * sin_theta_d2/(1.0-sin_theta_d2)) );
-        }
+  // Compute maximum allowable entry speed at junction by centripetal acceleration approximation.
+  // Let a circle be tangent to both previous and current path line segments, where the junction 
+  // deviation is defined as the distance from the junction to the closest edge of the circle, 
+  // colinear with the circle center. The circular segment joining the two paths represents the 
+  // path of centripetal acceleration. Solve for max velocity based on max acceleration about the
+  // radius of the circle, defined indirectly by junction deviation. This may be also viewed as 
+  // path width or max_jerk in the previous grbl version. This approach does not actually deviate 
+  // from path, but used as a robust way to compute cornering speeds, as it takes into account the
+  // nonlinearities of both the junction angle and junction velocity.
+  double vmax_junction = MINIMUM_PLANNER_SPEED; // Set default max junction speed
+
+  // Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles.
+  if ((block_buffer_head != block_buffer_tail) && (previous_nominal_speed > 0.0)) {
+    // Compute cosine of angle between previous and current path. (prev_unit_vec is negative)
+    // NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity.
+    double cos_theta = - previous_unit_vec[X_AXIS] * unit_vec[X_AXIS] 
+                       - previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS] 
+                       - previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ;
+                         
+    // Skip and use default max junction speed for 0 degree acute junction.
+    if (cos_theta < 0.95) {
+      vmax_junction = min(previous_nominal_speed,block->nominal_speed);
+      // Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds.
+      if (cos_theta > -0.95) {
+        // Compute maximum junction velocity based on maximum acceleration and junction deviation
+        double sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive.
+        vmax_junction = min(vmax_junction,
+          sqrt(settings.acceleration * settings.junction_deviation * sin_theta_d2/(1.0-sin_theta_d2)) );
       }
     }
-    block->max_entry_speed = vmax_junction;
-    
-    // Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED.
-    double v_allowable = max_allowable_speed(-settings.acceleration,MINIMUM_PLANNER_SPEED,block->millimeters);
-    block->entry_speed = min(vmax_junction, v_allowable);
-
-    // Initialize planner efficiency flags
-    // Set flag if block will always reach maximum junction speed regardless of entry/exit speeds.
-    // If a block can de/ac-celerate from nominal speed to zero within the length of the block, then
-    // the current block and next block junction speeds are guaranteed to always be at their maximum
-    // junction speeds in deceleration and acceleration, respectively. This is due to how the current
-    // block nominal speed limits both the current and next maximum junction speeds. Hence, in both
-    // the reverse and forward planners, the corresponding block junction speed will always be at the
-    // the maximum junction speed and may always be ignored for any speed reduction checks.
-    if (block->nominal_speed <= v_allowable) { block->nominal_length_flag = true; }
-    else { block->nominal_length_flag = false; }
-    block->recalculate_flag = true; // Always calculate trapezoid for new block
+  }  
+  block->max_entry_speed = vmax_junction;
   
-    // Update previous path unit_vector and nominal speed
-    memcpy(previous_unit_vec, unit_vec, sizeof(unit_vec)); // previous_unit_vec[] = unit_vec[]
-    previous_nominal_speed = block->nominal_speed;
+  // Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED.
+  double v_allowable = max_allowable_speed(-settings.acceleration,MINIMUM_PLANNER_SPEED,block->millimeters);
+  block->entry_speed = min(vmax_junction, v_allowable);
 
-  } else {
-    // Acceleration planner disabled. Set minimum that is required.
-    block->initial_rate = block->nominal_rate;
-    block->final_rate = block->nominal_rate;
-    block->accelerate_until = 0;
-    block->decelerate_after = block->step_event_count;
-    block->rate_delta = 0;
-  }
+  // Initialize planner efficiency flags
+  // Set flag if block will always reach maximum junction speed regardless of entry/exit speeds.
+  // If a block can de/ac-celerate from nominal speed to zero within the length of the block, then
+  // the current block and next block junction speeds are guaranteed to always be at their maximum
+  // junction speeds in deceleration and acceleration, respectively. This is due to how the current
+  // block nominal speed limits both the current and next maximum junction speeds. Hence, in both
+  // the reverse and forward planners, the corresponding block junction speed will always be at the
+  // the maximum junction speed and may always be ignored for any speed reduction checks.
+  if (block->nominal_speed <= v_allowable) { block->nominal_length_flag = true; }
+  else { block->nominal_length_flag = false; }
+  block->recalculate_flag = true; // Always calculate trapezoid for new block
+
+  // Update previous path unit_vector and nominal speed
+  memcpy(previous_unit_vec, unit_vec, sizeof(unit_vec)); // previous_unit_vec[] = unit_vec[]
+  previous_nominal_speed = block->nominal_speed;
   
   // Move buffer head
   block_buffer_head = next_buffer_head;     
   // Update position
   memcpy(position, target, sizeof(target)); // position[] = target[]
 
-  if (acceleration_manager_enabled) { planner_recalculate(); }  
+  planner_recalculate();  
   st_cycle_start();
 }
 
diff --git a/planner.h b/planner.h
index f91b7c2..f8f59e8 100644
--- a/planner.h
+++ b/planner.h
@@ -27,12 +27,12 @@
 // This struct is used when buffering the setup for each linear movement "nominal" values are as specified in 
 // the source g-code and may never actually be reached if acceleration management is active.
 typedef struct {
+
   // Fields used by the bresenham algorithm for tracing the line
-  uint32_t steps_x, steps_y, steps_z; // Step count along each axis
   uint8_t  direction_bits;            // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h)
+  uint32_t steps_x, steps_y, steps_z; // Step count along each axis
   int32_t  step_event_count;          // The number of step events required to complete this block
-  uint32_t nominal_rate;              // The nominal step rate for this block in step_events/minute
-  
+
   // Fields used by the motion planner to manage acceleration
   double nominal_speed;               // The nominal speed for this block in mm/min  
   double entry_speed;                 // Entry speed at previous-current junction in mm/min
@@ -42,12 +42,13 @@ typedef struct {
   uint8_t nominal_length_flag;        // Planner flag for nominal speed always reached
 
   // Settings for the trapezoid generator
-  uint32_t initial_rate;              // The jerk-adjusted step rate at start of block  
-  uint32_t final_rate;                // The minimal rate at exit
+  uint32_t initial_rate;              // The step rate at start of block  
+  uint32_t final_rate;                // The step rate at end of block
   int32_t rate_delta;                 // The steps/minute to add or subtract when changing speed (must be positive)
   uint32_t accelerate_until;          // The index of the step event on which to stop acceleration
   uint32_t decelerate_after;          // The index of the step event on which to start decelerating
-  
+  uint32_t nominal_rate;              // The nominal step rate for this block in step_events/minute
+
 } block_t;
       
 // Initialize the motion plan subsystem      
@@ -65,12 +66,6 @@ void plan_discard_current_block();
 // Gets the current block. Returns NULL if buffer empty
 block_t *plan_get_current_block();
 
-// Enables or disables acceleration-management for upcoming blocks
-void plan_set_acceleration_manager_enabled(uint8_t enabled);
-
-// Is acceleration-management currently enabled?
-int plan_is_acceleration_manager_enabled();
-
 // Reset the position vector
 void plan_set_current_position(double x, double y, double z); 
 
diff --git a/protocol.c b/protocol.c
index 4835c90..4010253 100644
--- a/protocol.c
+++ b/protocol.c
@@ -31,10 +31,12 @@
 #include <avr/pgmspace.h>
 #define LINE_BUFFER_SIZE 50
 
-static char line[LINE_BUFFER_SIZE];
-static uint8_t char_counter;
+static char line[LINE_BUFFER_SIZE]; // Line to be executed. Zero-terminated.
+static uint8_t char_counter; // Last character counter in line variable.
+static uint8_t iscomment; // Comment/block delete flag for processor to ignore comment characters.
 
-static void status_message(int status_code) {
+static void status_message(int status_code) 
+{
   if (status_code == 0) {
     printPgmString(PSTR("ok\r\n"));
   } else {
@@ -57,12 +59,15 @@ static void status_message(int status_code) {
 
 void protocol_init() 
 {
+  char_counter = 0; // Reset line input
+  iscomment = false;
   printPgmString(PSTR("\r\nGrbl " GRBL_VERSION));
   printPgmString(PSTR("\r\n"));  
 }
 
 // Executes one line of input according to protocol
-uint8_t protocol_execute_line(char *line) {
+uint8_t protocol_execute_line(char *line) 
+{     
   if(line[0] == '$') {
     return(settings_execute_line(line)); // Delegate lines starting with '$' to the settings module
   } else {
@@ -70,15 +75,16 @@ uint8_t protocol_execute_line(char *line) {
   }
 }
 
+
+// Process one line of incoming serial data. Remove unneeded characters and capitalize.
 void protocol_process()
 {
   char c;
-  uint8_t iscomment = false;
   while((c = serial_read()) != SERIAL_NO_DATA) 
   {
-    if ((c == '\n') || (c == '\r')) { // End of block reached
+    if ((c == '\n') || (c == '\r')) { // End of line reached
       if (char_counter > 0) {// Line is complete. Then execute!
-        line[char_counter] = 0; // terminate string
+        line[char_counter] = 0; // Terminate string
         status_message(protocol_execute_line(line));
       } else { 
         // Empty or comment line. Skip block.
diff --git a/protocol.h b/protocol.h
index 3ad6597..4490a4b 100644
--- a/protocol.h
+++ b/protocol.h
@@ -3,6 +3,7 @@
   Part of Grbl
 
   Copyright (c) 2009-2011 Simen Svale Skogsrud
+  Copyright (c) 2011 Sungeun K. Jeon
 
   Grbl is free software: you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
diff --git a/serial.c b/serial.c
index 7310e3f..04483bb 100644
--- a/serial.c
+++ b/serial.c
@@ -44,25 +44,25 @@ volatile uint8_t tx_buffer_tail = 0;
 
 static void set_baud_rate(long baud) {
   uint16_t UBRR0_value = ((F_CPU / 16 + baud / 2) / baud - 1);
-	UBRR0H = UBRR0_value >> 8;
-	UBRR0L = UBRR0_value;
+  UBRR0H = UBRR0_value >> 8;
+  UBRR0L = UBRR0_value;
 }
 
 void serial_init(long baud)
 {
   set_baud_rate(baud);
   
-	/* baud doubler off  - Only needed on Uno XXX */
+  /* baud doubler off  - Only needed on Uno XXX */
   UCSR0A &= ~(1 << U2X0);
           
-	// enable rx and tx
+  // enable rx and tx
   UCSR0B |= 1<<RXEN0;
   UCSR0B |= 1<<TXEN0;
 	
-	// enable interrupt on complete reception of a byte
+  // enable interrupt on complete reception of a byte
   UCSR0B |= 1<<RXCIE0;
 	  
-	// defaults to 8-bit, no parity, 1 stop bit
+  // defaults to 8-bit, no parity, 1 stop bit
 }
 
 void serial_write(uint8_t data) {
@@ -71,19 +71,19 @@ void serial_write(uint8_t data) {
   if (next_head == TX_BUFFER_SIZE) { next_head = 0; }
 
   // Wait until there's a space in the buffer
-  while (next_head == tx_buffer_tail) { sleep_mode(); };
+  while (next_head == tx_buffer_tail) { };//sleep_mode(); };
 
   // Store data and advance head
   tx_buffer[tx_buffer_head] = data;
   tx_buffer_head = next_head;
   
   // Enable Data Register Empty Interrupt to make sure tx-streaming is running
-	UCSR0B |=  (1 << UDRIE0); 
+  UCSR0B |=  (1 << UDRIE0); 
 }
 
 // Data Register Empty Interrupt handler
-SIGNAL(USART_UDRE_vect) {  
-  // temporary tx_buffer_tail (to optimize for volatile)
+ISR(USART_UDRE_vect) {  
+  // Temporary tx_buffer_tail (to optimize for volatile)
   uint8_t tail = tx_buffer_tail;
 
   // Send a byte from the buffer	
@@ -101,25 +101,25 @@ SIGNAL(USART_UDRE_vect) {
 
 uint8_t serial_read()
 {
-	if (rx_buffer_head == rx_buffer_tail) {
-		return SERIAL_NO_DATA;
-	} else {
-		uint8_t data = rx_buffer[rx_buffer_tail];
-		rx_buffer_tail++;
-		if (rx_buffer_tail == RX_BUFFER_SIZE) { rx_buffer_tail = 0; }
-		return data;
-	}
+  if (rx_buffer_head == rx_buffer_tail) {
+    return SERIAL_NO_DATA;
+  } else {
+    uint8_t data = rx_buffer[rx_buffer_tail];
+    rx_buffer_tail++;
+    if (rx_buffer_tail == RX_BUFFER_SIZE) { rx_buffer_tail = 0; }
+    return data;
+  }
 }
 
-SIGNAL(USART_RX_vect)
+ISR(USART_RX_vect)
 {
-	uint8_t data = UDR0;
-	uint8_t next_head = rx_buffer_head + 1;
-	if (next_head == RX_BUFFER_SIZE) { next_head = 0; }
+  uint8_t data = UDR0;
+  uint8_t next_head = rx_buffer_head + 1;
+  if (next_head == RX_BUFFER_SIZE) { next_head = 0; }
 
   // Write data to buffer unless it is full.
-	if (next_head != rx_buffer_tail) {
-		rx_buffer[rx_buffer_head] = data;
-		rx_buffer_head = next_head;
-	}
+  if (next_head != rx_buffer_tail) {
+    rx_buffer[rx_buffer_head] = data;
+    rx_buffer_head = next_head;
+  }
 }
diff --git a/stepper.c b/stepper.c
index d8110e5..3f2899c 100644
--- a/stepper.c
+++ b/stepper.c
@@ -79,10 +79,10 @@ static uint8_t cycle_start;     // Cycle start flag to indicate program start an
 static void set_step_events_per_minute(uint32_t steps_per_minute);
 
 // Stepper state initialization
-void st_wake_up() 
+static void st_wake_up() 
 {
   // Initialize stepper output bits
-  out_bits = (0) ^ (settings.invert_mask);
+  out_bits = (0) ^ (settings.invert_mask); 
   // Enable steppers by resetting the stepper disable port
   STEPPERS_DISABLE_PORT &= ~(1<<STEPPERS_DISABLE_BIT);
   // Enable stepper driver interrupt
@@ -90,7 +90,7 @@ void st_wake_up()
 }
 
 // Stepper shutdown
-void st_go_idle() 
+static void st_go_idle() 
 {
   // Cycle finished. Set flag to false.
   cycle_start = false; 
@@ -133,28 +133,24 @@ static uint8_t iterate_trapezoid_cycle_counter()
 // config_step_timer. It pops blocks from the block_buffer and executes them by pulsing the stepper pins appropriately. 
 // It is supported by The Stepper Port Reset Interrupt which it uses to reset the stepper port after each pulse. 
 // The bresenham line tracer algorithm controls all three stepper outputs simultaneously with these two interrupts.
-SIGNAL(TIMER1_COMPA_vect)
+// NOTE: ISR_NOBLOCK allows SIG_OVERFLOW2 to trigger on-time regardless of time in this handler. This is
+// the compiler optimizable equivalent of the old SIGNAL() and sei() method.
+ISR(TIMER1_COMPA_vect,ISR_NOBLOCK)
 {        
-  // TODO: Check if the busy-flag can be eliminated by just disabling this interrupt while we are in it
+  if (busy) { return; } // The busy-flag is used to avoid reentering this interrupt
+  busy = true;
   
-  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
   STEPPING_PORT = (STEPPING_PORT & ~DIRECTION_MASK) | (out_bits & DIRECTION_MASK);
   // Then pulse the stepping pins
   STEPPING_PORT = (STEPPING_PORT & ~STEP_MASK) | out_bits;
   // Reset step pulse reset timer so that The Stepper Port Reset Interrupt can reset the signal after
   // exactly settings.pulse_microseconds microseconds.
-//   TCNT2 = -(((settings.pulse_microseconds-2)*TICKS_PER_MICROSECOND)/8);
-  TCNT2 = -(((settings.pulse_microseconds-2)*TICKS_PER_MICROSECOND) >> 3); // Bit shift divide by 8.
-  
-  busy = true;
-  sei(); // Re enable interrupts (normally disabled while inside an interrupt handler)
-         // ((We re-enable interrupts in order for SIG_OVERFLOW2 to be able to be triggered 
-         // at exactly the right time even if we occasionally spend a lot of time inside this handler.))
+  TCNT2 = -(((settings.pulse_microseconds-2)*TICKS_PER_MICROSECOND) >> 3);
     
   // If there is no current block, attempt to pop one from the buffer
   if (current_block == NULL) {
-    // Anything in the buffer?
+    // Anything in the buffer? If so, initialize next motion.
     current_block = plan_get_current_block();
     if (current_block != NULL) {
       trapezoid_generator_reset();
@@ -256,37 +252,39 @@ SIGNAL(TIMER1_COMPA_vect)
 
 // This interrupt is set up by SIG_OUTPUT_COMPARE1A when it sets the motor port bits. It resets
 // the motor port after a short period (settings.pulse_microseconds) completing one step cycle.
-SIGNAL(TIMER2_OVF_vect)
+ISR(TIMER2_OVF_vect)
 {
-  // reset stepping pins (leave the direction pins)
+  // Reset stepping pins (leave the direction pins)
   STEPPING_PORT = (STEPPING_PORT & ~STEP_MASK) | (settings.invert_mask & STEP_MASK); 
 }
 
 // Initialize and start the stepper motor subsystem
 void st_init()
 {
-	// Configure directions of interface pins
-  STEPPING_DDR   |= STEPPING_MASK;
+  // Configure directions of interface pins
+  STEPPING_DDR |= STEPPING_MASK;
   STEPPING_PORT = (STEPPING_PORT & ~STEPPING_MASK) | settings.invert_mask;
   STEPPERS_DISABLE_DDR |= 1<<STEPPERS_DISABLE_BIT;
   
-	// waveform generation = 0100 = CTC
-	TCCR1B &= ~(1<<WGM13);
-	TCCR1B |=  (1<<WGM12);
-	TCCR1A &= ~(1<<WGM11); 
-	TCCR1A &= ~(1<<WGM10);
+  // waveform generation = 0100 = CTC
+  TCCR1B &= ~(1<<WGM13);
+  TCCR1B |=  (1<<WGM12);
+  TCCR1A &= ~(1<<WGM11); 
+  TCCR1A &= ~(1<<WGM10);
 
-	// output mode = 00 (disconnected)
-	TCCR1A &= ~(3<<COM1A0); 
-	TCCR1A &= ~(3<<COM1B0); 
+  // output mode = 00 (disconnected)
+  TCCR1A &= ~(3<<COM1A0); 
+  TCCR1A &= ~(3<<COM1B0); 
 	
-	// Configure Timer 2
+  // Configure Timer 2
   TCCR2A = 0;         // Normal operation
   TCCR2B = (1<<CS21); // Full speed, 1/8 prescaler
   TIMSK2 |= (1<<TOIE2);      
   
-  set_step_events_per_minute(6000);
+  set_step_events_per_minute(MINIMUM_STEPS_PER_MINUTE);
   trapezoid_tick_cycle_counter = 0;
+  current_block = NULL;
+  busy = false;
   
   // Start in the idle state
   st_go_idle();
@@ -306,30 +304,30 @@ static uint32_t config_step_timer(uint32_t cycles)
   uint16_t prescaler;
   uint32_t actual_cycles;
   if (cycles <= 0xffffL) {
-      ceiling = cycles;
-  prescaler = 0; // prescaler: 0
-  actual_cycles = ceiling;
+    ceiling = cycles;
+    prescaler = 0; // prescaler: 0
+    actual_cycles = ceiling;
   } else if (cycles <= 0x7ffffL) {
-  ceiling = cycles >> 3;
-  prescaler = 1; // prescaler: 8
-  actual_cycles = ceiling * 8L;
+    ceiling = cycles >> 3;
+    prescaler = 1; // prescaler: 8
+    actual_cycles = ceiling * 8L;
   } else if (cycles <= 0x3fffffL) {
-      ceiling =  cycles >> 6;
-  prescaler = 2; // prescaler: 64
-  actual_cycles = ceiling * 64L;
+    ceiling =  cycles >> 6;
+    prescaler = 2; // prescaler: 64
+    actual_cycles = ceiling * 64L;
   } else if (cycles <= 0xffffffL) {
-      ceiling =  (cycles >> 8);
-  prescaler = 3; // prescaler: 256
-  actual_cycles = ceiling * 256L;
+    ceiling =  (cycles >> 8);
+    prescaler = 3; // prescaler: 256
+    actual_cycles = ceiling * 256L;
   } else if (cycles <= 0x3ffffffL) {
-      ceiling = (cycles >> 10);
-  prescaler = 4; // prescaler: 1024
-  actual_cycles = ceiling * 1024L;    
+    ceiling = (cycles >> 10);
+    prescaler = 4; // prescaler: 1024
+    actual_cycles = ceiling * 1024L;    
   } else {
     // Okay, that was slower than we actually go. Just set the slowest speed
-      ceiling = 0xffff;
-  prescaler = 4;
-  actual_cycles = 0xffff * 1024;
+    ceiling = 0xffff;
+    prescaler = 4;
+    actual_cycles = 0xffff * 1024;
   }
   // Set prescaler
   TCCR1B = (TCCR1B & ~(0x07<<CS10)) | ((prescaler+1)<<CS10);
@@ -338,7 +336,8 @@ static uint32_t config_step_timer(uint32_t cycles)
   return(actual_cycles);
 }
 
-static void set_step_events_per_minute(uint32_t steps_per_minute) {
+static void set_step_events_per_minute(uint32_t steps_per_minute) 
+{
   if (steps_per_minute < MINIMUM_STEPS_PER_MINUTE) { steps_per_minute = MINIMUM_STEPS_PER_MINUTE; }
   cycles_per_step_event = config_step_timer((TICKS_PER_MICROSECOND*1000000*60)/steps_per_minute);
 }
@@ -350,7 +349,8 @@ void st_go_home()
 }
 
 // Planner external interface to start stepper interrupt and execute the blocks in queue.
-void st_cycle_start() {
+void st_cycle_start() 
+{
   if (!cycle_start) {
     cycle_start = true;
     st_wake_up();