diff --git a/config.h b/config.h
index d1587fa..3791b31 100644
--- a/config.h
+++ b/config.h
@@ -210,7 +210,7 @@
 // available RAM, like when re-compiling for a Mega or Sanguino. Or decrease if the Arduino
 // begins to crash due to the lack of available RAM or if the CPU is having trouble keeping
 // up with planning new incoming motions as they are executed. 
-// #define BLOCK_BUFFER_SIZE 18  // Uncomment to override default in planner.h.
+// #define BLOCK_BUFFER_SIZE 17  // Uncomment to override default in planner.h.
 
 // Line buffer size from the serial input stream to be executed. Also, governs the size of 
 // each of the startup blocks, as they are each stored as a string of this size. Make sure
@@ -220,7 +220,7 @@
 // can be too small and g-code blocks can get truncated. Officially, the g-code standards 
 // support up to 256 characters. In future versions, this default will be increased, when 
 // we know how much extra memory space we can re-invest into this.
-// #define LINE_BUFFER_SIZE 50  // Uncomment to override default in protocol.h
+// #define LINE_BUFFER_SIZE 70  // Uncomment to override default in protocol.h
   
 // Serial send and receive buffer size. The receive buffer is often used as another streaming
 // buffer to store incoming blocks to be processed by Grbl when its ready. Most streaming
diff --git a/gcode.c b/gcode.c
index b17a34a..d87ce52 100644
--- a/gcode.c
+++ b/gcode.c
@@ -245,7 +245,8 @@ uint8_t gc_execute_line(char *line)
   // If there were any errors parsing this line, we will return right away with the bad news
   if (gc.status_code) { return(gc.status_code); }
   
-  
+  uint8_t i;
+
   /* Execute Commands: Perform by order of execution defined in NIST RS274-NGC.v3, Table 8, pg.41. */
     
   // ([F]: Set feed rate.)
@@ -290,7 +291,6 @@ uint8_t gc_execute_line(char *line)
         else { int_value = gc.coord_select; } // Index P0 as the active coordinate system
         float coord_data[N_AXIS];
         if (!settings_read_coord_data(int_value,coord_data)) { return(STATUS_SETTING_READ_FAIL); }
-        uint8_t i;
         // Update axes defined only in block. Always in machine coordinates. Can change non-active system.
         for (i=0; i<N_AXIS; i++) { // Axes indices are consistent, so loop may be used.
           if (bit_istrue(axis_words,bit(i)) ) {
@@ -312,7 +312,6 @@ uint8_t gc_execute_line(char *line)
       // and absolute and incremental modes.
       if (axis_words) {
         // Apply absolute mode coordinate offsets or incremental mode offsets.
-        uint8_t i;
         for (i=0; i<N_AXIS; i++) { // Axes indices are consistent, so loop may be used.
           if ( bit_istrue(axis_words,bit(i)) ) {
             if (gc.absolute_mode) {
@@ -350,7 +349,6 @@ uint8_t gc_execute_line(char *line)
       } else {
         // Update axes defined only in block. Offsets current system to defined value. Does not update when
         // active coordinate system is selected, but is still active unless G92.1 disables it. 
-        uint8_t i;
         for (i=0; i<N_AXIS; i++) { // Axes indices are consistent, so loop may be used.
           if (bit_istrue(axis_words,bit(i)) ) {
             gc.coord_offset[i] = gc.position[i]-gc.coord_system[i]-target[i];
@@ -385,7 +383,6 @@ uint8_t gc_execute_line(char *line)
     // Convert all target position data to machine coordinates for executing motion. Apply
     // absolute mode coordinate offsets or incremental mode offsets.
     // NOTE: Tool offsets may be appended to these conversions when/if this feature is added.
-    uint8_t i;
     for (i=0; i<N_AXIS; i++) { // Axes indices are consistent, so loop may be used to save flash space.
       if ( bit_istrue(axis_words,bit(i)) ) {
         if (!absolute_override) { // Do not update target in absolute override mode
diff --git a/planner.h b/planner.h
index ce5126d..830735d 100644
--- a/planner.h
+++ b/planner.h
@@ -24,7 +24,7 @@
                  
 // The number of linear motions that can be in the plan at any give time
 #ifndef BLOCK_BUFFER_SIZE
-  #define BLOCK_BUFFER_SIZE 18
+  #define BLOCK_BUFFER_SIZE 17
 #endif
 
 // This struct is used when buffering the setup for each linear movement "nominal" values are as specified in 
diff --git a/protocol.c b/protocol.c
index 56aad3c..a75a141 100644
--- a/protocol.c
+++ b/protocol.c
@@ -37,10 +37,16 @@ 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 protocol_reset_line_buffer()
+{
+  char_counter = 0;
+  iscomment = false;
+}
+
+
 void protocol_init() 
 {
-  char_counter = 0; // Reset line input
-  iscomment = false;
+  protocol_reset_line_buffer(); // Reset line input
   report_init_message(); // Welcome message   
   
   PINOUT_DDR &= ~(PINOUT_MASK); // Set as input pins
@@ -49,6 +55,7 @@ void protocol_init()
   PCICR |= (1 << PINOUT_INT);   // Enable Pin Change Interrupt
 }
 
+
 // Executes user startup script, if stored.
 void protocol_execute_startup() 
 {
@@ -65,6 +72,7 @@ void protocol_execute_startup()
   }  
 }
 
+
 // Pin change interrupt for pin-out commands, i.e. cycle start, feed hold, and reset. Sets
 // only the runtime command execute variable to have the main program execute these when 
 // its ready. This works exactly like the character-based runtime commands when picked off
@@ -305,9 +313,8 @@ void protocol_process()
         // Empty or comment line. Skip block.
         report_status_message(STATUS_OK); // Send status message for syncing purposes.
       }
-      char_counter = 0; // Reset line buffer index
-      iscomment = false; // Reset comment flag
-      
+      protocol_reset_line_buffer();
+            
     } else {
       if (iscomment) {
         // Throw away all comment characters
@@ -324,7 +331,9 @@ void protocol_process()
           // Enable comments flag and ignore all characters until ')' or EOL.
           iscomment = true;
         } else if (char_counter >= LINE_BUFFER_SIZE-1) {
-          // Throw away any characters beyond the end of the line buffer          
+          // Detect line buffer overflow. Report error and reset line buffer.
+          report_status_message(STATUS_OVERFLOW);
+          protocol_reset_line_buffer();
         } else if (c >= 'a' && c <= 'z') { // Upcase lowercase
           line[char_counter++] = c-'a'+'A';
         } else {
diff --git a/protocol.h b/protocol.h
index 209d19d..4d90c1c 100644
--- a/protocol.h
+++ b/protocol.h
@@ -30,7 +30,7 @@
 // memory space we can invest into here or we re-write the g-code parser not to have his 
 // buffer.
 #ifndef LINE_BUFFER_SIZE
-  #define LINE_BUFFER_SIZE 50
+  #define LINE_BUFFER_SIZE 70
 #endif
 
 // Initialize the serial protocol
diff --git a/report.c b/report.c
index c115dc7..8c90a17 100644
--- a/report.c
+++ b/report.c
@@ -76,6 +76,8 @@ void report_status_message(uint8_t status_code)
       printPgmString(PSTR("Alarm lock")); break;
       case STATUS_SOFT_LIMIT_ERROR:
       printPgmString(PSTR("Homing not enabled")); break;
+      case STATUS_OVERFLOW:
+      printPgmString(PSTR("Line overflow")); break; 
     }
     printPgmString(PSTR("\r\n"));
   }
diff --git a/report.h b/report.h
index cd7f42d..7dcfc47 100644
--- a/report.h
+++ b/report.h
@@ -36,6 +36,7 @@
 #define STATUS_IDLE_ERROR 11
 #define STATUS_ALARM_LOCK 12
 #define STATUS_SOFT_LIMIT_ERROR 13
+#define STATUS_OVERFLOW 14
 
 // Define Grbl alarm codes. Less than zero to distinguish alarm error from status error.
 #define ALARM_LIMIT_ERROR -1
diff --git a/stepper.c b/stepper.c
index cac20f3..acb3282 100644
--- a/stepper.c
+++ b/stepper.c
@@ -78,6 +78,7 @@ static volatile uint8_t busy;   // True when "Stepper Driver Interrupt" is being
 //  after which it decelerates until the block is completed. The driver uses constant acceleration, which is applied as
 //  +/- block->rate_delta velocity increments by the midpoint rule at each ACCELERATION_TICKS_PER_SECOND.
 
+
 // 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.
 void st_wake_up() 
@@ -101,6 +102,7 @@ void st_wake_up()
   }
 }
 
+
 // Stepper shutdown
 void st_go_idle() 
 {
@@ -297,6 +299,7 @@ ISR(TIMER2_COMPA_vect)
 // SPINDLE_ENABLE_PORT ^= 1<<SPINDLE_ENABLE_BIT;  
 }
 
+
 // The Stepper Port Reset Interrupt: Timer0 OVF interrupt handles the falling edge of the
 // step pulse. This should always trigger before the next Timer2 COMPA interrupt and independently
 // finish, if Timer2 is disabled after completing a move.
@@ -315,6 +318,7 @@ void st_reset()
   busy = false;
 }
 
+
 // Initialize and start the stepper motor subsystem
 void st_init()
 {
@@ -352,6 +356,7 @@ void st_cycle_start()
   }
 }
 
+
 // Execute a feed hold with deceleration, only during cycle. Called by main program.
 void st_feed_hold() 
 {
@@ -361,6 +366,7 @@ void st_feed_hold()
   }
 }
 
+
 // Reinitializes the cycle plan and stepper system after a feed hold for a resume. Called by 
 // runtime command execution in the main program, ensuring that the planner re-plans safely.
 // NOTE: Bresenham algorithm variables are still maintained through both the planner and stepper
diff --git a/stepper_new.c b/stepper_new.c
new file mode 100644
index 0000000..ae6bd8b
--- /dev/null
+++ b/stepper_new.c
@@ -0,0 +1,424 @@
+/*
+  stepper.c - stepper motor driver: executes motion plans using stepper motors
+  Part of Grbl
+
+  Copyright (c) 2011-2013 Sungeun K. Jeon
+  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 <avr/interrupt.h>
+#include "stepper.h"
+#include "config.h"
+#include "settings.h"
+#include "planner.h"
+
+// Some useful constants
+#define TICKS_PER_MICROSECOND (F_CPU/1000000)
+#define CRUISE_RAMP 0
+#define ACCEL_RAMP 1
+#define DECEL_RAMP 2
+
+// Stepper state variable. Contains running data and trapezoid variables.
+typedef struct {
+  // Used by the bresenham line algorithm
+  int32_t counter_x,        // Counter variables for the bresenham line tracer
+          counter_y, 
+          counter_z;
+  uint32_t event_count;     // Total event count. Retained for feed holds. 
+  uint32_t step_events_remaining;  // Steps remaining in motion
+
+  // Used by Pramod Ranade inverse time algorithm
+  int32_t delta_d;     // Ranade distance traveled per interrupt tick
+  int32_t d_counter;   // Ranade distance traveled since last step event
+  uint8_t ramp_count; // Acceleration interrupt tick counter. 
+  uint8_t ramp_type; // Ramp type variable.
+  uint8_t execute_step; // Flags step execution for each interrupt.
+  
+} stepper_t;
+static stepper_t st;
+static block_t *current_block;  // A pointer to the block currently being traced
+
+// Used by the stepper driver interrupt
+static uint8_t step_pulse_time; // Step pulse reset time after step rise
+static uint8_t out_bits;        // The next stepping-bits to be output
+
+// NOTE: If the main interrupt is guaranteed to be complete before the next interrupt, then
+// this blocking variable is no longer needed. Only here for safety reasons.
+static volatile uint8_t busy;   // True when "Stepper Driver Interrupt" is being serviced. Used to avoid retriggering that handler.
+
+//         __________________________
+//        /|                        |\     _________________         ^
+//       / |                        | \   /|               |\        |
+//      /  |                        |  \ / |               | \       s
+//     /   |                        |   |  |               |  \      p
+//    /    |                        |   |  |               |   \     e
+//   +-----+------------------------+---+--+---------------+----+    e
+//   |               BLOCK 1            |      BLOCK 2          |    d
+//
+//                           time ----->
+// 
+//  The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates by block->rate_delta
+//  until reaching cruising speed block->nominal_rate, and/or until step_events_remaining reaches block->decelerate_after
+//  after which it decelerates until the block is completed. The driver uses constant acceleration, which is applied as
+//  +/- block->rate_delta velocity increments by the midpoint rule at each ACCELERATION_TICKS_PER_SECOND.
+
+
+// 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.
+void st_wake_up() 
+{
+  // Enable steppers by resetting the stepper disable port
+  if (bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)) { 
+    STEPPERS_DISABLE_PORT |= (1<<STEPPERS_DISABLE_BIT); 
+  } else { 
+    STEPPERS_DISABLE_PORT &= ~(1<<STEPPERS_DISABLE_BIT);
+  }
+  if (sys.state == STATE_CYCLE) {
+    // Initialize stepper output bits
+    out_bits = settings.invert_mask; 
+    // Initialize step pulse timing from settings.
+    step_pulse_time = -(((settings.pulse_microseconds-2)*TICKS_PER_MICROSECOND) >> 3);
+    // Enable stepper driver interrupt
+    st.execute_step = false;
+    TCNT0 = 0; // Clear Timer2
+    TIMSK0 |= (1<<OCIE0A); // Enable Timer0 Compare Match A interrupt
+    TCCR0B = (1<<CS21); // Begin Timer0. Full speed, 1/8 prescaler
+  }
+}
+
+
+// Stepper shutdown
+void st_go_idle() 
+{
+  // Disable stepper driver interrupt. Allow Timer2 to finish. It will disable itself.
+  TIMSK0 &= ~(1<<OCIE0A); // Disable Timer0 interrupt
+  TCCR0B = 0; // Disable Timer0
+  busy = false;
+
+  // Disable steppers only upon system alarm activated or by user setting to not be kept enabled.
+  if ((settings.stepper_idle_lock_time != 0xff) || bit_istrue(sys.execute,EXEC_ALARM)) {
+    // Force stepper dwell to lock axes for a defined amount of time to ensure the axes come to a complete
+    // stop and not drift from residual inertial forces at the end of the last movement.
+    delay_ms(settings.stepper_idle_lock_time);
+    if (bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)) { 
+      STEPPERS_DISABLE_PORT &= ~(1<<STEPPERS_DISABLE_BIT); 
+    } else { 
+      STEPPERS_DISABLE_PORT |= (1<<STEPPERS_DISABLE_BIT); 
+    }   
+  }
+}
+
+
+/* "The Stepper Driver Interrupt" - This timer interrupt is the workhorse of Grbl. It is based
+   on the Pramod Ranade inverse time stepper algorithm, where a timer ticks at a constant
+   frequency and uses time-distance counters to track when its the approximate time for any 
+   step event. However, the Ranade algorithm, as described, is susceptible to numerical round-off,
+   meaning that some axes steps may not execute/cause a phasing drift error between multiple axes.
+     Grbl's algorithm differs by using a single Ranade-type time-distance counter to manage
+   a Bresenham line algorithm for multi-axis step events, which ensures the number of steps for
+   each axis are executed exactly and always in phase by inherent algorithm design. In other
+   words, it uses a Bresenham within a Bresenham algorithm, where one tracks time(Ranade) and
+   the other steps.
+     This interrupt 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. */
+// NOTE: Average time in this ISR is: 5 usec iterating timers only, 20-25 usec with step event, or
+// 15 usec when popping a block. So, ensure Ranade frequency and step pulse times work with this.
+
+ISR(TIMER0_COMPA_vect)
+{
+// SPINDLE_ENABLE_PORT ^= 1<<SPINDLE_ENABLE_BIT; // Debug: Used to time ISR
+  
+  // Pulse stepper port pins, if flagged. New block dir will always be set one timer tick 
+  // before any step pulse due to algorithm design.
+  if (st.execute_step) {
+    st.execute_step = false;
+    STEPPING_PORT = ( STEPPING_PORT & ~(DIRECTION_MASK | STEP_MASK) ) | out_bits;
+    TCNT2 = step_pulse_time; // Reload Timer2 counter.
+    TCCR2B = (1<<CS21); // Begin Timer2. Full speed, 1/8 prescaler
+  }
+
+// Assume that this takes less than 5 usec. If not, then this might not work on an 328p.
+// Two sei() commands in two different interrupts will be hard to manage. If the main program
+// can push fast enough, then this might be ok.
+//   sei(); //  ??? The falling edge interrupt needs to fire before the rest of this executes.
+    
+    
+/* 
+1. Upon start, load segment/block.
+   - Set direction bit for entire block early. This never changes.
+   - Load Bresenham variables. Initialize their counters.
+   - If using segments, counters cannot be updated, but this breaks the direction bit? No. Only set when block begins.
+
+   (3) Generate step event. Can take up to an additional 10-15usec for the math.
+
+
+
+Override idea: Main program can request the step event count from the stepper algorithm, which will
+check for the request and write it to a safe variable for the main program. The main program will
+then wait until the request is fulfilled via a flag. From there, the main program can determine
+the safe point from which it can plan. This may require a snapshot of variables. Hopefully this 
+won't take too much time in the interrupt.
+*/    
+    
+  // Iterate inverse time counter. Triggers each Bresenham step event.
+  if (st.delta_d < MINIMUM_STEP_RATE) { st.d_counter -= MINIMUM_STEP_RATE; }
+  else { st.d_counter -= st.delta_d; }
+  
+  // Prepare Bresenham step event, when it's time to do so.
+  if (st.d_counter < 0) {  
+    st.d_counter += current_block->d_next;
+      
+    // Load next step
+    out_bits = current_block->direction_bits; // Reset out_bits and reload direction bits
+    st.execute_step = true;
+    
+    // Execute step displacement profile by Bresenham line algorithm
+    st.counter_x -= current_block->steps_x;
+    if (st.counter_x < 0) {
+      out_bits |= (1<<X_STEP_BIT);
+      st.counter_x += st.event_count;
+    }
+    st.counter_y -= current_block->steps_y;
+    if (st.counter_y < 0) {
+      out_bits |= (1<<Y_STEP_BIT);
+      st.counter_y += st.event_count;
+    }
+    st.counter_z -= current_block->steps_z;
+    if (st.counter_z < 0) {
+      out_bits |= (1<<Z_STEP_BIT);
+      st.counter_z += st.event_count;
+    }
+
+    // Check step events for trapezoid change or end of block.
+    st.step_events_remaining--; // Decrement step events count
+    if (st.step_events_remaining == 0) {
+      // Load next line motion
+    }
+
+    out_bits ^= settings.invert_mask;  // Apply step port invert mask  
+    
+    
+    
+    
+    // TIMSK2 |= (1<<OCIE2B); // Enable Timer2 Compare Match B interrupt
+        
+  }
+// SPINDLE_ENABLE_PORT ^= 1<<SPINDLE_ENABLE_BIT;  
+}
+
+
+// This needs to complete and load before the next timer?
+ISR(TIMER0_COMPB_vect)
+{
+  if (busy) { return; }
+  busy = true;
+  TIMSK0 &= ~(1<<OCIE0B); // Disable Timer2 Compare Match B interrupt
+  sei();
+  
+  if (out_bits & (1<<X_DIRECTION_BIT)) { sys.position[X_AXIS]--; }
+      else { sys.position[X_AXIS]++; }
+    
+          // If current block is finished, reset pointer 
+      current_block = NULL;
+      plan_discard_current_block();
+
+  
+    // If there is no current block, attempt to pop one from the buffer
+  if (current_block == NULL) {
+  
+    // Anything in the buffer? If so, initialize next motion.
+    current_block = plan_get_current_block();
+    if (current_block != NULL) {
+      // By algorithm design, the loading of the next block never coincides with a step event,
+      // since there is always one inverse time tick before a step event occurs. This means
+      // that the Bresenham counter math never is performed at the same time as the loading 
+      // of a block, hence helping minimize total time spent in this interrupt. Also, this
+      // allows the direction bits for the block to be always set one timer tick before the 
+      // first step event.
+
+      // Initialize direction bits for block      
+      out_bits = current_block->direction_bits ^ settings.invert_mask;
+      st.execute_step = true; // Set flag to set direction bits.
+      
+      // Initialize Bresenham variables
+      st.counter_x = (current_block->step_event_count >> 1);
+      st.counter_y = st.counter_x;
+      st.counter_z = st.counter_x;
+      st.event_count = current_block->step_event_count;
+      st.step_events_remaining = st.event_count;
+      
+      // During feed hold, do not update inverse time counter, rate, or ramp type. Keep decelerating.
+      if (sys.state == STATE_CYCLE) {
+        // Initialize Ranade variables
+        st.d_counter = current_block->d_next;  
+        st.delta_d = current_block->initial_rate;
+        st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2;
+        
+        // Initialize ramp type.
+        if (st.step_events_remaining == current_block->decelerate_after) { st.ramp_type = DECEL_RAMP; }
+        else if (st.delta_d == current_block->nominal_rate) { st.ramp_type = CRUISE_RAMP; }
+        else { st.ramp_type = ACCEL_RAMP; }
+      }
+      
+    } else {
+      st_go_idle();
+      bit_true(sys.execute,EXEC_CYCLE_STOP); // Flag main program for cycle end
+      return; // Nothing to do but exit.
+    }  
+  } 
+  
+    // Adjust inverse time counter for ac/de-celerations
+  if (st.ramp_type) {
+    // Tick acceleration ramp counter
+    st.ramp_count--;
+    if (st.ramp_count == 0) {
+      st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK; // Reload ramp counter
+      if (st.ramp_type == ACCEL_RAMP) { // Adjust velocity for acceleration
+        st.delta_d += current_block->rate_delta;
+        if (st.delta_d >= current_block->nominal_rate) { // Reached cruise state.
+          st.ramp_type = CRUISE_RAMP;
+          st.delta_d = current_block->nominal_rate; // Set cruise velocity
+        }
+      } else if (st.ramp_type == DECEL_RAMP) { // Adjust velocity for deceleration
+        if (st.delta_d > current_block->rate_delta) {
+          st.delta_d -= current_block->rate_delta;
+        } else {
+          st.delta_d >>= 1; // Integer divide by 2 until complete. Also prevents overflow.
+        }
+      }
+    }
+  }
+  
+      
+    // Check for feed hold state and execute accordingly.
+    if (sys.state == STATE_HOLD) {
+      if (st.ramp_type != DECEL_RAMP) {
+        st.ramp_type = DECEL_RAMP;
+        st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; 
+      } 
+      if (st.delta_d <= current_block->rate_delta) {
+        st_go_idle();
+        bit_true(sys.execute,EXEC_CYCLE_STOP);
+        return;
+      }
+    }
+  
+        if (st.ramp_type != DECEL_RAMP) {
+        // Acceleration and cruise handled by ramping. Just check for deceleration.
+        if (st.step_events_remaining <=  current_block->decelerate_after) {
+          st.ramp_type = DECEL_RAMP;
+          if (st.step_events_remaining == current_block->decelerate_after) {
+            if (st.delta_d == current_block->nominal_rate) {
+              st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2;  // Set ramp counter for trapezoid
+            } else {
+              st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK-st.ramp_count; // Set ramp counter for triangle
+            }
+          }
+        }
+      }
+    } else {
+  
+  busy = false;
+}
+
+
+// The Stepper Port Reset Interrupt: Timer2 OVF interrupt handles the falling edge of the
+// step pulse. This should always trigger before the next Timer0 COMPA interrupt and independently
+// finish, if Timer0 is disabled after completing a move.
+ISR(TIMER2_OVF_vect)
+{
+  STEPPING_PORT = (STEPPING_PORT & ~STEP_MASK) | (settings.invert_mask & STEP_MASK); 
+  TCCR2B = 0; // Disable timer until needed.
+}
+
+
+// Reset and clear stepper subsystem variables
+void st_reset()
+{
+  memset(&st, 0, sizeof(st));
+  current_block = NULL;
+  busy = false;
+}
+
+
+// Initialize and start the stepper motor subsystem
+void st_init()
+{
+  // 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;
+	
+  // Configure Timer 0
+  TIMSK0 &= ~(1<<OCIE0A); // Disable Timer0 interrupt while configuring it
+  TCCR0B = 0; // Disable Timer2 until needed
+  TCNT0 = 0; // Clear Timer2 counter
+  TCCR0A = (1<<WGM21);  // Set CTC mode
+  OCR0A = (F_CPU/ISR_TICKS_PER_SECOND)/8 - 1; // Set Timer2 CTC rate
+
+  // Configure Timer 2
+  TIMSK2 &= ~(1<<TOIE2);
+  TCCR2A = 0; // Normal operation
+  TCCR2B = 0; // Disable Timer2 until needed
+  TIMSK2 |= (1<<TOIE2); // Enable overflow interrupt
+  
+  // Start in the idle state, but first wake up to check for keep steppers enabled option.
+  st_wake_up();
+  st_go_idle();
+}
+
+
+// Planner external interface to start stepper interrupt and execute the blocks in queue. Called
+// by the main program functions: planner auto-start and run-time command execution.
+void st_cycle_start() 
+{
+  if (sys.state == STATE_QUEUED) {
+    sys.state = STATE_CYCLE;
+    st_wake_up();
+  }
+}
+
+
+// Execute a feed hold with deceleration, only during cycle. Called by main program.
+void st_feed_hold() 
+{
+  if (sys.state == STATE_CYCLE) {
+    sys.state = STATE_HOLD;
+    sys.auto_start = false; // Disable planner auto start upon feed hold.
+  }
+}
+
+
+// Reinitializes the cycle plan and stepper system after a feed hold for a resume. Called by 
+// runtime command execution in the main program, ensuring that the planner re-plans safely.
+// NOTE: Bresenham algorithm variables are still maintained through both the planner and stepper
+// cycle reinitializations. The stepper path should continue exactly as if nothing has happened.
+// Only the planner de/ac-celerations profiles and stepper rates have been updated.
+void st_cycle_reinitialize()
+{
+  if (current_block != NULL) {
+    // Replan buffer from the feed hold stop location.
+    plan_cycle_reinitialize(st.step_events_remaining);
+    st.ramp_type = ACCEL_RAMP;
+    st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; 
+    st.delta_d = 0;
+    sys.state = STATE_QUEUED;
+  } else {
+    sys.state = STATE_IDLE;
+  }
+}