diff --git a/Grbl_Esp32/Custom/atari_1020.cpp b/Grbl_Esp32/Custom/atari_1020.cpp
index 64aa5d68..278e052a 100644
--- a/Grbl_Esp32/Custom/atari_1020.cpp
+++ b/Grbl_Esp32/Custom/atari_1020.cpp
@@ -179,7 +179,7 @@ void atari_home_task(void* pvParameters) {
 void calc_solenoid(float penZ) {
     bool        isPenUp;
     static bool previousPenState = false;
-    uint32_t    solenoid_pen_pulse_len;                    // duty cycle of solenoid
+    uint32_t    solenoid_pen_pulse_len;                     // duty cycle of solenoid
     isPenUp = ((penZ > 0) || (sys.state == State::Alarm));  // is pen above Z0 or is there an alarm
     // if the state has not change, we only count down to the pull time
     if (previousPenState == isPenUp) {
@@ -247,23 +247,17 @@ void atari_next_pen() {
 void user_defined_macro(uint8_t index) {
     char gcode_line[20];
     switch (index) {
-#ifdef MACRO_BUTTON_0_PIN
-        case CONTROL_PIN_INDEX_MACRO_0:
+        case 0:
             grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Pen switch");
             WebUI::inputBuffer.push("$H\r");
             break;
-#endif
-#ifdef MACRO_BUTTON_1_PIN
-        case CONTROL_PIN_INDEX_MACRO_1:
+        case 1:
             grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Color switch");
             atari_next_pen();
             sprintf(gcode_line, "G90G0X%3.2f\r", ATARI_PAPER_WIDTH);  // alway return to right side to reduce home travel stalls
             WebUI::inputBuffer.push(gcode_line);
             break;
-#endif
-#ifdef MACRO_BUTTON_2_PIN
-        case CONTROL_PIN_INDEX_MACRO_2:
-            // feed out some paper and reset the Y 0
+        case 2:
             grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Paper switch");
             WebUI::inputBuffer.push("G0Y-25\r");
             WebUI::inputBuffer.push("G4P0.1\r");  // sync...forces wait for planner to clear
@@ -271,9 +265,7 @@ void user_defined_macro(uint8_t index) {
             gc_sync_position();
             plan_sync_position();
             break;
-#endif
         default:
-            grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Unknown Switch %d", index);
             break;
     }
 }
diff --git a/Grbl_Esp32/Custom/polar_coaster.cpp b/Grbl_Esp32/Custom/polar_coaster.cpp
index ba279da0..85cf3f8a 100644
--- a/Grbl_Esp32/Custom/polar_coaster.cpp
+++ b/Grbl_Esp32/Custom/polar_coaster.cpp
@@ -223,25 +223,15 @@ float abs_angle(float ang) {
 // Polar coaster has macro buttons, this handles those button pushes.
 void user_defined_macro(uint8_t index) {
     switch (index) {
-#ifdef MACRO_BUTTON_0_PIN
-        case CONTROL_PIN_INDEX_MACRO_0:
+        case 0:
             WebUI::inputBuffer.push("$H\r");  // home machine
             break;
-#endif
-#ifdef MACRO_BUTTON_1_PIN
-        case CONTROL_PIN_INDEX_MACRO_1:
+        case 1:
             WebUI::inputBuffer.push("[ESP220]/1.nc\r");  // run SD card file 1.nc
             break;
-#endif
-#ifdef MACRO_BUTTON_2_PIN
-        case CONTROL_PIN_INDEX_MACRO_2:
+        case 2:
             WebUI::inputBuffer.push("[ESP220]/2.nc\r");  // run SD card file 2.nc
             break;
-#endif
-#ifdef MACRO_BUTTON_3_PIN
-        case CONTROL_PIN_INDEX_MACRO_3:
-            break;
-#endif
         default:
             break;
     }
diff --git a/Grbl_Esp32/src/Config.h b/Grbl_Esp32/src/Config.h
index 205db46e..658cade3 100644
--- a/Grbl_Esp32/src/Config.h
+++ b/Grbl_Esp32/src/Config.h
@@ -162,45 +162,42 @@ const int DEFAULT_RADIO_MODE = ESP_RADIO_OFF;
 
 // Define realtime command special characters. These characters are 'picked-off' directly from the
 // serial read data stream and are not passed to the grbl line execution parser. Select characters
-// that do not and must not exist in the streamed g-code program. ASCII control characters may be
+// that do not and must not exist in the streamed GCode program. ASCII control characters may be
 // used, if they are available per user setup. Also, extended ASCII codes (>127), which are never in
-// g-code programs, maybe selected for interface programs.
+// GCode programs, maybe selected for interface programs.
 // NOTE: If changed, manually update help message in report.c.
 
-const uint8_t CMD_RESET         = 0x18;  // ctrl-x.
-const uint8_t CMD_STATUS_REPORT = '?';
-const uint8_t CMD_CYCLE_START   = '~';
-const uint8_t CMD_FEED_HOLD     = '!';
-
 // NOTE: All override realtime commands must be in the extended ASCII character set, starting
 // at character value 128 (0x80) and up to 255 (0xFF). If the normal set of realtime commands,
 // such as status reports, feed hold, reset, and cycle start, are moved to the extended set
 // space, serial.c's RX ISR will need to be modified to accommodate the change.
 
-// const uint8_t CMD_RESET = 0x80;
-// const uint8_t CMD_STATUS_REPORT = 0x81;
-// const uint8_t CMD_CYCLE_START = 0x82;
-// const uint8_t CMD_FEED_HOLD = 0x83;
-const uint8_t CMD_SAFETY_DOOR           = 0x84;
-const uint8_t CMD_JOG_CANCEL            = 0x85;
-const uint8_t CMD_DEBUG_REPORT          = 0x86;  // Only when DEBUG enabled, sends debug report in '{}' braces.
-const uint8_t CMD_FEED_OVR_RESET        = 0x90;  // Restores feed override value to 100%.
-const uint8_t CMD_FEED_OVR_COARSE_PLUS  = 0x91;
-const uint8_t CMD_FEED_OVR_COARSE_MINUS = 0x92;
-const uint8_t CMD_FEED_OVR_FINE_PLUS    = 0x93;
-const uint8_t CMD_FEED_OVR_FINE_MINUS   = 0x94;
-const uint8_t CMD_RAPID_OVR_RESET       = 0x95;  // Restores rapid override value to 100%.
-const uint8_t CMD_RAPID_OVR_MEDIUM      = 0x96;
-const uint8_t CMD_RAPID_OVR_LOW         = 0x97;
-// const uint8_t CMD_RAPID_OVR_EXTRA_LOW = 0x98; // *NOT SUPPORTED*
-const uint8_t CMD_SPINDLE_OVR_RESET        = 0x99;  // Restores spindle override value to 100%.
-const uint8_t CMD_SPINDLE_OVR_COARSE_PLUS  = 0x9A;  // 154
-const uint8_t CMD_SPINDLE_OVR_COARSE_MINUS = 0x9B;
-const uint8_t CMD_SPINDLE_OVR_FINE_PLUS    = 0x9C;
-const uint8_t CMD_SPINDLE_OVR_FINE_MINUS   = 0x9D;
-const uint8_t CMD_SPINDLE_OVR_STOP         = 0x9E;
-const uint8_t CMD_COOLANT_FLOOD_OVR_TOGGLE = 0xA0;
-const uint8_t CMD_COOLANT_MIST_OVR_TOGGLE  = 0xA1;
+enum class Cmd : uint8_t {
+    Reset                 = 0x18,  // Ctrl-X
+    StatusReport          = '?',
+    CycleStart            = '~',
+    FeedHold              = '!',
+    SafetyDoor            = 0x84,
+    JogCancel             = 0x85,
+    DebugReport           = 0x86,  // Only when DEBUG enabled, sends debug report in '{}' braces.
+    FeedOvrReset          = 0x90,  // Restores feed override value to 100%.
+    FeedOvrCoarsePlus     = 0x91,
+    FeedOvrCoarseMinus    = 0x92,
+    FeedOvrFinePlus       = 0x93,
+    FeedOvrFineMinus      = 0x94,
+    RapidOvrReset         = 0x95,  // Restores rapid override value to 100%.
+    RapidOvrMedium        = 0x96,
+    RapidOvrLow           = 0x97,
+    RapidOvrExtraLow      = 0x98,  // *NOT SUPPORTED*
+    SpindleOvrReset       = 0x99,  // Restores spindle override value to 100%.
+    SpindleOvrCoarsePlus  = 0x9A,  // 154
+    SpindleOvrCoarseMinus = 0x9B,
+    SpindleOvrFinePlus    = 0x9C,
+    SpindleOvrFineMinus   = 0x9D,
+    SpindleOvrStop        = 0x9E,
+    CoolantFloodOvrToggle = 0xA0,
+    CoolantMistOvrToggle  = 0xA1,
+};
 
 // If homing is enabled, homing init lock sets Grbl into an alarm state upon power up. This forces
 // the user to perform the homing cycle (or override the locks) before doing anything else. This is
@@ -218,9 +215,9 @@ static const uint8_t NHomingLocateCycle = 1;  // Integer (1-128)
 // If you have a two-axis machine, DON'T USE THIS. Instead, just alter the homing cycle for two-axes.
 #define HOMING_SINGLE_AXIS_COMMANDS  // Default disabled. Uncomment to enable.
 
-// Number of blocks Grbl executes upon startup. These blocks are stored in EEPROM, where the size
+// Number of blocks Grbl executes upon startup. These blocks are stored in non-volatile storage.
 // and addresses are defined in settings.h. With the current settings, up to 2 startup blocks may
-// be stored and executed in order. These startup blocks would typically be used to set the g-code
+// be stored and executed in order. These startup blocks would typically be used to set the GCode
 // parser state depending on user preferences.
 #define N_STARTUP_LINE 2  // Integer (1-2)
 
@@ -248,7 +245,7 @@ static const uint8_t NHomingLocateCycle = 1;  // Integer (1-128)
 // coordinates through Grbl '$#' print parameters.
 #define MESSAGE_PROBE_COORDINATES  // Enabled by default. Comment to disable.
 
-// Enables a second coolant control pin via the mist coolant g-code command M7 on the Arduino Uno
+// Enables a second coolant control pin via the mist coolant GCode command M7 on the Arduino Uno
 // analog pin 4. Only use this option if you require a second coolant control pin.
 // NOTE: The M8 flood coolant control pin on analog pin 3 will still be functional regardless.
 // ESP32 NOTE! This is here for reference only. You enable both M7 and M8 by assigning them a GPIO Pin
@@ -309,24 +306,29 @@ const double SAFETY_DOOR_COOLANT_DELAY = 1.0;  // Float (seconds)
 // Configure rapid, feed, and spindle override settings. These values define the max and min
 // allowable override values and the coarse and fine increments per command received. Please
 // note the allowable values in the descriptions following each define.
-const int DEFAULT_FEED_OVERRIDE          = 100;  // 100%. Don't change this value.
-const int MAX_FEED_RATE_OVERRIDE         = 200;  // Percent of programmed feed rate (100-255). Usually 120% or 200%
-const int MIN_FEED_RATE_OVERRIDE         = 10;   // Percent of programmed feed rate (1-100). Usually 50% or 1%
-const int FEED_OVERRIDE_COARSE_INCREMENT = 10;   // (1-99). Usually 10%.
-const int FEED_OVERRIDE_FINE_INCREMENT   = 1;    // (1-99). Usually 1%.
+namespace FeedOverride {
+    const int Default         = 100;  // 100%. Don't change this value.
+    const int Max             = 200;  // Percent of programmed feed rate (100-255). Usually 120% or 200%
+    const int Min             = 10;   // Percent of programmed feed rate (1-100). Usually 50% or 1%
+    const int CoarseIncrement = 10;   // (1-99). Usually 10%.
+    const int FineIncrement   = 1;    // (1-99). Usually 1%.
+};
+namespace RapidOverride {
+    const int Default  = 100;  // 100%. Don't change this value.
+    const int Medium   = 50;   // Percent of rapid (1-99). Usually 50%.
+    const int Low      = 25;   // Percent of rapid (1-99). Usually 25%.
+    const int ExtraLow = 5;    // Percent of rapid (1-99). Usually 5%.  Not Supported
+};
 
-const int DEFAULT_RAPID_OVERRIDE = 100;  // 100%. Don't change this value.
-const int RAPID_OVERRIDE_MEDIUM  = 50;   // Percent of rapid (1-99). Usually 50%.
-const int RAPID_OVERRIDE_LOW     = 25;   // Percent of rapid (1-99). Usually 25%.
-// const int RAPID_OVERRIDE_EXTRA_LOW = 5; // *NOT SUPPORTED* Percent of rapid (1-99). Usually 5%.
+namespace SpindleSpeedOverride {
+    const int Default         = 100;  // 100%. Don't change this value.
+    const int Max             = 200;  // Percent of programmed spindle speed (100-255). Usually 200%.
+    const int Min             = 10;   // Percent of programmed spindle speed (1-100). Usually 10%.
+    const int CoarseIncrement = 10;   // (1-99). Usually 10%.
+    const int FineIncrement   = 1;    // (1-99). Usually 1%.
+}
 
-const int DEFAULT_SPINDLE_SPEED_OVERRIDE    = 100;  // 100%. Don't change this value.
-const int MAX_SPINDLE_SPEED_OVERRIDE        = 200;  // Percent of programmed spindle speed (100-255). Usually 200%.
-const int MIN_SPINDLE_SPEED_OVERRIDE        = 10;   // Percent of programmed spindle speed (1-100). Usually 10%.
-const int SPINDLE_OVERRIDE_COARSE_INCREMENT = 10;   // (1-99). Usually 10%.
-const int SPINDLE_OVERRIDE_FINE_INCREMENT   = 1;    // (1-99). Usually 1%.
-
-// When a M2 or M30 program end command is executed, most g-code states are restored to their defaults.
+// When a M2 or M30 program end command is executed, most GCode states are restored to their defaults.
 // This compile-time option includes the restoring of the feed, rapid, and spindle speed override values
 // to their default values at program end.
 #define RESTORE_OVERRIDES_AFTER_PROGRAM_END  // Default enabled. Comment to disable.
@@ -367,13 +369,6 @@ const int REPORT_WCO_REFRESH_IDLE_COUNT = 10;  // (2-255) Must be less than or e
 // certain the step segment buffer is increased/decreased to account for these changes.
 const int ACCELERATION_TICKS_PER_SECOND = 100;
 
-// Adaptive Multi-Axis Step Smoothing (AMASS) is an advanced feature that does what its name implies,
-// smoothing the stepping of multi-axis motions. This feature smooths motion particularly at low step
-// frequencies below 10kHz, where the aliasing between axes of multi-axis motions can cause audible
-// noise and shake your machine. At even lower step frequencies, AMASS adapts and provides even better
-// step smoothing. See Stepper.c for more details on the AMASS system works.
-#define ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING  // Default enabled. Comment to disable.
-
 // Sets the maximum step rate allowed to be written as a Grbl setting. This option enables an error
 // check in the settings module to prevent settings values that will exceed this limitation. The maximum
 // step rate is strictly limited by the CPU speed and will change if something other than an AVR running
@@ -438,7 +433,7 @@ const double MINIMUM_FEED_RATE = 1.0;  // (mm/min)
 // bogged down by too many trig calculations.
 const int N_ARC_CORRECTION = 12;  // Integer (1-255)
 
-// The arc G2/3 g-code standard is problematic by definition. Radius-based arcs have horrible numerical
+// The arc G2/3 GCode standard is problematic by definition. Radius-based arcs have horrible numerical
 // errors when arc at semi-circles(pi) or full-circles(2*pi). Offset-based arcs are much more accurate
 // but still have a problem when arcs are full-circles (2*pi). This define accounts for the floating
 // point issues when offset-based arcs are commanded as full circles, but get interpreted as extremely
@@ -491,13 +486,10 @@ const int DWELL_TIME_STEP = 50;  // Integer (1-255) (milliseconds)
 // #define SEGMENT_BUFFER_SIZE 6 // Uncomment to override default in stepper.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
-// to account for the available EEPROM at the defined memory address in settings.h and for
-// the number of desired startup blocks.
+// each of the startup blocks, as they are each stored as a string of this size.
 // NOTE: 80 characters is not a problem except for extreme cases, but the line buffer size
-// 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.
+// can be too small and GCode blocks can get truncated. Officially, the GCode standards
+// support up to 256 characters.
 // #define LINE_BUFFER_SIZE 80  // Uncomment to override default in protocol.h
 
 // Serial send and receive buffer size. The receive buffer is often used as another streaming
@@ -545,19 +537,9 @@ const int DEBOUNCE_PERIOD = 32;  // in milliseconds default 32 microseconds
 
 // Enable the '$RST=*', '$RST=$', and '$RST=#' eeprom restore commands. There are cases where
 // these commands may be undesirable. Simply comment the desired macro to disable it.
-// NOTE: See SETTINGS_RESTORE_ALL macro for customizing the `$RST=*` command.
-#define ENABLE_RESTORE_EEPROM_WIPE_ALL          // '$RST=*' Default enabled. Comment to disable.
-#define ENABLE_RESTORE_EEPROM_DEFAULT_SETTINGS  // '$RST=$' Default enabled. Comment to disable.
-#define ENABLE_RESTORE_EEPROM_CLEAR_PARAMETERS  // '$RST=#' Default enabled. Comment to disable.
-
-// Defines the EEPROM data restored upon a settings version change and `$RST=*` command. Whenever the
-// the settings or other EEPROM data structure changes between Grbl versions, Grbl will automatically
-// wipe and restore the EEPROM. This macro controls what data is wiped and restored. This is useful
-// particularily for OEMs that need to retain certain data. For example, the BUILD_INFO string can be
-// written into the Arduino EEPROM via a seperate .INO sketch to contain product data. Altering this
-// macro to not restore the build info EEPROM will ensure this data is retained after firmware upgrades.
-// NOTE: Uncomment to override defaults in settings.h
-// #define SETTINGS_RESTORE_ALL (SETTINGS_RESTORE_DEFAULTS | SETTINGS_RESTORE_PARAMETERS | SETTINGS_RESTORE_STARTUP_LINES | SETTINGS_RESTORE_BUILD_INFO)
+#define ENABLE_RESTORE_WIPE_ALL          // '$RST=*' Default enabled. Comment to disable.
+#define ENABLE_RESTORE_DEFAULT_SETTINGS  // '$RST=$' Default enabled. Comment to disable.
+#define ENABLE_RESTORE_PARAMETERS  // '$RST=#' Default enabled. Comment to disable.
 
 // Additional settings have been added to the original set that you see with the $$ command
 // Some senders may not be able to parse anything different from the original set
@@ -565,30 +547,20 @@ const int DEBOUNCE_PERIOD = 32;  // in milliseconds default 32 microseconds
 // Default is off to limit support issues...you can enable here or in your machine definition file
 // #define SHOW_EXTENDED_SETTINGS
 
-// Enable the '$I=(string)' build info write command. If disabled, any existing build info data must
-// be placed into EEPROM via external means with a valid checksum value. This macro option is useful
-// to prevent this data from being over-written by a user, when used to store OEM product data.
-// NOTE: If disabled and to ensure Grbl can never alter the build info line, you'll also need to enable
-// the SETTING_RESTORE_ALL macro above and remove SETTINGS_RESTORE_BUILD_INFO from the mask.
-// NOTE: See the included grblWrite_BuildInfo.ino example file to write this string seperately.
-#define ENABLE_BUILD_INFO_WRITE_COMMAND  // '$I=' Default enabled. Comment to disable.
-
-// AVR processors require all interrupts to be disabled during an EEPROM write. This includes both
-// the stepper ISRs and serial comm ISRs. In the event of a long EEPROM write, this ISR pause can
-// cause active stepping to lose position and serial receive data to be lost. This configuration
-// option forces the planner buffer to completely empty whenever the EEPROM is written to prevent
-// any chance of lost steps.
-// However, this doesn't prevent issues with lost serial RX data during an EEPROM write, especially
-// if a GUI is premptively filling up the serial RX buffer simultaneously. It's highly advised for
-// GUIs to flag these gcodes (G10,G28.1,G30.1) to always wait for an 'ok' after a block containing
-// one of these commands before sending more data to eliminate this issue.
-// NOTE: Most EEPROM write commands are implicitly blocked during a job (all '$' commands). However,
-// coordinate set g-code commands (G10,G28/30.1) are not, since they are part of an active streaming
-// job. At this time, this option only forces a planner buffer sync with these g-code commands.
-#define FORCE_BUFFER_SYNC_DURING_EEPROM_WRITE  // Default enabled. Comment to disable.
+// Writing to non-volatile storage (NVS) can take a long time and interfere with timely instruction
+// execution, causing problems for the stepper ISRs and serial comm ISRs and subsequent loss of
+// stepper position and serial data. This configuration option forces the planner buffer to completely
+// empty whenever the NVS is written, to prevent any chance of lost steps.
+// It doesn't prevent loss of serial Rx data, especially if a GUI is premptively filling up the
+// serial Rx buffer.  GUIs should detect GCodes that write to NVS - notably G10,G28.1,G30.1 -
+// and wait for an 'ok' before sending more data.
+// NOTE: Most setting changes - $ commands - are blocked when a job is running. Coordinate setting
+// GCode commands (G10,G28/30.1) are not blocked, since they are part of an active streaming job.
+// This option forces a planner buffer sync only with such GCode commands.
+#define FORCE_BUFFER_SYNC_DURING_NVS_WRITE  // Default enabled. Comment to disable.
 
 // In Grbl v0.9 and prior, there is an old outstanding bug where the `WPos:` work position reported
-// may not correlate to what is executing, because `WPos:` is based on the g-code parser state, which
+// may not correlate to what is executing, because `WPos:` is based on the GCode parser state, which
 // can be several motions behind. This option forces the planner buffer to empty, sync, and stop
 // motion whenever there is a command that alters the work coordinate offsets `G10,G43.1,G92,G54-59`.
 // This is the simplest way to ensure `WPos:` is always correct. Fortunately, it's exceedingly rare
@@ -624,8 +596,8 @@ const double PARKING_PULLOUT_INCREMENT = 5.0;    // Spindle pull-out and plunge
 
 // Enables a special set of M-code commands that enables and disables the parking motion.
 // These are controlled by `M56`, `M56 P1`, or `M56 Px` to enable and `M56 P0` to disable.
-// The command is modal and will be set after a planner sync. Since it is g-code, it is
-// executed in sync with g-code commands. It is not a real-time command.
+// The command is modal and will be set after a planner sync. Since it is GCode, it is
+// executed in sync with GCode commands. It is not a real-time command.
 // NOTE: PARKING_ENABLE is required. By default, M56 is active upon initialization. Use
 // DEACTIVATE_PARKING_UPON_INIT to set M56 P0 as the power-up default.
 // #define ENABLE_PARKING_OVERRIDE_CONTROL   // Default disabled. Uncomment to enable
diff --git a/Grbl_Esp32/src/CoolantControl.cpp b/Grbl_Esp32/src/CoolantControl.cpp
index ca14a8f1..6e823c77 100644
--- a/Grbl_Esp32/src/CoolantControl.cpp
+++ b/Grbl_Esp32/src/CoolantControl.cpp
@@ -24,12 +24,25 @@
 #include "Grbl.h"
 
 void coolant_init() {
+    static bool init_message = true;  // used to show messages only once.
+
 #ifdef COOLANT_FLOOD_PIN
     pinMode(COOLANT_FLOOD_PIN, OUTPUT);
 #endif
 #ifdef COOLANT_MIST_PIN
     pinMode(COOLANT_MIST_PIN, OUTPUT);
 #endif
+
+    if (init_message) {
+#ifdef COOLANT_FLOOD_PIN
+        grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Flood coolant on pin %s", pinName(COOLANT_FLOOD_PIN).c_str());
+#endif
+#ifdef COOLANT_MIST_PIN
+        grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Mist coolant on pin %s", pinName(COOLANT_MIST_PIN).c_str());
+#endif
+        init_message = false;
+    }
+
     coolant_stop();
 }
 
diff --git a/Grbl_Esp32/src/Eeprom.cpp b/Grbl_Esp32/src/Eeprom.cpp
index d88d9bc2..083570c1 100644
--- a/Grbl_Esp32/src/Eeprom.cpp
+++ b/Grbl_Esp32/src/Eeprom.cpp
@@ -1,5 +1,5 @@
 /*
-  Eeprom.cpp - Header for system level commands and real-time processes
+  Eeprom.cpp - Coordinate data stored in EEPROM
   Part of Grbl
   Copyright (c) 2014-2016 Sungeun K. Jeon for Gnea Research LLC
 
@@ -57,3 +57,25 @@ int memcpy_from_eeprom_with_checksum(char* destination, unsigned int source, uns
     }
     return (checksum == EEPROM.read(source));
 }
+
+// Read selected coordinate data from EEPROM. Updates pointed coord_data value.
+// This is now a compatibility routine that is used to propagate coordinate data
+// in the old EEPROM format to the new tagged NVS format.
+bool old_settings_read_coord_data(uint8_t coord_select, float* coord_data) {
+    uint32_t addr = coord_select * (sizeof(float) * N_AXIS + 1) + EEPROM_ADDR_PARAMETERS;
+    if (!(memcpy_from_eeprom_with_old_checksum((char*)coord_data, addr, sizeof(float) * N_AXIS)) &&
+        !(memcpy_from_eeprom_with_checksum((char*)coord_data, addr, sizeof(float) * MAX_N_AXIS))) {
+        // Reset with default zero vector
+        clear_vector_float(coord_data);
+        // The old code used to rewrite the zeroed data but now that is done
+        // elsewhere, in a different format.
+        return false;
+    }
+    return true;
+}
+
+// Allow iteration over CoordIndex values
+CoordIndex& operator ++ (CoordIndex& i) {
+    i = static_cast<CoordIndex>(static_cast<uint8_t>(i) + 1);
+    return i;
+}
diff --git a/Grbl_Esp32/src/Eeprom.h b/Grbl_Esp32/src/Eeprom.h
index 0a69ec3e..32d66c65 100644
--- a/Grbl_Esp32/src/Eeprom.h
+++ b/Grbl_Esp32/src/Eeprom.h
@@ -22,8 +22,40 @@
 
 #include "Grbl.h"
 
+// Define EEPROM memory address location values for saved coordinate data.
+const int EEPROM_SIZE            = 1024U;
+const int EEPROM_ADDR_PARAMETERS = 512U;
+
 //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, const char* source, unsigned int size);
 int  memcpy_from_eeprom_with_checksum(char* destination, unsigned int source, unsigned int size);
 int  memcpy_from_eeprom_with_old_checksum(char* destination, unsigned int source, unsigned int size);
+
+// Reads selected coordinate data from EEPROM
+bool old_settings_read_coord_data(uint8_t coord_select, float* coord_data);
+
+// Various places in the code access saved coordinate system data
+// by a small integer index according to the values below.
+enum CoordIndex : uint8_t{
+    Begin = 0,
+    G54 = Begin,
+    G55,
+    G56,
+    G57,
+    G58,
+    G59,
+    // To support 9 work coordinate systems it would be necessary to define
+    // the following 3 and modify GCode.cpp to support G59.1, G59.2, G59.3
+    // G59_1,
+    // G59_2,
+    // G59_3,
+    NWCSystems,
+    G28 = NWCSystems,
+    G30,
+    // G92_2,
+    // G92_3,
+    End,
+};
+// Allow iteration over CoordIndex values
+CoordIndex& operator ++ (CoordIndex& i);
diff --git a/Grbl_Esp32/src/GCode.cpp b/Grbl_Esp32/src/GCode.cpp
index 0761eafd..0134952e 100644
--- a/Grbl_Esp32/src/GCode.cpp
+++ b/Grbl_Esp32/src/GCode.cpp
@@ -873,10 +873,10 @@ Error gc_execute_line(char* line, uint8_t client) {
         }
     }
     // [15. Coordinate system selection ]: *N/A. Error, if cutter radius comp is active.
-    // TODO: An EEPROM read of the coordinate data may require a buffer sync when the cycle
+    // TODO: Reading the coordinate data may require a buffer sync when the cycle
     // is active. The read pauses the processor temporarily and may cause a rare crash. For
     // future versions on processors with enough memory, all coordinate data should be stored
-    // in memory and written to EEPROM only when there is not a cycle active.
+    // in memory and written to non-volatile storage only when there is not a cycle active.
     float block_coord_system[MAX_N_AXIS];
     memcpy(block_coord_system, gc_state.coord_system, sizeof(gc_state.coord_system));
     if (bit_istrue(command_words, bit(ModalGroup::MG12))) {  // Check if called in block
@@ -1002,7 +1002,7 @@ Error gc_execute_line(char* line, uint8_t client) {
                 case NonModal::GoHome0:  // G28
                 case NonModal::GoHome1:  // G30
                     // [G28/30 Errors]: Cutter compensation is enabled.
-                    // Retreive G28/30 go-home position data (in machine coordinates) from EEPROM
+                    // Retreive G28/30 go-home position data (in machine coordinates) from non-volatile storage
                     if (gc_block.non_modal_command == NonModal::GoHome0) {
                         coords[CoordIndex::G28]->get(coord_data);
                     } else {  // == NonModal::GoHome1
@@ -1502,9 +1502,6 @@ Error gc_execute_line(char* line, uint8_t client) {
     if (gc_state.modal.motion != Motion::None) {
         if (axis_command == AxisCommand::MotionMode) {
             GCUpdatePos gc_update_pos = GCUpdatePos::Target;
-#ifdef USE_I2S_STEPS
-            stepper_id_t save_stepper = current_stepper;
-#endif
             if (gc_state.modal.motion == Motion::Linear) {
                 //mc_line(gc_block.values.xyz, pl_data);
                 mc_line_kins(gc_block.values.xyz, pl_data, gc_state.position);
@@ -1527,12 +1524,6 @@ Error gc_execute_line(char* line, uint8_t client) {
                 // upon a successful probing cycle, the machine position and the returned value should be the same.
 #ifndef ALLOW_FEED_OVERRIDE_DURING_PROBE_CYCLES
                 pl_data->motion.noFeedOverride = 1;
-#endif
-#ifdef USE_I2S_STEPS
-                save_stepper = current_stepper;  // remember the stepper
-                if (save_stepper == ST_I2S_STREAM) {
-                    stepper_switch(ST_I2S_STATIC);  // Change the stepper to reduce the delay for accurate probing.
-                }
 #endif
                 gc_update_pos = mc_probe_cycle(gc_block.values.xyz, pl_data, gc_parser_flags);
             }
@@ -1544,11 +1535,6 @@ Error gc_execute_line(char* line, uint8_t client) {
             } else if (gc_update_pos == GCUpdatePos::System) {
                 gc_sync_position();  // gc_state.position[] = sys_position
             }                        // == GCUpdatePos::None
-#ifdef USE_I2S_STEPS
-            if (save_stepper == ST_I2S_STREAM && current_stepper != ST_I2S_STREAM) {
-                stepper_switch(ST_I2S_STREAM);  // Put the stepper back on.
-            }
-#endif
         }
     }
     // [21. Program flow ]:
@@ -1565,8 +1551,8 @@ Error gc_execute_line(char* line, uint8_t client) {
         case ProgramFlow::Paused:
             protocol_buffer_synchronize();  // Sync and finish all remaining buffered motions before moving on.
             if (sys.state != State::CheckMode) {
-                system_set_exec_state_flag(EXEC_FEED_HOLD);  // Use feed hold for program pause.
-                protocol_execute_realtime();                 // Execute suspend.
+                sys_rt_exec_state.bit.feedHold = true;  // Use feed hold for program pause.
+                protocol_execute_realtime();            // Execute suspend.
             }
             break;
         case ProgramFlow::CompletedM2:
@@ -1594,9 +1580,9 @@ Error gc_execute_line(char* line, uint8_t client) {
 #endif
             // gc_state.modal.override = OVERRIDE_DISABLE; // Not supported.
 #ifdef RESTORE_OVERRIDES_AFTER_PROGRAM_END
-            sys.f_override        = DEFAULT_FEED_OVERRIDE;
-            sys.r_override        = DEFAULT_RAPID_OVERRIDE;
-            sys.spindle_speed_ovr = DEFAULT_SPINDLE_SPEED_OVERRIDE;
+            sys.f_override        = FeedOverride::Default;
+            sys.r_override        = RapidOverride::Default;
+            sys.spindle_speed_ovr = SpindleSpeedOverride::Default;
 #endif
             // Execute coordinate change and spindle/coolant stop.
             if (sys.state != State::CheckMode) {
diff --git a/Grbl_Esp32/src/GCode.h b/Grbl_Esp32/src/GCode.h
index 1f594bfe..4b070459 100644
--- a/Grbl_Esp32/src/GCode.h
+++ b/Grbl_Esp32/src/GCode.h
@@ -163,15 +163,6 @@ struct CoolantState {
 
 // Modal Group M8: Coolant control
 // Modal Group M9: Override control
-enum class Override : uint8_t {
-#ifdef DEACTIVATE_PARKING_UPON_INIT
-    Disabled      = 0,  // (Default: Must be zero)
-    ParkingMotion = 1,  // M56
-#else
-    ParkingMotion = 0,  // M56 (Default: Must be zero)
-    Disabled      = 1,  // Parking disabled.
-#endif
-};
 
 // Modal Group M10: User I/O control
 enum class IoControl : uint8_t {
@@ -286,7 +277,7 @@ typedef struct {
     float position[MAX_N_AXIS];  // Where the interpreter considers the tool to be at this point in the code
 
     float coord_system[MAX_N_AXIS];  // Current work coordinate system (G54+). Stores offset from absolute machine
-    // position in mm. Loaded from EEPROM when called.
+    // position in mm. Loaded from non-volatile storage when called.
     float coord_offset[MAX_N_AXIS];  // Retains the G92 coordinate offset (work coordinates) relative to
     // machine zero in mm. Non-persistent. Cleared upon reset and boot.
     float tool_length_offset;  // Tracks tool length offset value when enabled.
diff --git a/Grbl_Esp32/src/Grbl.cpp b/Grbl_Esp32/src/Grbl.cpp
index 737bb63e..7a29bc4f 100644
--- a/Grbl_Esp32/src/Grbl.cpp
+++ b/Grbl_Esp32/src/Grbl.cpp
@@ -37,7 +37,7 @@ void grbl_init() {
 #ifdef MACHINE_NAME
     report_machine_type(CLIENT_SERIAL);
 #endif
-    settings_init();  // Load Grbl settings from EEPROM
+    settings_init();  // Load Grbl settings from non-volatile storage
     stepper_init();   // Configure stepper pins and interrupt timers
     init_motors();
     system_ini();  // Configure pinout pins and pin-change interrupt (Renamed due to conflict with esp32 files)
@@ -80,16 +80,20 @@ static void reset_variables() {
     State prior_state = sys.state;
     memset(&sys, 0, sizeof(system_t));  // Clear system struct variable.
     sys.state             = prior_state;
-    sys.f_override        = DEFAULT_FEED_OVERRIDE;              // Set to 100%
-    sys.r_override        = DEFAULT_RAPID_OVERRIDE;             // Set to 100%
-    sys.spindle_speed_ovr = DEFAULT_SPINDLE_SPEED_OVERRIDE;     // Set to 100%
+    sys.f_override        = FeedOverride::Default;              // Set to 100%
+    sys.r_override        = RapidOverride::Default;             // Set to 100%
+    sys.spindle_speed_ovr = SpindleSpeedOverride::Default;      // Set to 100%
     memset(sys_probe_position, 0, sizeof(sys_probe_position));  // Clear probe position.
-    sys_probe_state                = 0;
-    sys_rt_exec_state              = 0;
-    cycle_stop                     = false;
-    sys_rt_exec_motion_override    = 0;
-    sys_rt_exec_accessory_override = 0;
-    system_clear_exec_alarm();
+
+    sys_probe_state                      = Probe::Off;
+    sys_rt_exec_state.value              = 0;
+    sys_rt_exec_accessory_override.value = 0;
+    sys_rt_exec_alarm                    = ExecAlarm::None;
+    cycle_stop                           = false;
+    sys_rt_f_override                    = FeedOverride::Default;
+    sys_rt_r_override                    = RapidOverride::Default;
+    sys_rt_s_override                    = SpindleSpeedOverride::Default;
+
     // Reset Grbl primary systems.
     serial_reset_read_buffer(CLIENT_ALL);  // Clear serial read buffer
     gc_init();                             // Set g-code parser to default state
diff --git a/Grbl_Esp32/src/Grbl.h b/Grbl_Esp32/src/Grbl.h
index 2f3cbf92..b2e9c2d8 100644
--- a/Grbl_Esp32/src/Grbl.h
+++ b/Grbl_Esp32/src/Grbl.h
@@ -23,7 +23,7 @@
 // Grbl versioning system
 
 const char* const GRBL_VERSION       = "1.3a";
-const char* const GRBL_VERSION_BUILD = "20201004";
+const char* const GRBL_VERSION_BUILD = "20201022";
 
 //#include <sdkconfig.h>
 #include <Arduino.h>
@@ -41,18 +41,17 @@ const char* const GRBL_VERSION_BUILD = "20201004";
 
 #include "Defaults.h"
 #include "Error.h"
-#include "SettingsStorage.h"
+#include "Eeprom.h"
 #include "WebUI/Authentication.h"
 #include "WebUI/Commands.h"
+#include "Probe.h"
 #include "System.h"
 
 #include "GCode.h"
 #include "Planner.h"
-#include "Eeprom.h"
 #include "CoolantControl.h"
 #include "Limits.h"
 #include "MotionControl.h"
-#include "Probe.h"
 #include "Protocol.h"
 #include "Report.h"
 #include "Serial.h"
@@ -88,9 +87,7 @@ const char* const GRBL_VERSION_BUILD = "20201004";
 #    endif
 #endif
 
-#ifdef USE_I2S_OUT
-#    include "I2SOut.h"
-#endif
+#include "I2SOut.h"
 
 void grbl_init();
 void run_once();
diff --git a/Grbl_Esp32/src/I2SOut.cpp b/Grbl_Esp32/src/I2SOut.cpp
index 419caf23..d41fed02 100644
--- a/Grbl_Esp32/src/I2SOut.cpp
+++ b/Grbl_Esp32/src/I2SOut.cpp
@@ -42,21 +42,27 @@
  */
 #include "Config.h"
 
-#ifdef USE_I2S_OUT
+// This block of #includes is necessary for Report.h
+#include "Error.h"
+#include "WebUI/Authentication.h"
+#include "WebUI/ESPResponse.h"
+#include "Probe.h"
+#include "System.h"
+#include "Report.h"
 
-#    include <FreeRTOS.h>
-#    include <driver/periph_ctrl.h>
-#    include <rom/lldesc.h>
-#    include <soc/i2s_struct.h>
-#    include <freertos/queue.h>
+#include <FreeRTOS.h>
+#include <driver/periph_ctrl.h>
+#include <rom/lldesc.h>
+#include <soc/i2s_struct.h>
+#include <freertos/queue.h>
 
-#    include <stdatomic.h>
+#include <stdatomic.h>
 
-#    include "Pins.h"
-#    include "I2SOut.h"
+#include "Pins.h"
+#include "I2SOut.h"
 
 // Always enable I2S streaming logic
-#    define USE_I2S_OUT_STREAM_IMPL
+#define USE_I2S_OUT_STREAM_IMPL
 
 //
 // Configrations for DMA connected I2S
@@ -78,7 +84,7 @@ const int I2S_SAMPLE_SIZE   = 4;                                    /* 4 bytes,
 const int DMA_SAMPLE_COUNT  = I2S_OUT_DMABUF_LEN / I2S_SAMPLE_SIZE; /* number of samples per buffer */
 const int SAMPLE_SAFE_COUNT = (20 / I2S_OUT_USEC_PER_PULSE);        /* prevent buffer overrun (GRBL's $0 should be less than or equal 20) */
 
-#    ifdef USE_I2S_OUT_STREAM_IMPL
+#ifdef USE_I2S_OUT_STREAM_IMPL
 typedef struct {
     uint32_t**   buffers;
     uint32_t*    current;
@@ -89,39 +95,39 @@ typedef struct {
 
 static i2s_out_dma_t o_dma;
 static intr_handle_t i2s_out_isr_handle;
-#    endif
+#endif
 
 // output value
 static atomic_uint_least32_t i2s_out_port_data = ATOMIC_VAR_INIT(0);
 
 // inner lock
 static portMUX_TYPE i2s_out_spinlock = portMUX_INITIALIZER_UNLOCKED;
-#    define I2S_OUT_ENTER_CRITICAL()                                                                                                       \
-        do {                                                                                                                               \
-            if (xPortInIsrContext()) {                                                                                                     \
-                portENTER_CRITICAL_ISR(&i2s_out_spinlock);                                                                                 \
-            } else {                                                                                                                       \
-                portENTER_CRITICAL(&i2s_out_spinlock);                                                                                     \
-            }                                                                                                                              \
-        } while (0)
-#    define I2S_OUT_EXIT_CRITICAL()                                                                                                        \
-        do {                                                                                                                               \
-            if (xPortInIsrContext()) {                                                                                                     \
-                portEXIT_CRITICAL_ISR(&i2s_out_spinlock);                                                                                  \
-            } else {                                                                                                                       \
-                portEXIT_CRITICAL(&i2s_out_spinlock);                                                                                      \
-            }                                                                                                                              \
-        } while (0)
-#    define I2S_OUT_ENTER_CRITICAL_ISR() portENTER_CRITICAL_ISR(&i2s_out_spinlock)
-#    define I2S_OUT_EXIT_CRITICAL_ISR() portEXIT_CRITICAL_ISR(&i2s_out_spinlock)
+#define I2S_OUT_ENTER_CRITICAL()                                                                                                           \
+    do {                                                                                                                                   \
+        if (xPortInIsrContext()) {                                                                                                         \
+            portENTER_CRITICAL_ISR(&i2s_out_spinlock);                                                                                     \
+        } else {                                                                                                                           \
+            portENTER_CRITICAL(&i2s_out_spinlock);                                                                                         \
+        }                                                                                                                                  \
+    } while (0)
+#define I2S_OUT_EXIT_CRITICAL()                                                                                                            \
+    do {                                                                                                                                   \
+        if (xPortInIsrContext()) {                                                                                                         \
+            portEXIT_CRITICAL_ISR(&i2s_out_spinlock);                                                                                      \
+        } else {                                                                                                                           \
+            portEXIT_CRITICAL(&i2s_out_spinlock);                                                                                          \
+        }                                                                                                                                  \
+    } while (0)
+#define I2S_OUT_ENTER_CRITICAL_ISR() portENTER_CRITICAL_ISR(&i2s_out_spinlock)
+#define I2S_OUT_EXIT_CRITICAL_ISR() portEXIT_CRITICAL_ISR(&i2s_out_spinlock)
 
 static int i2s_out_initialized = 0;
 
-#    ifdef USE_I2S_OUT_STREAM_IMPL
-static volatile uint64_t             i2s_out_pulse_period;
-static uint64_t                      i2s_out_remain_time_until_next_pulse;  // Time remaining until the next pulse (μsec)
+#ifdef USE_I2S_OUT_STREAM_IMPL
+static volatile uint32_t             i2s_out_pulse_period;
+static uint32_t                      i2s_out_remain_time_until_next_pulse;  // Time remaining until the next pulse (μsec)
 static volatile i2s_out_pulse_func_t i2s_out_pulse_func;
-#    endif
+#endif
 
 static uint8_t i2s_out_ws_pin   = 255;
 static uint8_t i2s_out_bck_pin  = 255;
@@ -131,24 +137,24 @@ static volatile i2s_out_pulser_status_t i2s_out_pulser_status = PASSTHROUGH;
 
 // outer lock
 static portMUX_TYPE i2s_out_pulser_spinlock = portMUX_INITIALIZER_UNLOCKED;
-#    define I2S_OUT_PULSER_ENTER_CRITICAL()                                                                                                \
-        do {                                                                                                                               \
-            if (xPortInIsrContext()) {                                                                                                     \
-                portENTER_CRITICAL_ISR(&i2s_out_pulser_spinlock);                                                                          \
-            } else {                                                                                                                       \
-                portENTER_CRITICAL(&i2s_out_pulser_spinlock);                                                                              \
-            }                                                                                                                              \
-        } while (0)
-#    define I2S_OUT_PULSER_EXIT_CRITICAL()                                                                                                 \
-        do {                                                                                                                               \
-            if (xPortInIsrContext()) {                                                                                                     \
-                portEXIT_CRITICAL_ISR(&i2s_out_pulser_spinlock);                                                                           \
-            } else {                                                                                                                       \
-                portEXIT_CRITICAL(&i2s_out_pulser_spinlock);                                                                               \
-            }                                                                                                                              \
-        } while (0)
-#    define I2S_OUT_PULSER_ENTER_CRITICAL_ISR() portENTER_CRITICAL_ISR(&i2s_out_pulser_spinlock)
-#    define I2S_OUT_PULSER_EXIT_CRITICAL_ISR() portEXIT_CRITICAL_ISR(&i2s_out_pulser_spinlock)
+#define I2S_OUT_PULSER_ENTER_CRITICAL()                                                                                                    \
+    do {                                                                                                                                   \
+        if (xPortInIsrContext()) {                                                                                                         \
+            portENTER_CRITICAL_ISR(&i2s_out_pulser_spinlock);                                                                              \
+        } else {                                                                                                                           \
+            portENTER_CRITICAL(&i2s_out_pulser_spinlock);                                                                                  \
+        }                                                                                                                                  \
+    } while (0)
+#define I2S_OUT_PULSER_EXIT_CRITICAL()                                                                                                     \
+    do {                                                                                                                                   \
+        if (xPortInIsrContext()) {                                                                                                         \
+            portEXIT_CRITICAL_ISR(&i2s_out_pulser_spinlock);                                                                               \
+        } else {                                                                                                                           \
+            portEXIT_CRITICAL(&i2s_out_pulser_spinlock);                                                                                   \
+        }                                                                                                                                  \
+    } while (0)
+#define I2S_OUT_PULSER_ENTER_CRITICAL_ISR() portENTER_CRITICAL_ISR(&i2s_out_pulser_spinlock)
+#define I2S_OUT_PULSER_EXIT_CRITICAL_ISR() portEXIT_CRITICAL_ISR(&i2s_out_pulser_spinlock)
 
 //
 // Internal functions
@@ -163,13 +169,13 @@ static inline void gpio_matrix_out_check(uint8_t gpio, uint32_t signal_idx, bool
 }
 
 static inline void i2s_out_single_data() {
-#    if I2S_OUT_NUM_BITS == 16
+#if I2S_OUT_NUM_BITS == 16
     uint32_t port_data = atomic_load(&i2s_out_port_data);
     port_data <<= 16;                   // Shift needed. This specification is not spelled out in the manual.
     I2S0.conf_single_data = port_data;  // Apply port data in real-time (static I2S)
-#    else
+#else
     I2S0.conf_single_data = atomic_load(&i2s_out_port_data);  // Apply port data in real-time (static I2S)
-#    endif
+#endif
 }
 
 static inline void i2s_out_reset_fifo_without_lock() {
@@ -185,7 +191,7 @@ static void IRAM_ATTR i2s_out_reset_fifo() {
     I2S_OUT_EXIT_CRITICAL();
 }
 
-#    ifdef USE_I2S_OUT_STREAM_IMPL
+#ifdef USE_I2S_OUT_STREAM_IMPL
 static int IRAM_ATTR i2s_clear_dma_buffer(lldesc_t* dma_desc, uint32_t port_data) {
     uint32_t* buf = (uint32_t*)dma_desc->buf;
     for (int i = 0; i < DMA_SAMPLE_COUNT; i++) {
@@ -212,7 +218,7 @@ static int IRAM_ATTR i2s_clear_o_dma_buffers(uint32_t port_data) {
     }
     return 0;
 }
-#    endif
+#endif
 
 static int IRAM_ATTR i2s_out_gpio_attach(uint8_t ws, uint8_t bck, uint8_t data) {
     // Route the i2s pins to the appropriate GPIO
@@ -245,13 +251,13 @@ static int IRAM_ATTR i2s_out_gpio_shiftout(uint32_t port_data) {
 
 static int IRAM_ATTR i2s_out_stop() {
     I2S_OUT_ENTER_CRITICAL();
-#    ifdef USE_I2S_OUT_STREAM_IMPL
+#ifdef USE_I2S_OUT_STREAM_IMPL
     // Stop FIFO DMA
     I2S0.out_link.stop = 1;
 
     // Disconnect DMA from FIFO
     I2S0.fifo_conf.dscr_en = 0;  //Unset this bit to disable I2S DMA mode. (R/W)
-#    endif
+#endif
     // stop TX module
     I2S0.conf.tx_start = 0;
 
@@ -271,10 +277,10 @@ static int IRAM_ATTR i2s_out_stop() {
     uint32_t port_data = atomic_load(&i2s_out_port_data);  // current expanded port value
     i2s_out_gpio_shiftout(port_data);
 
-#    ifdef USE_I2S_OUT_STREAM_IMPL
+#ifdef USE_I2S_OUT_STREAM_IMPL
     //clear pending interrupt
     I2S0.int_clr.val = I2S0.int_st.val;
-#    endif
+#endif
     I2S_OUT_EXIT_CRITICAL();
     return 0;
 }
@@ -291,10 +297,28 @@ static int IRAM_ATTR i2s_out_start() {
 
     // Attach I2S to specified GPIO pin
     i2s_out_gpio_attach(i2s_out_ws_pin, i2s_out_bck_pin, i2s_out_data_pin);
-    //start DMA link
+
+    // reest TX/RX module
+    I2S0.conf.tx_reset = 1;
+    I2S0.conf.tx_reset = 0;
+    I2S0.conf.rx_reset = 1;
+    I2S0.conf.rx_reset = 0;
+
+#ifdef USE_I2S_OUT_STREAM_IMPL
+    // reset DMA
+    I2S0.lc_conf.in_rst  = 1;
+    I2S0.lc_conf.in_rst  = 0;
+    I2S0.lc_conf.out_rst = 1;
+    I2S0.lc_conf.out_rst = 0;
+
+    I2S0.out_link.addr = (uint32_t)o_dma.desc[0];
+#endif
+
+    // reset FIFO
     i2s_out_reset_fifo_without_lock();
 
-#    ifdef USE_I2S_OUT_STREAM_IMPL
+    // start DMA link
+#ifdef USE_I2S_OUT_STREAM_IMPL
     if (i2s_out_pulser_status == PASSTHROUGH) {
         I2S0.conf_chan.tx_chan_mod = 3;  // 3:right+constant 4:left+constant (when tx_msb_right = 1)
         I2S0.conf_single_data      = port_data;
@@ -302,32 +326,17 @@ static int IRAM_ATTR i2s_out_start() {
         I2S0.conf_chan.tx_chan_mod = 4;  // 3:right+constant 4:left+constant (when tx_msb_right = 1)
         I2S0.conf_single_data      = 0;
     }
-#    endif
-
-#    ifdef USE_I2S_OUT_STREAM_IMPL
-    //reset DMA
-    I2S0.lc_conf.in_rst  = 1;
-    I2S0.lc_conf.in_rst  = 0;
-    I2S0.lc_conf.out_rst = 1;
-    I2S0.lc_conf.out_rst = 0;
-
-    I2S0.out_link.addr = (uint32_t)o_dma.desc[0];
-#    endif
-
-    I2S0.conf.tx_reset = 1;
-    I2S0.conf.tx_reset = 0;
-    I2S0.conf.rx_reset = 1;
-    I2S0.conf.rx_reset = 0;
+#endif
 
     I2S0.conf1.tx_stop_en = 1;  // BCK and WCK are suppressed while FIFO is empty
 
-#    ifdef USE_I2S_OUT_STREAM_IMPL
+#ifdef USE_I2S_OUT_STREAM_IMPL
     // Connect DMA to FIFO
     I2S0.fifo_conf.dscr_en = 1;  // Set this bit to enable I2S DMA mode. (R/W)
 
     I2S0.int_clr.val    = 0xFFFFFFFF;
     I2S0.out_link.start = 1;
-#    endif
+#endif
     I2S0.conf.tx_start = 1;
     // Wait for the first FIFO data to prevent the unintentional generation of 0 data
     ets_delay_us(20);
@@ -338,14 +347,15 @@ static int IRAM_ATTR i2s_out_start() {
     return 0;
 }
 
-#    ifdef USE_I2S_OUT_STREAM_IMPL
-
+#ifdef USE_I2S_OUT_STREAM_IMPL
+// Fill out one DMA buffer
+// Call with the I2S_OUT_PULSER lock acquired.
+// Note that the lock is temporarily released while calling the callback function.
 static int IRAM_ATTR i2s_fillout_dma_buffer(lldesc_t* dma_desc) {
     uint32_t* buf = (uint32_t*)dma_desc->buf;
     o_dma.rw_pos  = 0;
     // It reuses the oldest (just transferred) buffer with the name "current"
     // and fills the buffer for later DMA.
-    I2S_OUT_PULSER_ENTER_CRITICAL();  // Lock pulser status
     if (i2s_out_pulser_status == STEPPING) {
         //
         // Fillout the buffer for pulse
@@ -408,7 +418,6 @@ static int IRAM_ATTR i2s_fillout_dma_buffer(lldesc_t* dma_desc) {
         o_dma.rw_pos                         = 0;  // If someone calls i2s_out_push_sample, make sure there is no buffer overflow
         i2s_out_remain_time_until_next_pulse = 0;
     }
-    I2S_OUT_PULSER_EXIT_CRITICAL();  // Unlock pulser status
 
     return 0;
 }
@@ -515,15 +524,18 @@ static void IRAM_ATTR i2sOutTask(void* parameter) {
             o_dma.rw_pos = 0;                   // If someone calls i2s_out_push_sample, make sure there is no buffer overflow
         }
         I2S_OUT_PULSER_EXIT_CRITICAL();  // Unlock pulser status
+
+        static UBaseType_t uxHighWaterMark = 0;
+        reportTaskStackSize(uxHighWaterMark);
     }
 }
-#    endif
+#endif
 
 //
 // External funtions
 //
 void IRAM_ATTR i2s_out_delay() {
-#    ifdef USE_I2S_OUT_STREAM_IMPL
+#ifdef USE_I2S_OUT_STREAM_IMPL
     I2S_OUT_PULSER_ENTER_CRITICAL();
     if (i2s_out_pulser_status == PASSTHROUGH) {
         // Depending on the timing, it may not be reflected immediately,
@@ -535,9 +547,9 @@ void IRAM_ATTR i2s_out_delay() {
         delay(I2S_OUT_DELAY_MS);
     }
     I2S_OUT_PULSER_EXIT_CRITICAL();
-#    else
+#else
     ets_delay_us(I2S_OUT_USEC_PER_PULSE * 2);
-#    endif
+#endif
 }
 
 void IRAM_ATTR i2s_out_write(uint8_t pin, uint8_t val) {
@@ -547,15 +559,15 @@ void IRAM_ATTR i2s_out_write(uint8_t pin, uint8_t val) {
     } else {
         atomic_fetch_and(&i2s_out_port_data, ~bit);
     }
-#    ifdef USE_I2S_OUT_STREAM_IMPL
+#ifdef USE_I2S_OUT_STREAM_IMPL
     // It needs a lock for access, but I've given up because I need speed.
     // This is not a problem as long as there is no overlap between the status change and digitalWrite().
     if (i2s_out_pulser_status == PASSTHROUGH) {
         i2s_out_single_data();
     }
-#    else
+#else
     i2s_out_single_data();
-#    endif
+#endif
 }
 
 uint8_t IRAM_ATTR i2s_out_read(uint8_t pin) {
@@ -563,12 +575,14 @@ uint8_t IRAM_ATTR i2s_out_read(uint8_t pin) {
     return (!!(port_data & bit(pin)));
 }
 
-uint32_t IRAM_ATTR i2s_out_push_sample(uint32_t num) {
-#    ifdef USE_I2S_OUT_STREAM_IMPL
+uint32_t IRAM_ATTR i2s_out_push_sample(uint32_t usec) {
+    uint32_t num = usec / I2S_OUT_USEC_PER_PULSE;
+
+#ifdef USE_I2S_OUT_STREAM_IMPL
     if (num > SAMPLE_SAFE_COUNT) {
         return 0;
     }
-    // push at least one sample (even if num is zero)
+    // push at least one sample, even if num is zero)
     uint32_t port_data = atomic_load(&i2s_out_port_data);
     uint32_t n         = 0;
     do {
@@ -576,9 +590,9 @@ uint32_t IRAM_ATTR i2s_out_push_sample(uint32_t num) {
         n++;
     } while (n < num);
     return n;
-#    else
+#else
     return 0;
-#    endif
+#endif
 }
 
 i2s_out_pulser_status_t IRAM_ATTR i2s_out_get_pulser_status() {
@@ -590,20 +604,28 @@ i2s_out_pulser_status_t IRAM_ATTR i2s_out_get_pulser_status() {
 
 int IRAM_ATTR i2s_out_set_passthrough() {
     I2S_OUT_PULSER_ENTER_CRITICAL();
-#    ifdef USE_I2S_OUT_STREAM_IMPL
+#ifdef USE_I2S_OUT_STREAM_IMPL
+    // Triggers a change of mode if it is compiled to use I2S stream.
+    // The mode is not changed directly by this function.
+    // Pull the trigger
     if (i2s_out_pulser_status == STEPPING) {
-        i2s_out_pulser_status = WAITING;  // Start stopping the pulser
+        i2s_out_pulser_status = WAITING;  // Start stopping the pulser (trigger)
     }
-#    else
+    // It is a function that may be called via i2sOutTask().
+    // (i2sOutTask() -> stepper_pulse_func() -> st_go_idle() -> Stepper_Timer_Stop() -> this function)
+    // And only i2sOutTask() can change the state to PASSTHROUGH.
+    // So, to change the state, you need to return to i2sOutTask() as soon as possible.
+#else
+    // If it is compiled to not use I2S streams, change the mode directly.
     i2s_out_pulser_status = PASSTHROUGH;
-#    endif
+#endif
     I2S_OUT_PULSER_EXIT_CRITICAL();
     return 0;
 }
 
 int IRAM_ATTR i2s_out_set_stepping() {
     I2S_OUT_PULSER_ENTER_CRITICAL();
-#    ifdef USE_I2S_OUT_STREAM_IMPL
+#ifdef USE_I2S_OUT_STREAM_IMPL
     if (i2s_out_pulser_status == STEPPING) {
         // Re-entered (fail safe)
         I2S_OUT_PULSER_EXIT_CRITICAL();
@@ -627,6 +649,7 @@ int IRAM_ATTR i2s_out_set_stepping() {
             I2S_OUT_PULSER_EXIT_CRITICAL();
             return 0;
         }
+        // Now, DMA completed. Fallthrough.
     }
 
     // Change I2S state from PASSTHROUGH to STEPPING
@@ -638,32 +661,31 @@ int IRAM_ATTR i2s_out_set_stepping() {
     // because the process in i2s_out_start() is different depending on the status.
     i2s_out_pulser_status = STEPPING;
     i2s_out_start();
-#    else
+#else
     i2s_out_pulser_status = STEPPING;
-#    endif
+#endif
     I2S_OUT_PULSER_EXIT_CRITICAL();
     return 0;
 }
 
-int IRAM_ATTR i2s_out_set_pulse_period(uint64_t period) {
-#    ifdef USE_I2S_OUT_STREAM_IMPL
-    // Use 64-bit values to avoid overflowing during the calculation.
-    i2s_out_pulse_period = period * 1000000 / F_STEPPER_TIMER;
-#    endif
+int IRAM_ATTR i2s_out_set_pulse_period(uint32_t period) {
+#ifdef USE_I2S_OUT_STREAM_IMPL
+    i2s_out_pulse_period = period;
+#endif
     return 0;
 }
 
 int IRAM_ATTR i2s_out_set_pulse_callback(i2s_out_pulse_func_t func) {
-#    ifdef USE_I2S_OUT_STREAM_IMPL
+#ifdef USE_I2S_OUT_STREAM_IMPL
     i2s_out_pulse_func = func;
-#    endif
+#endif
     return 0;
 }
 
 int IRAM_ATTR i2s_out_reset() {
     I2S_OUT_PULSER_ENTER_CRITICAL();
     i2s_out_stop();
-#    ifdef USE_I2S_OUT_STREAM_IMPL
+#ifdef USE_I2S_OUT_STREAM_IMPL
     if (i2s_out_pulser_status == STEPPING) {
         uint32_t port_data = atomic_load(&i2s_out_port_data);
         i2s_clear_o_dma_buffers(port_data);
@@ -671,7 +693,7 @@ int IRAM_ATTR i2s_out_reset() {
         i2s_clear_o_dma_buffers(0);
         i2s_out_pulser_status = PASSTHROUGH;
     }
-#    endif
+#endif
     // You need to set the status before calling i2s_out_start()
     // because the process in i2s_out_start() is different depending on the status.
     i2s_out_start();
@@ -722,7 +744,7 @@ int IRAM_ATTR i2s_out_init(i2s_out_init_t& init_param) {
    *      M = 2
    */
 
-#    ifdef USE_I2S_OUT_STREAM_IMPL
+#ifdef USE_I2S_OUT_STREAM_IMPL
     // Allocate the array of pointers to the buffers
     o_dma.buffers = (uint32_t**)malloc(sizeof(uint32_t*) * I2S_OUT_DMABUF_COUNT);
     if (o_dma.buffers == nullptr) {
@@ -759,7 +781,7 @@ int IRAM_ATTR i2s_out_init(i2s_out_init_t& init_param) {
 
     // Set the first DMA descriptor
     I2S0.out_link.addr = (uint32_t)o_dma.desc[0];
-#    endif
+#endif
 
     // stop i2s
     I2S0.out_link.stop = 1;
@@ -772,7 +794,6 @@ int IRAM_ATTR i2s_out_init(i2s_out_init_t& init_param) {
     //
 
     // configure I2S data port interface.
-    i2s_out_reset_fifo();
 
     //reset i2s
     I2S0.conf.tx_reset = 1;
@@ -786,6 +807,8 @@ int IRAM_ATTR i2s_out_init(i2s_out_init_t& init_param) {
     I2S0.lc_conf.out_rst = 1;  // Set this bit to reset out DMA FSM. (R/W)
     I2S0.lc_conf.out_rst = 0;
 
+    i2s_out_reset_fifo_without_lock();
+
     //Enable and configure DMA
     I2S0.lc_conf.check_owner        = 0;
     I2S0.lc_conf.out_loop_test      = 0;
@@ -794,9 +817,9 @@ int IRAM_ATTR i2s_out_init(i2s_out_init_t& init_param) {
     I2S0.lc_conf.outdscr_burst_en   = 0;
     I2S0.lc_conf.out_no_restart_clr = 0;
     I2S0.lc_conf.indscr_burst_en    = 0;
-#    ifdef USE_I2S_OUT_STREAM_IMPL
+#ifdef USE_I2S_OUT_STREAM_IMPL
     I2S0.lc_conf.out_eof_mode = 1;  // I2S_OUT_EOF_INT generated when DMA has popped all data from the FIFO;
-#    endif
+#endif
     I2S0.conf2.lcd_en             = 0;
     I2S0.conf2.camera_en          = 0;
     I2S0.pdm_conf.pcm2pdm_conv_en = 0;
@@ -804,7 +827,7 @@ int IRAM_ATTR i2s_out_init(i2s_out_init_t& init_param) {
 
     I2S0.fifo_conf.dscr_en = 0;
 
-#    ifdef USE_I2S_OUT_STREAM_IMPL
+#ifdef USE_I2S_OUT_STREAM_IMPL
     if (i2s_out_pulser_status == STEPPING) {
         // Stream output mode
         I2S0.conf_chan.tx_chan_mod = 4;  // 3:right+constant 4:left+constant (when tx_msb_right = 1)
@@ -814,31 +837,31 @@ int IRAM_ATTR i2s_out_init(i2s_out_init_t& init_param) {
         I2S0.conf_chan.tx_chan_mod = 3;  // 3:right+constant 4:left+constant (when tx_msb_right = 1)
         I2S0.conf_single_data      = init_param.init_val;
     }
-#    else
+#else
     // For the static output mode
     I2S0.conf_chan.tx_chan_mod = 3;                    // 3:right+constant 4:left+constant (when tx_msb_right = 1)
     I2S0.conf_single_data      = init_param.init_val;  // initial constant value
-#    endif
-#    if I2S_OUT_NUM_BITS == 16
+#endif
+#if I2S_OUT_NUM_BITS == 16
     I2S0.fifo_conf.tx_fifo_mod        = 0;   // 0: 16-bit dual channel data, 3: 32-bit single channel data
     I2S0.fifo_conf.rx_fifo_mod        = 0;   // 0: 16-bit dual channel data, 3: 32-bit single channel data
     I2S0.sample_rate_conf.tx_bits_mod = 16;  // default is 16-bits
     I2S0.sample_rate_conf.rx_bits_mod = 16;  // default is 16-bits
-#    else
+#else
     I2S0.fifo_conf.tx_fifo_mod = 3;                    // 0: 16-bit dual channel data, 3: 32-bit single channel data
     I2S0.fifo_conf.rx_fifo_mod = 3;                    // 0: 16-bit dual channel data, 3: 32-bit single channel data
     // Data width is 32-bit. Forgetting this setting will result in a 16-bit transfer.
     I2S0.sample_rate_conf.tx_bits_mod = 32;
     I2S0.sample_rate_conf.rx_bits_mod = 32;
-#    endif
+#endif
     I2S0.conf.tx_mono = 0;  // Set this bit to enable transmitter’s mono mode in PCM standard mode.
 
     I2S0.conf_chan.rx_chan_mod = 1;  // 1: right+right
     I2S0.conf.rx_mono          = 0;
 
-#    ifdef USE_I2S_OUT_STREAM_IMPL
+#ifdef USE_I2S_OUT_STREAM_IMPL
     I2S0.fifo_conf.dscr_en = 1;  //connect DMA to fifo
-#    endif
+#endif
     I2S0.conf.tx_start = 0;
     I2S0.conf.rx_start = 0;
 
@@ -846,9 +869,9 @@ int IRAM_ATTR i2s_out_init(i2s_out_init_t& init_param) {
     I2S0.conf.tx_right_first = 0;  // Setting this bit allows the right-channel data to be sent first.
 
     I2S0.conf.tx_slave_mod = 0;  // Master
-#    ifdef USE_I2S_OUT_STREAM_IMPL
+#ifdef USE_I2S_OUT_STREAM_IMPL
     I2S0.fifo_conf.tx_fifo_mod_force_en = 1;  //The bit should always be set to 1.
-#    endif
+#endif
     I2S0.pdm_conf.rx_pdm_en = 0;  // Set this bit to enable receiver’s PDM mode.
     I2S0.pdm_conf.tx_pdm_en = 0;  // Set this bit to enable transmitter’s PDM mode.
 
@@ -865,13 +888,13 @@ int IRAM_ATTR i2s_out_init(i2s_out_init_t& init_param) {
     // set clock (fi2s) 160MHz / 5
     I2S0.clkm_conf.clka_en = 0;  // Use 160 MHz PLL_D2_CLK as reference
                                  // N + b/a = 0
-#    if I2S_OUT_NUM_BITS == 16
+#if I2S_OUT_NUM_BITS == 16
     // N = 10
     I2S0.clkm_conf.clkm_div_num = 10;  // minimum value of 2, reset value of 4, max 256 (I²S clock divider’s integral value)
-#    else
+#else
     // N = 5
     I2S0.clkm_conf.clkm_div_num = 5;  // minimum value of 2, reset value of 4, max 256 (I²S clock divider’s integral value)
-#    endif
+#endif
     // b/a = 0
     I2S0.clkm_conf.clkm_div_b = 0;  // 0 at reset
     I2S0.clkm_conf.clkm_div_a = 0;  // 0 at reset, what about divide by 0? (not an issue)
@@ -881,7 +904,7 @@ int IRAM_ATTR i2s_out_init(i2s_out_init_t& init_param) {
     I2S0.sample_rate_conf.tx_bck_div_num = 2;  // minimum value of 2 defaults to 6
     I2S0.sample_rate_conf.rx_bck_div_num = 2;
 
-#    ifdef USE_I2S_OUT_STREAM_IMPL
+#ifdef USE_I2S_OUT_STREAM_IMPL
     // Enable TX interrupts (DMA Interrupts)
     I2S0.int_ena.out_eof       = 1;  // Triggered when rxlink has finished sending a packet.
     I2S0.int_ena.out_dscr_err  = 0;  // Triggered when invalid rxlink descriptors are encountered.
@@ -895,7 +918,7 @@ int IRAM_ATTR i2s_out_init(i2s_out_init_t& init_param) {
     // Create the task that will feed the buffer
     xTaskCreatePinnedToCore(i2sOutTask,
                             "I2SOutTask",
-                            1024 * 10,
+                            4096,
                             NULL,
                             1,
                             nullptr,
@@ -905,7 +928,7 @@ int IRAM_ATTR i2s_out_init(i2s_out_init_t& init_param) {
     // Allocate and Enable the I2S interrupt
     esp_intr_alloc(ETS_I2S0_INTR_SOURCE, 0, i2s_out_intr_handler, nullptr, &i2s_out_isr_handle);
     esp_intr_enable(i2s_out_isr_handle);
-#    endif
+#endif
 
     // Remember GPIO pin numbers
     i2s_out_ws_pin      = init_param.ws_pin;
@@ -919,18 +942,18 @@ int IRAM_ATTR i2s_out_init(i2s_out_init_t& init_param) {
     return 0;
 }
 
-#    ifndef I2S_OUT_WS
-#        define I2S_OUT_WS GPIO_NUM_17
-#    endif
-#    ifndef I2S_OUT_BCK
-#        define I2S_OUT_BCK GPIO_NUM_22
-#    endif
-#    ifndef I2S_OUT_DATA
-#        define I2S_OUT_DATA GPIO_NUM_21
-#    endif
-#    ifndef I2S_OUT_INIT_VAL
-#        define I2S_OUT_INIT_VAL 0
-#    endif
+#ifndef I2S_OUT_WS
+#    define I2S_OUT_WS GPIO_NUM_17
+#endif
+#ifndef I2S_OUT_BCK
+#    define I2S_OUT_BCK GPIO_NUM_22
+#endif
+#ifndef I2S_OUT_DATA
+#    define I2S_OUT_DATA GPIO_NUM_21
+#endif
+#ifndef I2S_OUT_INIT_VAL
+#    define I2S_OUT_INIT_VAL 0
+#endif
 /*
   Initialize I2S out by default parameters.
 
@@ -947,5 +970,3 @@ int IRAM_ATTR i2s_out_init() {
     };
     return i2s_out_init(default_param);
 }
-
-#endif
diff --git a/Grbl_Esp32/src/I2SOut.h b/Grbl_Esp32/src/I2SOut.h
index b5defb10..5ae31c16 100644
--- a/Grbl_Esp32/src/I2SOut.h
+++ b/Grbl_Esp32/src/I2SOut.h
@@ -41,8 +41,6 @@
 // It should be included at the outset to know the machine configuration.
 #include "Config.h"
 
-#ifdef USE_I2S_OUT
-
 #    include <stdint.h>
 
 /* Assert */
@@ -126,12 +124,14 @@ void i2s_out_write(uint8_t pin, uint8_t val);
 /*
     Set current pin state to the I2S bitstream buffer
     (This call will generate a future I2S_OUT_USEC_PER_PULSE μs x N bitstream)
-    num: Number of samples to be generated
+    usec: The length of time that the pulse should be repeated.
+         That time will be converted to an integer number of pulses of
+         length I2S_OUT_USEC_PER_PULSE.
          The number of samples is limited to (20 / I2S_OUT_USEC_PER_PULSE).
-    return: number of puhsed samples
+    return: number of pushed samples
             0 .. no space for push
  */
-uint32_t i2s_out_push_sample(uint32_t num);
+uint32_t i2s_out_push_sample(uint32_t usec);
 
 /*
    Set pulser mode to passtrough
@@ -156,10 +156,9 @@ int i2s_out_set_stepping();
 void i2s_out_delay();
 
 /*
-   Set the pulse callback period in ISR ticks.
-   (same value of the timer period for the ISR)
+   Set the pulse callback period in microseconds
  */
-int i2s_out_set_pulse_period(uint64_t period);
+int i2s_out_set_pulse_period(uint32_t usec);
 
 /*
    Register a callback function to generate pulse data
@@ -184,8 +183,6 @@ i2s_out_pulser_status_t IRAM_ATTR i2s_out_get_pulser_status();
  */
 int i2s_out_reset();
 
-#endif
-
 /*
    Reference: "ESP32 Technical Reference Manual" by Espressif Systems
      https://www.espressif.com/sites/default/files/documentation/esp32_technical_reference_manual_en.pdf
diff --git a/Grbl_Esp32/src/Jog.cpp b/Grbl_Esp32/src/Jog.cpp
index 6dca36e2..5d7b5773 100644
--- a/Grbl_Esp32/src/Jog.cpp
+++ b/Grbl_Esp32/src/Jog.cpp
@@ -33,7 +33,7 @@ Error jog_execute(plan_line_data_t* pl_data, parser_block_t* gc_block) {
     pl_data->line_number = gc_block->values.n;
 #endif
     if (soft_limits->get()) {
-        if (system_check_travel_limits(gc_block->values.xyz)) {
+        if (limitsCheckTravel(gc_block->values.xyz)) {
             return Error::TravelExceeded;
         }
     }
diff --git a/Grbl_Esp32/src/Limits.cpp b/Grbl_Esp32/src/Limits.cpp
index e3539fee..2aed18b7 100644
--- a/Grbl_Esp32/src/Limits.cpp
+++ b/Grbl_Esp32/src/Limits.cpp
@@ -55,12 +55,12 @@ void IRAM_ATTR isr_limit_switches() {
 #    ifdef HARD_LIMIT_FORCE_STATE_CHECK
             // Check limit pin state.
             if (limits_get_state()) {
-                mc_reset();                                   // Initiate system kill.
-                system_set_exec_alarm(ExecAlarm::HardLimit);  // Indicate hard limit critical event
+                mc_reset();                                // Initiate system kill.
+                sys_rt_exec_alarm = ExecAlarm::HardLimit;  // Indicate hard limit critical event
             }
 #    else
-            mc_reset();                                   // Initiate system kill.
-            system_set_exec_alarm(ExecAlarm::HardLimit);  // Indicate hard limit critical event
+            mc_reset();                                // Initiate system kill.
+            sys_rt_exec_alarm = ExecAlarm::HardLimit;  // Indicate hard limit critical event
 #    endif
 #endif
         }
@@ -79,18 +79,12 @@ void limits_go_home(uint8_t cycle_mask) {
         return;  // Block if system reset has been issued.
     }
     // Initialize plan data struct for homing motion. Spindle and coolant are disabled.
-    motors_set_homing_mode(cycle_mask, true);  // tell motors homing is about to start
 
-    // Remove any motor that cannot be homed from the mask
-    // Motors with non standard homing can do that during motors_set_homing_mode(...) above
-    auto n_axis = number_axis->get();
-    for (uint8_t idx = 0; idx < n_axis; idx++) {
-        if (bit_istrue(cycle_mask, bit(idx))) {
-            if (!motor_can_home(idx)) {
-                bit_false(cycle_mask, bit(idx));
-            }
-        }
-    }
+    // Put motors on axes listed in cycle_mask in homing mode and
+    // replace cycle_mask with the list of motors that are ready for homing.
+    // Motors with non standard homing can home during motors_set_homing_mode(...)
+    cycle_mask = motors_set_homing_mode(cycle_mask, true);  // tell motors homing is about to start
+
     // see if any motors are left
     if (cycle_mask == 0) {
         return;
@@ -111,12 +105,13 @@ void limits_go_home(uint8_t cycle_mask) {
     float   target[MAX_N_AXIS];
     float   max_travel = 0.0;
 
+    auto n_axis = number_axis->get();
     for (uint8_t idx = 0; idx < n_axis; idx++) {
         // Initialize step pin masks
-        step_pin[idx] = get_step_pin_mask(idx);
+        step_pin[idx] = bit(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));
+            step_pin[idx] = (bit(X_AXIS) | bit(Y_AXIS));
         }
 #endif
         if (bit_istrue(cycle_mask, bit(idx))) {
@@ -125,9 +120,10 @@ void limits_go_home(uint8_t cycle_mask) {
         }
     }
     // Set search mode with approach at seek rate to quickly engage the specified cycle_mask limit switches.
-    bool    approach    = true;
-    float   homing_rate = homing_seek_rate->get();
-    uint8_t limit_state, axislock, n_active_axis;
+    bool     approach    = true;
+    float    homing_rate = homing_seek_rate->get();
+    uint8_t  n_active_axis;
+    AxisMask limit_state, axislock;
     do {
         system_convert_array_steps_to_mpos(target, sys_position);
         // Initialize and declare variables needed for homing routine.
@@ -174,11 +170,12 @@ void limits_go_home(uint8_t cycle_mask) {
         homing_rate *= sqrt(n_active_axis);  // [sqrt(number of active 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
+        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                  = {};
+        sys.step_control.executeSysMotion = true;  // 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.
@@ -202,23 +199,24 @@ void limits_go_home(uint8_t cycle_mask) {
             }
             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)) || cycle_stop) {
-                uint8_t rt_exec = sys_rt_exec_state;
+            if (sys_rt_exec_state.bit.safetyDoor || sys_rt_exec_state.bit.reset || cycle_stop) {
+                ExecState rt_exec_state;
+                rt_exec_state.value = sys_rt_exec_state.value;
                 // Homing failure condition: Reset issued during cycle.
-                if (rt_exec & EXEC_RESET) {
-                    system_set_exec_alarm(ExecAlarm::HomingFailReset);
+                if (rt_exec_state.bit.reset) {
+                    sys_rt_exec_alarm = ExecAlarm::HomingFailReset;
                 }
                 // Homing failure condition: Safety door was opened.
-                if (rt_exec & EXEC_SAFETY_DOOR) {
-                    system_set_exec_alarm(ExecAlarm::HomingFailDoor);
+                if (rt_exec_state.bit.safetyDoor) {
+                    sys_rt_exec_alarm = ExecAlarm::HomingFailDoor;
                 }
                 // Homing failure condition: Limit switch still engaged after pull-off motion
                 if (!approach && (limits_get_state() & cycle_mask)) {
-                    system_set_exec_alarm(ExecAlarm::HomingFailPulloff);
+                    sys_rt_exec_alarm = ExecAlarm::HomingFailPulloff;
                 }
                 // Homing failure condition: Limit switch not found during approach.
                 if (approach && cycle_stop) {
-                    system_set_exec_alarm(ExecAlarm::HomingFailApproach);
+                    sys_rt_exec_alarm = ExecAlarm::HomingFailApproach;
                 }
 
                 if (sys_rt_exec_alarm != ExecAlarm::None) {
@@ -303,7 +301,7 @@ void limits_go_home(uint8_t cycle_mask) {
 #endif
         }
     }
-    sys.step_control = STEP_CONTROL_NORMAL_OP;  // Return step control to normal operation.
+    sys.step_control = {};                      // Return step control to normal operation.
     motors_set_homing_mode(cycle_mask, false);  // tell motors homing is done
 }
 
@@ -332,12 +330,8 @@ void limits_init() {
                 }
 
                 if (limit_sw_queue == NULL) {
-                    grbl_msg_sendf(CLIENT_SERIAL,
-                                   MsgLevel::Info,
-                                   "%c%s Axis limit switch on pin %s",
-                                   report_get_axis_letter(axis),
-                                   gang_index ? "2" : " ",
-                                   pinName(pin).c_str());
+                    grbl_msg_sendf(
+                        CLIENT_SERIAL, MsgLevel::Info, "%s limit switch on pin %s", reportAxisNameMsg(axis, gang_index), pinName(pin).c_str());
                 }
             }
         }
@@ -371,9 +365,9 @@ void limits_disable() {
 // Returns limit state as a bit-wise uint8 variable. Each bit indicates an axis limit, where
 // triggered is 1 and not triggered is 0. Invert mask is applied. Axes are defined by their
 // number in bit position, i.e. Z_AXIS is bit(2), and Y_AXIS is bit(1).
-uint8_t limits_get_state() {
-    uint8_t pinMask = 0;
-    auto    n_axis  = number_axis->get();
+AxisMask limits_get_state() {
+    AxisMask pinMask = 0;
+    auto     n_axis  = number_axis->get();
     for (int axis = 0; axis < n_axis; axis++) {
         for (int gang_index = 0; gang_index < 2; gang_index++) {
             uint8_t pin = limit_pins[axis][gang_index];
@@ -396,13 +390,13 @@ uint8_t limits_get_state() {
 // the workspace volume is in all negative space, and the system is in normal operation.
 // NOTE: Used by jogging to limit travel within soft-limit volume.
 void limits_soft_check(float* target) {
-    if (system_check_travel_limits(target)) {
+    if (limitsCheckTravel(target)) {
         sys.soft_limit = true;
         // Force feed hold if cycle is active. All buffered blocks are guaranteed to be within
         // workspace volume so just come to a controlled stop so position is not lost. When complete
         // enter alarm mode.
         if (sys.state == State::Cycle) {
-            system_set_exec_state_flag(EXEC_FEED_HOLD);
+            sys_rt_exec_state.bit.feedHold = true;
             do {
                 protocol_execute_realtime();
                 if (sys.abort) {
@@ -410,9 +404,9 @@ void limits_soft_check(float* target) {
                 }
             } while (sys.state != State::Idle);
         }
-        mc_reset();                                   // Issue system reset and ensure spindle and coolant are shutdown.
-        system_set_exec_alarm(ExecAlarm::SoftLimit);  // Indicate soft limit critical event
-        protocol_execute_realtime();                  // Execute to enter critical event loop and system abort
+        mc_reset();                                // Issue system reset and ensure spindle and coolant are shutdown.
+        sys_rt_exec_alarm = ExecAlarm::SoftLimit;  // Indicate soft limit critical event
+        protocol_execute_realtime();               // Execute to enter critical event loop and system abort
         return;
     }
 }
@@ -423,12 +417,41 @@ void limitCheckTask(void* pvParameters) {
         int evt;
         xQueueReceive(limit_sw_queue, &evt, portMAX_DELAY);  // block until receive queue
         vTaskDelay(DEBOUNCE_PERIOD / portTICK_PERIOD_MS);    // delay a while
-        uint8_t switch_state;
+        AxisMask switch_state;
         switch_state = limits_get_state();
         if (switch_state) {
             //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Limit Switch State %08d", switch_state);
-            mc_reset();                                   // Initiate system kill.
-            system_set_exec_alarm(ExecAlarm::HardLimit);  // Indicate hard limit critical event
+            mc_reset();                                // Initiate system kill.
+            sys_rt_exec_alarm = ExecAlarm::HardLimit;  // Indicate hard limit critical event
         }
+        static UBaseType_t uxHighWaterMark = 0;
+        reportTaskStackSize(uxHighWaterMark);
     }
 }
+
+float limitsMaxPosition(uint8_t axis) {
+    float mpos = axis_settings[axis]->home_mpos->get();
+
+    return bitnum_istrue(homing_dir_mask->get(), axis) ? mpos + axis_settings[axis]->max_travel->get() : mpos;
+}
+
+float limitsMinPosition(uint8_t axis) {
+    float mpos = axis_settings[axis]->home_mpos->get();
+
+    return bitnum_istrue(homing_dir_mask->get(), axis) ? mpos : mpos - axis_settings[axis]->max_travel->get();
+}
+
+// Checks and reports if target array exceeds machine travel limits.
+// Return true if exceeding limits
+bool limitsCheckTravel(float* target) {
+    uint8_t idx;
+    auto    n_axis = number_axis->get();
+    for (idx = 0; idx < n_axis; idx++) {
+        float max_mpos, min_mpos;
+
+        if (target[idx] < limitsMinPosition(idx) || target[idx] > limitsMaxPosition(idx)) {
+            return true;
+        }
+    }
+    return false;
+}
diff --git a/Grbl_Esp32/src/Limits.h b/Grbl_Esp32/src/Limits.h
index c0782ed6..3cc2fccd 100644
--- a/Grbl_Esp32/src/Limits.h
+++ b/Grbl_Esp32/src/Limits.h
@@ -36,7 +36,7 @@ void limits_init();
 void limits_disable();
 
 // Returns limit state as a bit-wise uint8 variable.
-uint8_t limits_get_state();
+AxisMask limits_get_state();
 
 // Perform one portion of the homing cycle based on the input settings.
 void limits_go_home(uint8_t cycle_mask);
@@ -48,3 +48,9 @@ void isr_limit_switches();
 
 // A task that runs after a limit switch interrupt.
 void limitCheckTask(void* pvParameters);
+
+float limitsMaxPosition(uint8_t axis);
+float limitsMinPosition(uint8_t axis);
+
+// Internal factor used by limits_soft_check
+bool limitsCheckTravel(float* target);
diff --git a/Grbl_Esp32/src/MachineCommon.h b/Grbl_Esp32/src/MachineCommon.h
index eea0c7dd..95cea998 100644
--- a/Grbl_Esp32/src/MachineCommon.h
+++ b/Grbl_Esp32/src/MachineCommon.h
@@ -1,9 +1,5 @@
 #pragma once
 
-#ifndef SPINDLE_TYPE
-#    define SPINDLE_TYPE SpindleType::PWM
-#endif
-
 // Grbl setting that are common to all machines
 // It should not be necessary to change anything herein
 
@@ -19,8 +15,7 @@
 #endif
 
 // ESP32 CPU Settings
-const int32_t F_TIMERS = 80000000;  // a reference to the speed of ESP32 timers
-#define F_STEPPER_TIMER 20000000    // frequency of step pulse timer
+const uint32_t fTimers = 80000000;  // a reference to the speed of ESP32 timers
 
 // =============== Don't change or comment these out ======================
 // They are for legacy purposes and will not affect your I/O
diff --git a/Grbl_Esp32/src/Machines/6_pack_Lowrider_stepstick_v1.h b/Grbl_Esp32/src/Machines/6_pack_Lowrider_stepstick_v1.h
index 0107cd94..9e935607 100644
--- a/Grbl_Esp32/src/Machines/6_pack_Lowrider_stepstick_v1.h
+++ b/Grbl_Esp32/src/Machines/6_pack_Lowrider_stepstick_v1.h
@@ -45,8 +45,6 @@
 #define X2_STEPPER_MS3          I2SO(14)  // A_CS
 #define Y2_STEPPER_MS3          I2SO(19)  // B_CS
 
-#define STEPPER_RESET           GPIO_NUM_19
-
 // Motor Socket #1
 #define X_DISABLE_PIN           I2SO(0)
 #define X_DIRECTION_PIN         I2SO(1)
diff --git a/Grbl_Esp32/src/Machines/6_pack_MPCNC_stepstick_v1.h b/Grbl_Esp32/src/Machines/6_pack_MPCNC_stepstick_v1.h
index d1f36e54..034dade2 100644
--- a/Grbl_Esp32/src/Machines/6_pack_MPCNC_stepstick_v1.h
+++ b/Grbl_Esp32/src/Machines/6_pack_MPCNC_stepstick_v1.h
@@ -45,8 +45,6 @@
 #define X2_STEPPER_MS3          I2SO(14)  // A_CS
 #define Y2_STEPPER_MS3          I2SO(19)  // B_CS
 
-#define STEPPER_RESET           GPIO_NUM_19
-
 // Motor Socket #1
 #define X_DISABLE_PIN           I2SO(0)
 #define X_DIRECTION_PIN         I2SO(1)
diff --git a/Grbl_Esp32/src/Machines/6_pack_stepstick_XYZ_v1.h b/Grbl_Esp32/src/Machines/6_pack_stepstick_XYZ_v1.h
index ea00a45e..28ab79db 100644
--- a/Grbl_Esp32/src/Machines/6_pack_stepstick_XYZ_v1.h
+++ b/Grbl_Esp32/src/Machines/6_pack_stepstick_XYZ_v1.h
@@ -45,8 +45,6 @@
 #define Y_STEPPER_MS3           I2SO(6)   // Y_CS
 #define Z_STEPPER_MS3           I2SO(11)  // Z_CS
 
-#define STEPPER_RESET           GPIO_NUM_19
-
 // Motor Socket #1
 #define X_DISABLE_PIN           I2SO(0)
 #define X_DIRECTION_PIN         I2SO(1)
@@ -122,7 +120,6 @@ Socket #5
 // #define CONTROL_SAFETY_DOOR_PIN      GPIO_NUM_27
 // //#define INVERT_CONTROL_PIN_MASK      B0000
 
-
 // ================= Setting Defaults ==========================
 #define DEFAULT_X_STEPS_PER_MM      800
 #define DEFAULT_Y_STEPS_PER_MM      800
diff --git a/Grbl_Esp32/src/Machines/6_pack_stepstick_v1.h b/Grbl_Esp32/src/Machines/6_pack_stepstick_v1.h
index d08faf2b..caf673be 100644
--- a/Grbl_Esp32/src/Machines/6_pack_stepstick_v1.h
+++ b/Grbl_Esp32/src/Machines/6_pack_stepstick_v1.h
@@ -47,8 +47,6 @@
 #define B_STEPPER_MS3           I2SO(19)  // B_CS
 #define C_STEPPER_MS3           I2SO(22)  // C_CS
 
-#define STEPPER_RESET           GPIO_NUM_19
-
 // Motor Socket #1
 #define X_DISABLE_PIN           I2SO(0)
 #define X_DIRECTION_PIN         I2SO(1)
diff --git a/Grbl_Esp32/src/Machines/midtbot.h b/Grbl_Esp32/src/Machines/midtbot.h
index 39e9c721..b9ceee16 100644
--- a/Grbl_Esp32/src/Machines/midtbot.h
+++ b/Grbl_Esp32/src/Machines/midtbot.h
@@ -45,9 +45,6 @@
 #define Y_LIMIT_PIN     GPIO_NUM_4
 
 #define Z_SERVO_PIN             GPIO_NUM_27
-#define Z_SERVO_CAL_MIN               1.0       // calibration factor for the minimum PWM duty
-#define Z_SERVO_CAL_MAX               1.0       // calibration factor for the maximum PWM duty
-
 
 // Set $Homing/Cycle0=Y and $Homing/Cycle1=X
 
diff --git a/Grbl_Esp32/src/Machines/pen_laser.h b/Grbl_Esp32/src/Machines/pen_laser.h
index d41b9f70..880c5094 100644
--- a/Grbl_Esp32/src/Machines/pen_laser.h
+++ b/Grbl_Esp32/src/Machines/pen_laser.h
@@ -55,8 +55,6 @@
 
 #ifdef USING_SERVO
     #define Z_SERVO_PIN             GPIO_NUM_27
-    #define Z_SERVO_CAL_MIN               1.0       // calibration factor for the minimum PWM duty
-    #define Z_SERVO_CAL_MAX               1.0       // calibration factor for the maximum PWM duty
 #endif
 
 #define SPINDLE_TYPE SpindleType::NONE
diff --git a/Grbl_Esp32/src/Machines/polar_coaster.h b/Grbl_Esp32/src/Machines/polar_coaster.h
index 5e600d76..b0e45e7c 100644
--- a/Grbl_Esp32/src/Machines/polar_coaster.h
+++ b/Grbl_Esp32/src/Machines/polar_coaster.h
@@ -49,8 +49,6 @@
 #define STEPPERS_DISABLE_PIN    GPIO_NUM_17
 
 #define Z_SERVO_PIN             GPIO_NUM_16
-#define Z_SERVO_CAL_MIN               1.0       // calibration factor for the minimum PWM duty
-#define Z_SERVO_CAL_MAX               1.0       // calibration factor for the maximum PWM duty
 
 #define X_LIMIT_PIN             GPIO_NUM_4
 
diff --git a/Grbl_Esp32/src/Machines/template.h b/Grbl_Esp32/src/Machines/template.h
index fc321a11..644734da 100644
--- a/Grbl_Esp32/src/Machines/template.h
+++ b/Grbl_Esp32/src/Machines/template.h
@@ -132,16 +132,14 @@
 // pins for that axis, but instead include a block like this:
 
 // #define SERVO_Z_PIN             GPIO_NUM_22
-// #define Z_SERVO_CAL_MIN       1.0 // calibration factor for the minimum PWM duty
-// #define Z_SERVO_CAL_MIN       1.0 // calibration factor for the maximum PWM duty
 
 // === Homing cycles
 // Set them using $Homing/Cycle0= optionally up to $Homing/Cycle5=
 
 // === Default settings
 // Grbl has many run-time settings that the user can changed by
-// commands like $110=2000 .  Their values are stored in EEPROM
-// so they persist after the controller has been powered down.
+// commands like $110=2000 .  Their values are stored in non-volatile
+// storage so they persist after the controller has been powered down.
 // Those settings have default values that are used if the user
 // has not altered them, or if the settings are explicitly reset
 // to the default values wth $RST=$.
diff --git a/Grbl_Esp32/src/Machines/tmc2130_pen.h b/Grbl_Esp32/src/Machines/tmc2130_pen.h
index 8c938015..93380577 100644
--- a/Grbl_Esp32/src/Machines/tmc2130_pen.h
+++ b/Grbl_Esp32/src/Machines/tmc2130_pen.h
@@ -59,9 +59,6 @@
 
 // Define one of these 2 options for spindle or servo
 #define Z_SERVO_PIN                     GPIO_NUM_27 // comment this out if PWM spindle/laser control.
-#define Z_SERVO_CAL_MIN               1.0       // calibration factor for the minimum PWM duty
-#define Z_SERVO_CAL_MAX               1.0       // calibration factor for the maximum PWM duty
-
 
 // #define X_LIMIT_PIN          See version section at beginning of file
 #define Y_LIMIT_PIN             GPIO_NUM_4
diff --git a/Grbl_Esp32/src/MotionControl.cpp b/Grbl_Esp32/src/MotionControl.cpp
index 33238736..a3be9ce9 100644
--- a/Grbl_Esp32/src/MotionControl.cpp
+++ b/Grbl_Esp32/src/MotionControl.cpp
@@ -260,6 +260,25 @@ static bool axis_is_squared(uint8_t axis_mask) {
     return mask_is_single_axis(axis_mask) && mask_has_squared_axis(axis_mask);
 }
 
+#ifdef USE_I2S_STEPS
+#    define BACKUP_STEPPER(save_stepper)                                                                                                   \
+        do {                                                                                                                               \
+            if (save_stepper == ST_I2S_STREAM) {                                                                                           \
+                stepper_switch(ST_I2S_STATIC); /* Change the stepper to reduce the delay for accurate probing. */                          \
+            }                                                                                                                              \
+        } while (0)
+
+#    define RESTORE_STEPPER(save_stepper)                                                                                                  \
+        do {                                                                                                                               \
+            if (save_stepper == ST_I2S_STREAM && current_stepper != ST_I2S_STREAM) {                                                       \
+                stepper_switch(ST_I2S_STREAM); /* Put the stepper back on. */                                                              \
+            }                                                                                                                              \
+        } while (0)
+#else
+#    define BACKUP_STEPPER(save_stepper)
+#    define RESTORE_STEPPER(save_stepper)
+#endif
+
 // Perform homing cycle to locate and set machine zero. Only '$H' executes this command.
 // NOTE: There should be no motions in the buffer and Grbl must be in an idle state before
 // executing the homing cycle. This prevents incorrect buffered plans after homing.
@@ -283,7 +302,7 @@ void mc_homing_cycle(uint8_t cycle_mask) {
 #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(ExecAlarm::HardLimit);
+        sys_rt_exec_alarm = ExecAlarm::HardLimit;
         return;
     }
 #endif
@@ -371,6 +390,13 @@ GCUpdatePos mc_probe_cycle(float* target, plan_line_data_t* pl_data, uint8_t par
     if (sys.abort) {
         return GCUpdatePos::None;  // Return if system reset has been issued.
     }
+
+#ifdef USE_I2S_STEPS
+    stepper_id_t save_stepper = current_stepper; /* remember the stepper */
+#endif
+    // Switch stepper mode to the I2S static (realtime mode)
+    BACKUP_STEPPER(save_stepper);
+
     // Initialize probing control variables
     uint8_t is_probe_away = bit_istrue(parser_flags, GCParserProbeIsAway);
     uint8_t is_no_error   = bit_istrue(parser_flags, GCParserProbeIsNoError);
@@ -379,35 +405,41 @@ GCUpdatePos mc_probe_cycle(float* target, plan_line_data_t* pl_data, uint8_t par
     // 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() ^ is_probe_away) {  // Check probe pin state.
-        system_set_exec_alarm(ExecAlarm::ProbeFailInitial);
+        sys_rt_exec_alarm = ExecAlarm::ProbeFailInitial;
         protocol_execute_realtime();
-        return GCUpdatePos::None;  // Nothing else to do but bail.
+        RESTORE_STEPPER(save_stepper);  // Switch the stepper mode to the previous mode
+        return GCUpdatePos::None;       // 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;
+    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);
+    sys_rt_exec_state.bit.cycleStart = true;
     do {
         protocol_execute_realtime();
         if (sys.abort) {
-            return GCUpdatePos::None;  // Check for system abort
+            RESTORE_STEPPER(save_stepper);  // Switch the stepper mode to the previous mode
+            return GCUpdatePos::None;       // Check for system abort
         }
     } while (sys.state != State::Idle);
+
+    // Switch the stepper mode to the previous mode
+    RESTORE_STEPPER(save_stepper);
+
     // 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 (sys_probe_state == Probe::Active) {
         if (is_no_error) {
             memcpy(sys_probe_position, sys_position, sizeof(sys_position));
         } else {
-            system_set_exec_alarm(ExecAlarm::ProbeFailContact);
+            sys_rt_exec_alarm = ExecAlarm::ProbeFailContact;
         }
     } else {
         sys.probe_succeeded = true;  // Indicate to system the probing cycle completed successfully.
     }
-    sys_probe_state = PROBE_OFF;  // Ensure probe state monitor is disabled.
-    protocol_execute_realtime();  // Check and execute run-time commands
+    sys_probe_state = Probe::Off;  // Ensure probe state monitor is disabled.
+    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.
@@ -431,9 +463,9 @@ void mc_parking_motion(float* parking_target, plan_line_data_t* pl_data) {
     }
     uint8_t plan_status = plan_buffer_line(parking_target, pl_data);
     if (plan_status) {
-        bit_true(sys.step_control, STEP_CONTROL_EXECUTE_SYS_MOTION);
-        bit_false(sys.step_control, STEP_CONTROL_END_MOTION);  // Allow parking motion to execute, if feed hold is active.
-        st_parking_setup_buffer();                             // Setup step segment buffer for special parking motion case
+        sys.step_control.executeSysMotion = true;
+        sys.step_control.endMotion        = false;  // Allow parking motion to execute, if feed hold is active.
+        st_parking_setup_buffer();                  // Setup step segment buffer for special parking motion case
         st_prep_buffer();
         st_wake_up();
         do {
@@ -441,10 +473,10 @@ void mc_parking_motion(float* parking_target, plan_line_data_t* pl_data) {
             if (sys.abort) {
                 return;
             }
-        } while (sys.step_control & STEP_CONTROL_EXECUTE_SYS_MOTION);
+        } while (sys.step_control.executeSysMotion);
         st_parking_restore_buffer();  // Restore step segment buffer to normal run state.
     } else {
-        bit_false(sys.step_control, STEP_CONTROL_EXECUTE_SYS_MOTION);
+        sys.step_control.executeSysMotion = false;
         protocol_exec_rt_system();
     }
 }
@@ -467,8 +499,8 @@ void mc_override_ctrl_update(uint8_t override_state) {
 // realtime abort command and hard limits. So, keep to a minimum.
 void mc_reset() {
     // 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);
+    if (!sys_rt_exec_state.bit.reset) {
+        sys_rt_exec_state.bit.reset = true;
         // Kill spindle and coolant.
         spindle->stop();
         coolant_stop();
@@ -489,13 +521,13 @@ void mc_reset() {
         // the steppers enabled by avoiding the go_idle call altogether, unless the motion state is
         // violated, by which, all bets are off.
         if ((sys.state == State::Cycle || sys.state == State::Homing || sys.state == State::Jog) ||
-            (sys.step_control & (STEP_CONTROL_EXECUTE_HOLD | STEP_CONTROL_EXECUTE_SYS_MOTION))) {
+            (sys.step_control.executeHold || sys.step_control.executeSysMotion)) {
             if (sys.state == State::Homing) {
                 if (sys_rt_exec_alarm == ExecAlarm::None) {
-                    system_set_exec_alarm(ExecAlarm::HomingFailReset);
+                    sys_rt_exec_alarm = ExecAlarm::HomingFailReset;
                 }
             } else {
-                system_set_exec_alarm(ExecAlarm::AbortCycle);
+                sys_rt_exec_alarm = ExecAlarm::AbortCycle;
             }
             st_go_idle();  // Force kill steppers. Position has likely been lost.
         }
diff --git a/Grbl_Esp32/src/Motors/Dynamixel2.cpp b/Grbl_Esp32/src/Motors/Dynamixel2.cpp
index fb0314f4..3c3faedc 100644
--- a/Grbl_Esp32/src/Motors/Dynamixel2.cpp
+++ b/Grbl_Esp32/src/Motors/Dynamixel2.cpp
@@ -4,7 +4,7 @@
     This allows an Dynamixel sero to be used like any other motor. Servos
     do have limitation in travel and speed, so you do need to respect that.
 
-    Protocol 2   
+    Protocol 2
 
     Part of Grbl_ESP32
 
@@ -27,33 +27,20 @@
 #include "Dynamixel2.h"
 
 namespace Motors {
-    Dynamixel2::Dynamixel2() {}
-
-    Dynamixel2::Dynamixel2(uint8_t axis_index, uint8_t id, uint8_t tx_pin, uint8_t rx_pin, uint8_t rts_pin) {
-        type_id                = DYNAMIXEL2;
-        this->_axis_index      = axis_index % MAX_AXES;
-        this->_dual_axis_index = axis_index < MAX_AXES ? 0 : 1;  // 0 = primary 1 = ganged
-
-        _id      = id;
-        _tx_pin  = tx_pin;
-        _rx_pin  = rx_pin;
-        _rts_pin = rts_pin;
+    bool    Motors::Dynamixel2::uart_ready         = false;
+    uint8_t Motors::Dynamixel2::ids[MAX_N_AXIS][2] = { { 0, 0 }, { 0, 0 }, { 0, 0 }, { 0, 0 }, { 0, 0 }, { 0, 0 } };
 
+    Dynamixel2::Dynamixel2(uint8_t axis_index, uint8_t id, uint8_t tx_pin, uint8_t rx_pin, uint8_t rts_pin) :
+        Servo(axis_index), _id(id), _tx_pin(tx_pin), _rx_pin(rx_pin), _rts_pin(rts_pin) {
         if (_tx_pin == UNDEFINED_PIN || _rx_pin == UNDEFINED_PIN || _rts_pin == UNDEFINED_PIN) {
-            grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Dymanixel Error. Missing pin definitions");
-            is_active = false;
-
-            return;
+            grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Dynamixel Error. Missing pin definitions");
+            _has_errors = true;
+        } else {
+            _has_errors = false;   // The motor can be used
         }
-
-        set_axis_name();
-        init();
     }
 
     void Dynamixel2::init() {
-        is_active = true;   // as opposed to NullMotors, this is a real motor
-        _can_home = false;  // this axis cannot be conventionally homed
-
         init_uart(_id, _axis_index, _dual_axis_index);  // static and only allows one init
 
         read_settings();
@@ -61,7 +48,7 @@ namespace Motors {
         config_message();  // print the config
 
         if (!test()) {  // ping the motor
-            is_active = false;
+            _has_errors = true;
             return;
         }
 
@@ -72,18 +59,19 @@ namespace Motors {
         LED_on(true);
         vTaskDelay(100);
         LED_on(false);
+
+        startUpdateTask();
     }
 
     void Dynamixel2::config_message() {
         grbl_msg_sendf(CLIENT_SERIAL,
                        MsgLevel::Info,
-                       "%s Axis Dynamixel Servo ID:%d Count(%5.0f,%5.0f) Limits(%0.3fmm,%5.3f)",
-                       _axis_name,
+                       "%s Dynamixel Servo ID:%d Count(%5.0f,%5.0f) %s",
+                       reportAxisNameMsg(_axis_index, _dual_axis_index),
                        _id,
                        _dxl_count_min,
                        _dxl_count_max,
-                       _position_min,
-                       _position_max);
+                       reportAxisLimitsMsg(_axis_index));
     }
 
     bool Dynamixel2::test() {
@@ -100,22 +88,23 @@ namespace Motors {
             if (model_num == 1060) {
                 grbl_msg_sendf(CLIENT_SERIAL,
                                MsgLevel::Info,
-                               "%s Axis Dynamixel Detected ID %d Model XL430-W250 F/W Rev %x",
-                               _axis_name,
+                               "%s Dynamixel Detected ID %d Model XL430-W250 F/W Rev %x",
+                               reportAxisNameMsg(_axis_index, _dual_axis_index),
                                _id,
                                _dxl_rx_message[11]);
             } else {
                 grbl_msg_sendf(CLIENT_SERIAL,
                                MsgLevel::Info,
-                               "%s Axis Dynamixel Detected ID %d M/N %d F/W Rev %x",
-                               _axis_name,
+                               "%s Dynamixel Detected ID %d M/N %d F/W Rev %x",
+                               reportAxisNameMsg(_axis_index, _dual_axis_index),
                                _id,
                                model_num,
                                _dxl_rx_message[11]);
             }
 
         } else {
-            grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "%s Axis Dynamixel Servo ID %d Ping failed", _axis_name, _id);
+            grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "%s Dynamixel Servo ID %d Ping failed", reportAxisNameMsg(_axis_index, _dual_axis_index),
+_id);
             return false;
         }
 
@@ -123,21 +112,10 @@ namespace Motors {
     }
 
     void Dynamixel2::read_settings() {
-        float travel = axis_settings[_axis_index]->max_travel->get();
-        float mpos   = axis_settings[_axis_index]->home_mpos->get();
-
-        if (bit_istrue(homing_dir_mask->get(), bit(_axis_index))) {
-            _position_min = mpos;
-            _position_max = mpos + travel;
-        } else {
-            _position_min = mpos - travel;
-            _position_max = mpos;
-        }
-
         _dxl_count_min = DXL_COUNT_MIN;
         _dxl_count_max = DXL_COUNT_MAX;
 
-        if (bit_istrue(dir_invert_mask->get(), bit(_axis_index)))  // normal direction
+        if (bitnum_istrue(dir_invert_mask->get(), _axis_index))  // normal direction
             swap(_dxl_count_min, _dxl_count_min);
     }
 
@@ -150,10 +128,7 @@ namespace Motors {
 
         _disabled = disable;
 
-        if (_disabled)
-            dxl_write(DXL_ADDR_TORQUE_EN, param_count, 0);
-        else
-            dxl_write(DXL_ADDR_TORQUE_EN, param_count, 1);
+        dxl_write(DXL_ADDR_TORQUE_EN, param_count, !_disabled);
     }
 
     void Dynamixel2::set_operating_mode(uint8_t mode) {
@@ -162,8 +137,9 @@ namespace Motors {
     }
 
     void Dynamixel2::update() {
-        if (!is_active)
+        if (_has_errors) {
             return;
+        }
 
         if (_disabled) {
             dxl_read_position();
@@ -212,12 +188,16 @@ namespace Motors {
 
     // This motor will not do a standard home to a limit switch (maybe future)
     // If it is in the homing mask it will a quick move to $<axis>/Home/Mpos
-    void Dynamixel2::set_homing_mode(uint8_t homing_mask, bool isHoming) {
+    bool Dynamixel2::set_homing_mode(bool isHoming) {
+        if (_has_errors) {
+            return false;
+        }
         sys_position[_axis_index] =
             axis_settings[_axis_index]->home_mpos->get() * axis_settings[_axis_index]->steps_per_mm->get();  // convert to steps
 
         set_disable(false);
         set_location();  // force the PWM to update now
+        return false;  // Cannot do conventional homing
     }
 
     void Dynamixel2::dxl_goal_position(int32_t position) {
@@ -244,8 +224,8 @@ namespace Motors {
 
             read_settings();
 
-            int32_t pos_min_steps = lround(_position_min * axis_settings[_axis_index]->steps_per_mm->get());
-            int32_t pos_max_steps = lround(_position_max * axis_settings[_axis_index]->steps_per_mm->get());
+            int32_t pos_min_steps = lround(limitsMinPosition(_axis_index) * axis_settings[_axis_index]->steps_per_mm->get());
+            int32_t pos_max_steps = lround(limitsMaxPosition(_axis_index) * axis_settings[_axis_index]->steps_per_mm->get());
 
             int32_t temp = map(dxl_position, DXL_COUNT_MIN, DXL_COUNT_MAX, pos_min_steps, pos_max_steps);
 
@@ -375,25 +355,14 @@ namespace Motors {
                     //determine the location of the axis
                     float target = system_convert_axis_steps_to_mpos(sys_position, axis);  // get the axis machine position in mm
 
-                    float travel = axis_settings[axis]->max_travel->get();
-                    float mpos   = axis_settings[axis]->home_mpos->get();
-
-                    if (bit_istrue(homing_dir_mask->get(), bit(axis))) {
-                        position_min = mpos;
-                        position_max = mpos + travel;
-                    } else {
-                        position_min = mpos - travel;
-                        position_max = mpos;
-                    }
-
                     dxl_count_min = DXL_COUNT_MIN;
                     dxl_count_max = DXL_COUNT_MAX;
 
-                    if (bit_istrue(dir_invert_mask->get(), bit(axis)))  // normal direction
+                    if (bitnum_istrue(dir_invert_mask->get(), axis))  // normal direction
                         swap(dxl_count_min, dxl_count_max);
 
                     // map the mm range to the servo range
-                    dxl_position = (uint32_t)mapConstrain(target, position_min, position_max, dxl_count_min, dxl_count_max);
+                    dxl_position = (uint32_t)mapConstrain(target, limitsMinPosition(axis), limitsMaxPosition(axis), dxl_count_min, dxl_count_max);
 
                     tx_message[++msg_index] = current_id;                         // ID of the servo
                     tx_message[++msg_index] = dxl_position & 0xFF;                // data
diff --git a/Grbl_Esp32/src/Motors/Dynamixel2.h b/Grbl_Esp32/src/Motors/Dynamixel2.h
index 99aa5bdb..d4a482c3 100644
--- a/Grbl_Esp32/src/Motors/Dynamixel2.h
+++ b/Grbl_Esp32/src/Motors/Dynamixel2.h
@@ -8,7 +8,7 @@
     Part of Grbl_ESP32
 
     2020 -	Bart Dring
-    
+
     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
@@ -69,25 +69,28 @@ const int DXL_CONTROL_MODE_POSITION = 3;
 #endif
 
 #include "Motor.h"
+#include "Servo.h"
 
 namespace Motors {
-    class Dynamixel2 : public Motor {
+    class Dynamixel2 : public Servo {
     public:
-        Dynamixel2();
         Dynamixel2(uint8_t axis_index, uint8_t address, uint8_t tx_pin, uint8_t rx_pin, uint8_t rts_pin);
 
-        virtual void config_message();
-        virtual void init();
-        virtual void set_disable(bool disable);
-        virtual void update();
+
+        // Overrides for inherited methods
+        void init() override;
+        void read_settings() override;
+        bool set_homing_mode(bool isHoming) override;
+        void set_disable(bool disable) override;
+        void update() override;
 
         static bool    uart_ready;
         static uint8_t ids[MAX_N_AXIS][2];
 
-        void set_homing_mode(uint8_t homing_mask, bool isHoming) override;
-        void read_settings();
 
     protected:
+        void config_message() override;
+
         void set_location();
 
         uint8_t _id;
@@ -108,7 +111,6 @@ namespace Motors {
         static uint16_t dxl_update_crc(uint16_t crc_accum, char* data_blk_ptr, uint8_t data_blk_size);
         static void     dxl_bulk_goal_position();  // set all motorsd init_uart(uint8_t id, uint8_t axis_index, uint8_t dual_axis_index);
 
-        
         float _homing_position;
 
         float _dxl_count_min;
@@ -118,5 +120,8 @@ namespace Motors {
         uint8_t     _rx_pin;
         uint8_t     _rts_pin;
         uart_port_t _uart_num;
+
+        bool _disabled;
+        bool _has_errors;
     };
 }
diff --git a/Grbl_Esp32/src/Motors/Motor.cpp b/Grbl_Esp32/src/Motors/Motor.cpp
index 5c431437..195601f8 100644
--- a/Grbl_Esp32/src/Motors/Motor.cpp
+++ b/Grbl_Esp32/src/Motors/Motor.cpp
@@ -34,34 +34,11 @@
 #include "Motor.h"
 
 namespace Motors {
-    Motor::Motor() { type_id = MOTOR; }
+    Motor::Motor(uint8_t axis_index) :
+        _axis_index(axis_index % MAX_AXES), _dual_axis_index(axis_index / MAX_AXES) {}
 
-    void Motor::init() { _homing_mask = 0; }
-
-    void Motor::config_message() {}
     void Motor::debug_message() {}
 
-    void Motor::read_settings() {
-        float max_travel = axis_settings[_axis_index]->max_travel->get();
-        float mpos       = axis_settings[_axis_index]->home_mpos->get();
-
-        if (bit_istrue(homing_dir_mask->get(), bit(_axis_index))) {
-            _position_min = mpos;
-            _position_max = mpos + max_travel;
-        } else {
-            _position_min = mpos - max_travel;
-            _position_max = mpos;
-        }
-    }
-
-    void Motor::set_disable(bool disable) {}
-    void Motor::set_direction_pins(uint8_t onMask) {}
-    void Motor::step(uint8_t step_mask, uint8_t dir_mask) {}
     bool Motor::test() { return true; };  // true = OK
-    void Motor::update() {}
-    bool Motor::can_home() { return _can_home; };
 
-    void Motor::set_axis_name() { sprintf(_axis_name, "%c%s", report_get_axis_letter(_axis_index), _dual_axis_index ? "2" : " "); }
-
-    void Motor::set_homing_mode(uint8_t homing_mask, bool isHoming) { _homing_mask = homing_mask; }
 }
diff --git a/Grbl_Esp32/src/Motors/Motor.h b/Grbl_Esp32/src/Motors/Motor.h
index a32dccab..c9c0300b 100644
--- a/Grbl_Esp32/src/Motors/Motor.h
+++ b/Grbl_Esp32/src/Motors/Motor.h
@@ -36,38 +36,82 @@
 namespace Motors {
     class Motor {
     public:
-        Motor();
+        Motor(uint8_t axis_index);
 
-        virtual void init();  // not in constructor because this also gets called when $$ settings change
-        virtual void config_message();
+        // init() establishes configured motor parameters.  It is called after
+        // all motor objects have been constructed.
+        virtual void init() {}
+
+        // debug_message() displays motor-specific information that can be
+        // used to assist with motor configuration.  For many motor types,
+        // it is a no-op.
+        // TODO Architecture: Should this be private?  It only applies to
+        // Trinamic drivers so maybe there is a cleaner approach to solving
+        // the stallguard debugging problem.
         virtual void debug_message();
-        virtual void read_settings();
-        virtual void set_homing_mode(uint8_t homing_mask, bool isHoming);
-        virtual void set_disable(bool disable);
-        virtual void set_direction_pins(uint8_t onMask);
-        virtual void step(uint8_t step_mask, uint8_t dir_mask);  // only used on Unipolar right now
-        virtual bool test();
-        virtual void set_axis_name();
-        virtual void update();
-        virtual bool can_home();
 
-        motor_class_id_t type_id;
-        uint8_t          is_active  = false;
-        uint8_t          has_errors = false;
+        // read_settings(), called from init() and motors_read_settings(),
+        // re-establishes the motor configuration parameters that come
+        // from $ settings.
+        // TODO Architecture: Maybe this should be subsumed by init()
+        virtual void read_settings() {}
+
+        // set_homing_mode() is called from motors_set_homing_mode(),
+        // which in turn is called at the beginning of a homing cycle
+        // with isHoming true, and at the end with isHoming false.
+        // Some motor types require differ setups for homing and
+        // normal operation.  Returns true if the motor can home
+        virtual bool set_homing_mode(bool isHoming) = 0;
+
+        // set_disable() disables or enables a motor.  It is used to
+        // make a motor transition between idle and non-idle states.
+        virtual void set_disable(bool disable) {}
+
+        // set_direction() sets the motor movement direction.  It is
+        // invoked for every motion segment.
+        virtual void set_direction(bool) {}
+
+        // step() initiates a step operation on a motor.  It is called
+        // from motors_step() for ever motor than needs to step now.
+        // For ordinary step/direction motors, it sets the step pin
+        // to the active state.
+        virtual void step() {}
+
+        // unstep() turns off the step pin, if applicable, for a motor.
+        // It is called from motors_unstep() for all motors, since
+        // motors_unstep() is used in many contexts where the previous
+        // states of the step pins are unknown.
+        virtual void unstep() {}
+
+        // test(), called from init(), checks to see if a motor is
+        // responsive, returning true on failure.  Typical
+        // implementations also display messages to show the result.
+        // TODO Architecture: Should this be private?
+        virtual bool test();
+
+        // update() is used for some types of "smart" motors that
+        // can be told to move to a specific position.  It is
+        // called from a periodic task.
+        virtual void update() {}
 
     protected:
+        // config_message(), called from init(), displays a message describing
+        // the motor configuration - pins and other motor-specific items
+        virtual void config_message() {}
+
+        // _axis_index is the axis from XYZABC, while
+        // _dual_axis_index is 0 for the primary motor on that
+        // axis and 1 for the ganged motor.
+        // These variables are used for several purposes:
+        // * Displaying the axis name in messages
+        // * When reading settings, determining which setting
+        //   applies to this motor
+        // * For some motor types, it is necessary to maintain
+        //   tables of all the motors of that type; those
+        //   tables can be indexed by these variables.
+        // TODO Architecture: It might be useful to cache a
+        // reference to the axis settings entry.
         uint8_t _axis_index;       // X_AXIS, etc
         uint8_t _dual_axis_index;  // 0 = primary 1=ganged
-
-        bool    _showError;
-        bool    _use_mpos = true;
-        uint8_t _homing_mask;
-        char    _axis_name[10];  // this the name to use when reporting like "X" or "X2"
-
-        float _position_min = 0;
-        float _position_max = 0;  // position in millimeters
-
-        bool _can_home = true;
-        bool _disabled;
     };
 }
diff --git a/Grbl_Esp32/src/Motors/Motors.cpp b/Grbl_Esp32/src/Motors/Motors.cpp
index a42b920d..8c7d550a 100644
--- a/Grbl_Esp32/src/Motors/Motors.cpp
+++ b/Grbl_Esp32/src/Motors/Motors.cpp
@@ -44,18 +44,6 @@
 #include "TrinamicDriver.h"
 
 Motors::Motor*      myMotor[MAX_AXES][MAX_GANGED];  // number of axes (normal and ganged)
-static TaskHandle_t readSgTaskHandle      = 0;      // for realtime stallguard data diaplay
-static TaskHandle_t servoUpdateTaskHandle = 0;
-
-bool    Motors::Dynamixel2::uart_ready         = false;
-uint8_t Motors::Dynamixel2::ids[MAX_N_AXIS][2] = { { 0, 0 }, { 0, 0 }, { 0, 0 }, { 0, 0 }, { 0, 0 }, { 0, 0 } };
-
-uint8_t      rmt_chan_num[MAX_AXES][MAX_GANGED];
-rmt_item32_t rmtItem[2];
-rmt_config_t rmtConfig;
-
-bool motor_class_steps;  // true if at least one motor class is handling steps
-
 void init_motors() {
     grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Init Motors");
 
@@ -64,9 +52,9 @@ void init_motors() {
     if (n_axis >= 1) {
 #ifdef X_TRINAMIC_DRIVER
         myMotor[X_AXIS][0] = new Motors::TrinamicDriver(
-            X_AXIS, X_STEP_PIN, X_DIRECTION_PIN, X_DISABLE_PIN, X_CS_PIN, X_TRINAMIC_DRIVER, X_RSENSE, get_next_trinamic_driver_index());
+            X_AXIS, X_STEP_PIN, X_DIRECTION_PIN, X_DISABLE_PIN, X_CS_PIN, X_TRINAMIC_DRIVER, X_RSENSE);
 #elif defined(X_SERVO_PIN)
-        myMotor[X_AXIS][0] = new Motors::RcServo(X_AXIS, X_SERVO_PIN, X_SERVO_CAL_MIN, X_SERVO_CAL_MAX);
+        myMotor[X_AXIS][0] = new Motors::RcServo(X_AXIS, X_SERVO_PIN);
 #elif defined(X_UNIPOLAR)
         myMotor[X_AXIS][0] = new Motors::UnipolarMotor(X_AXIS, X_PIN_PHASE_0, X_PIN_PHASE_1, X_PIN_PHASE_2, X_PIN_PHASE_3);
 #elif defined(X_STEP_PIN)
@@ -74,33 +62,31 @@ void init_motors() {
 #elif defined(X_DYNAMIXEL_ID)
         myMotor[X_AXIS][0] = new Motors::Dynamixel2(X_AXIS, X_DYNAMIXEL_ID, DYNAMIXEL_TXD, DYNAMIXEL_RXD, DYNAMIXEL_RTS);
 #else
-        myMotor[X_AXIS][0] = new Motors::Nullmotor();
+        myMotor[X_AXIS][0] = new Motors::Nullmotor(X_AXIS);
 #endif
 
 #ifdef X2_TRINAMIC_DRIVER
         myMotor[X_AXIS][1] = new Motors::TrinamicDriver(
-            X2_AXIS, X2_STEP_PIN, X2_DIRECTION_PIN, X2_DISABLE_PIN, X2_CS_PIN, X2_TRINAMIC_DRIVER, X2_RSENSE, get_next_trinamic_driver_index());
-#elif defined(X2_SERVO_PIN)
-        myMotor[X_AXIS][1] = new Motors::RcServo(X2_AXIS, X2_SERVO_PIN, X2_SERVO_CAL_MIN, X2_SERVO_CAL_MAX);
+            X2_AXIS, X2_STEP_PIN, X2_DIRECTION_PIN, X2_DISABLE_PIN, X2_CS_PIN, X2_TRINAMIC_DRIVER, X2_RSENSE);
 #elif defined(X2_UNIPOLAR)
         myMotor[X_AXIS][1] = new Motors::UnipolarMotor(X2_AXIS, X2_PIN_PHASE_0, X2_PIN_PHASE_1, X2_PIN_PHASE_2, X2_PIN_PHASE_3);
 #elif defined(X2_STEP_PIN)
         myMotor[X_AXIS][1] = new Motors::StandardStepper(X2_AXIS, X2_STEP_PIN, X2_DIRECTION_PIN, X2_DISABLE_PIN);
 #else
-        myMotor[X_AXIS][1] = new Motors::Nullmotor();
+        myMotor[X_AXIS][1] = new Motors::Nullmotor(X2_AXIS);
 #endif
     } else {
-        myMotor[X_AXIS][0] = new Motors::Nullmotor();
-        myMotor[X_AXIS][1] = new Motors::Nullmotor();
+        myMotor[X_AXIS][0] = new Motors::Nullmotor(X_AXIS);
+        myMotor[X_AXIS][1] = new Motors::Nullmotor(X2_AXIS);
     }
 
     if (n_axis >= 2) {
         // this WILL be done better with settings
 #ifdef Y_TRINAMIC_DRIVER
         myMotor[Y_AXIS][0] = new Motors::TrinamicDriver(
-            Y_AXIS, Y_STEP_PIN, Y_DIRECTION_PIN, Y_DISABLE_PIN, Y_CS_PIN, Y_TRINAMIC_DRIVER, Y_RSENSE, get_next_trinamic_driver_index());
+            Y_AXIS, Y_STEP_PIN, Y_DIRECTION_PIN, Y_DISABLE_PIN, Y_CS_PIN, Y_TRINAMIC_DRIVER, Y_RSENSE);
 #elif defined(Y_SERVO_PIN)
-        myMotor[Y_AXIS][0] = new Motors::RcServo(Y_AXIS, Y_SERVO_PIN, Y_SERVO_CAL_MIN, Y_SERVO_CAL_MAX);
+        myMotor[Y_AXIS][0] = new Motors::RcServo(Y_AXIS, Y_SERVO_PIN);
 #elif defined(Y_UNIPOLAR)
         myMotor[Y_AXIS][0] = new Motors::UnipolarMotor(Y_AXIS, Y_PIN_PHASE_0, Y_PIN_PHASE_1, Y_PIN_PHASE_2, Y_PIN_PHASE_3);
 #elif defined(Y_STEP_PIN)
@@ -108,33 +94,31 @@ void init_motors() {
 #elif defined(Y_DYNAMIXEL_ID)
         myMotor[Y_AXIS][0] = new Motors::Dynamixel2(Y_AXIS, Y_DYNAMIXEL_ID, DYNAMIXEL_TXD, DYNAMIXEL_RXD, DYNAMIXEL_RTS);
 #else
-        myMotor[Y_AXIS][0] = new Motors::Nullmotor();
+        myMotor[Y_AXIS][0] = new Motors::Nullmotor(Y_AXIS);
 #endif
 
 #ifdef Y2_TRINAMIC_DRIVER
         myMotor[Y_AXIS][1] = new Motors::TrinamicDriver(
-            Y2_AXIS, Y2_STEP_PIN, Y2_DIRECTION_PIN, Y2_DISABLE_PIN, Y2_CS_PIN, Y2_TRINAMIC_DRIVER, Y2_RSENSE, get_next_trinamic_driver_index());
-#elif defined(Y2_SERVO_PIN)
-        myMotor[Y_AXIS][1] = new Motors::RcServo(Y2_AXIS, Y2_SERVO_PIN, Y2_SERVO_CAL_MIN, Y2_SERVO_CAL_MAX);
+            Y2_AXIS, Y2_STEP_PIN, Y2_DIRECTION_PIN, Y2_DISABLE_PIN, Y2_CS_PIN, Y2_TRINAMIC_DRIVER, Y2_RSENSE);
 #elif defined(Y2_UNIPOLAR)
         myMotor[Y_AXIS][1] = new Motors::UnipolarMotor(Y2_AXIS, Y2_PIN_PHASE_0, Y2_PIN_PHASE_1, Y2_PIN_PHASE_2, Y2_PIN_PHASE_3);
 #elif defined(Y2_STEP_PIN)
         myMotor[Y_AXIS][1] = new Motors::StandardStepper(Y2_AXIS, Y2_STEP_PIN, Y2_DIRECTION_PIN, Y2_DISABLE_PIN);
 #else
-        myMotor[Y_AXIS][1] = new Motors::Nullmotor();
+        myMotor[Y_AXIS][1] = new Motors::Nullmotor(Y2_AXIS);
 #endif
     } else {
-        myMotor[Y_AXIS][0] = new Motors::Nullmotor();
-        myMotor[Y_AXIS][1] = new Motors::Nullmotor();
+        myMotor[Y_AXIS][0] = new Motors::Nullmotor(Y_AXIS);
+        myMotor[Y_AXIS][1] = new Motors::Nullmotor(Y2_AXIS);
     }
 
     if (n_axis >= 3) {
         // this WILL be done better with settings
 #ifdef Z_TRINAMIC_DRIVER
         myMotor[Z_AXIS][0] = new Motors::TrinamicDriver(
-            Z_AXIS, Z_STEP_PIN, Z_DIRECTION_PIN, Z_DISABLE_PIN, Z_CS_PIN, Z_TRINAMIC_DRIVER, Z_RSENSE, get_next_trinamic_driver_index());
+            Z_AXIS, Z_STEP_PIN, Z_DIRECTION_PIN, Z_DISABLE_PIN, Z_CS_PIN, Z_TRINAMIC_DRIVER, Z_RSENSE);
 #elif defined(Z_SERVO_PIN)
-        myMotor[Z_AXIS][0] = new Motors::RcServo(Z_AXIS, Z_SERVO_PIN, Z_SERVO_CAL_MIN, Z_SERVO_CAL_MAX);
+        myMotor[Z_AXIS][0] = new Motors::RcServo(Z_AXIS, Z_SERVO_PIN);
 #elif defined(Z_UNIPOLAR)
         myMotor[Z_AXIS][0] = new Motors::UnipolarMotor(Z_AXIS, Z_PIN_PHASE_0, Z_PIN_PHASE_1, Z_PIN_PHASE_2, Z_PIN_PHASE_3);
 #elif defined(Z_STEP_PIN)
@@ -142,120 +126,118 @@ void init_motors() {
 #elif defined(Z_DYNAMIXEL_ID)
         myMotor[Z_AXIS][0] = new Motors::Dynamixel2(Z_AXIS, Z_DYNAMIXEL_ID, DYNAMIXEL_TXD, DYNAMIXEL_RXD, DYNAMIXEL_RTS);
 #else
-        myMotor[Z_AXIS][0] = new Motors::Nullmotor();
+        myMotor[Z_AXIS][0] = new Motors::Nullmotor(Z_AXIS);
 #endif
 
 #ifdef Z2_TRINAMIC_DRIVER
         myMotor[Z_AXIS][1] = new Motors::TrinamicDriver(
-            Z2_AXIS, Z2_STEP_PIN, Z2_DIRECTION_PIN, Z2_DISABLE_PIN, Z2_CS_PIN, Z2_TRINAMIC_DRIVER, Z2_RSENSE, get_next_trinamic_driver_index());
-#elif defined(Z2_SERVO_PIN)
-        myMotor[Z_AXIS][1] = new Motors::RcServo(Z2_AXIS, Z2_SERVO_PIN, Z2_SERVO_CAL_MIN, Z2_SERVO_CAL_MAX);
+            Z2_AXIS, Z2_STEP_PIN, Z2_DIRECTION_PIN, Z2_DISABLE_PIN, Z2_CS_PIN, Z2_TRINAMIC_DRIVER, Z2_RSENSE);
 #elif defined(Z2_UNIPOLAR)
         myMotor[Z_AXIS][1] = new Motors::UnipolarMotor(Z2_AXIS, Z2_PIN_PHASE_0, Z2_PIN_PHASE_1, Z2_PIN_PHASE_2, Z2_PIN_PHASE_3);
 #elif defined(Z2_STEP_PIN)
         myMotor[Z_AXIS][1] = new Motors::StandardStepper(Z2_AXIS, Z2_STEP_PIN, Z2_DIRECTION_PIN, Z2_DISABLE_PIN);
 #else
-        myMotor[Z_AXIS][1] = new Motors::Nullmotor();
+        myMotor[Z_AXIS][1] = new Motors::Nullmotor(Z2_AXIS);
 #endif
     } else {
-        myMotor[Z_AXIS][0] = new Motors::Nullmotor();
-        myMotor[Z_AXIS][1] = new Motors::Nullmotor();
+        myMotor[Z_AXIS][0] = new Motors::Nullmotor(Z_AXIS);
+        myMotor[Z_AXIS][1] = new Motors::Nullmotor(Z2_AXIS);
     }
 
     if (n_axis >= 4) {
         // this WILL be done better with settings
 #ifdef A_TRINAMIC_DRIVER
         myMotor[A_AXIS][0] = new Motors::TrinamicDriver(
-            A_AXIS, A_STEP_PIN, A_DIRECTION_PIN, A_DISABLE_PIN, A_CS_PIN, A_TRINAMIC_DRIVER, A_RSENSE, get_next_trinamic_driver_index());
+            A_AXIS, A_STEP_PIN, A_DIRECTION_PIN, A_DISABLE_PIN, A_CS_PIN, A_TRINAMIC_DRIVER, A_RSENSE);
 #elif defined(A_SERVO_PIN)
-        myMotor[A_AXIS][0] = new Motors::RcServo(A_AXIS, A_SERVO_PIN, A_SERVO_CAL_MIN, A_SERVO_CAL_MAX);
+        myMotor[A_AXIS][0] = new Motors::RcServo(A_AXIS, A_SERVO_PIN);
 #elif defined(A_UNIPOLAR)
         myMotor[A_AXIS][0] = new Motors::UnipolarMotor(A_AXIS, A_PIN_PHASE_0, A_PIN_PHASE_1, A_PIN_PHASE_2, A_PIN_PHASE_3);
 #elif defined(A_STEP_PIN)
         myMotor[A_AXIS][0] = new Motors::StandardStepper(A_AXIS, A_STEP_PIN, A_DIRECTION_PIN, A_DISABLE_PIN);
+#elif defined(A_DYNAMIXEL_ID)
+        myMotor[A_AXIS][0] = new Motors::Dynamixel2(A_AXIS, A_DYNAMIXEL_ID, DYNAMIXEL_TXD, DYNAMIXEL_RXD, DYNAMIXEL_RTS);
 #else
-        myMotor[A_AXIS][0] = new Motors::Nullmotor();
+        myMotor[A_AXIS][0] = new Motors::Nullmotor(A_AXIS);
 #endif
 
 #ifdef A2_TRINAMIC_DRIVER
         myMotor[A_AXIS][1] = new Motors::TrinamicDriver(
-            A2_AXIS, A2_STEP_PIN, A2_DIRECTION_PIN, A2_DISABLE_PIN, A2_CS_PIN, A2_TRINAMIC_DRIVER, A2_RSENSE, get_next_trinamic_driver_index());
-#elif defined(A2_SERVO_PIN)
-        myMotor[A_AXIS][1] = new Motors::RcServo(A2_AXIS, A2_SERVO_PIN, A2_SERVO_CAL_MIN, A2_SERVO_CAL_MAX);
+            A2_AXIS, A2_STEP_PIN, A2_DIRECTION_PIN, A2_DISABLE_PIN, A2_CS_PIN, A2_TRINAMIC_DRIVER, A2_RSENSE);
 #elif defined(A2_UNIPOLAR)
         myMotor[A_AXIS][1] = new Motors::UnipolarMotor(A2_AXIS, A2_PIN_PHASE_0, A2_PIN_PHASE_1, A2_PIN_PHASE_2, A2_PIN_PHASE_3);
 #elif defined(A2_STEP_PIN)
         myMotor[A_AXIS][1] = new Motors::StandardStepper(A2_AXIS, A2_STEP_PIN, A2_DIRECTION_PIN, A2_DISABLE_PIN);
 #else
-        myMotor[A_AXIS][1] = new Motors::Nullmotor();
+        myMotor[A_AXIS][1] = new Motors::Nullmotor(A2_AXIS);
 #endif
     } else {
-        myMotor[A_AXIS][0] = new Motors::Nullmotor();
-        myMotor[A_AXIS][1] = new Motors::Nullmotor();
+        myMotor[A_AXIS][0] = new Motors::Nullmotor(A_AXIS);
+        myMotor[A_AXIS][1] = new Motors::Nullmotor(A2_AXIS);
     }
 
     if (n_axis >= 5) {
         // this WILL be done better with settings
 #ifdef B_TRINAMIC_DRIVER
         myMotor[B_AXIS][0] = new Motors::TrinamicDriver(
-            B_AXIS, B_STEP_PIN, B_DIRECTION_PIN, B_DISABLE_PIN, B_CS_PIN, B_TRINAMIC_DRIVER, B_RSENSE, get_next_trinamic_driver_index());
+            B_AXIS, B_STEP_PIN, B_DIRECTION_PIN, B_DISABLE_PIN, B_CS_PIN, B_TRINAMIC_DRIVER, B_RSENSE);
 #elif defined(B_SERVO_PIN)
-        myMotor[B_AXIS][0] = new Motors::RcServo(B_AXIS, B_SERVO_PIN, B_SERVO_CAL_MIN, B_SERVO_CAL_MAX);
+        myMotor[B_AXIS][0] = new Motors::RcServo(B_AXIS, B_SERVO_PIN);
 #elif defined(B_UNIPOLAR)
         myMotor[B_AXIS][0] = new Motors::UnipolarMotor(B_AXIS, B_PIN_PHASE_0, B_PIN_PHASE_1, B_PIN_PHASE_2, B_PIN_PHASE_3);
 #elif defined(B_STEP_PIN)
         myMotor[B_AXIS][0] = new Motors::StandardStepper(B_AXIS, B_STEP_PIN, B_DIRECTION_PIN, B_DISABLE_PIN);
+#elif defined(B_DYNAMIXEL_ID)
+        myMotor[B_AXIS][0] = new Motors::Dynamixel2(B_AXIS, B_DYNAMIXEL_ID, DYNAMIXEL_TXD, DYNAMIXEL_RXD, DYNAMIXEL_RTS);
 #else
-        myMotor[B_AXIS][0] = new Motors::Nullmotor();
+        myMotor[B_AXIS][0] = new Motors::Nullmotor(B_AXIS);
 #endif
 
 #ifdef B2_TRINAMIC_DRIVER
         myMotor[B_AXIS][1] = new Motors::TrinamicDriver(
-            B2_AXIS, B2_STEP_PIN, B2_DIRECTION_PIN, B2_DISABLE_PIN, B2_CS_PIN, B2_TRINAMIC_DRIVER, B2_RSENSE, get_next_trinamic_driver_index());
-#elif defined(B2_SERVO_PIN)
-        myMotor[B_AXIS][1] = new Motors::RcServo(B2_AXIS, B2_SERVO_PIN, B2_SERVO_CAL_MIN, B2_SERVO_CAL_MAX);
+            B2_AXIS, B2_STEP_PIN, B2_DIRECTION_PIN, B2_DISABLE_PIN, B2_CS_PIN, B2_TRINAMIC_DRIVER, B2_RSENSE);
 #elif defined(B2_UNIPOLAR)
         myMotor[B_AXIS][1] = new Motors::UnipolarMotor(B2_AXIS, B2_PIN_PHASE_0, B2_PIN_PHASE_1, B2_PIN_PHASE_2, B2_PIN_PHASE_3);
 #elif defined(B2_STEP_PIN)
         myMotor[B_AXIS][1] = new Motors::StandardStepper(B2_AXIS, B2_STEP_PIN, B2_DIRECTION_PIN, B2_DISABLE_PIN);
 #else
-        myMotor[B_AXIS][1] = new Motors::Nullmotor();
+        myMotor[B_AXIS][1] = new Motors::Nullmotor(B2_AXIS);
 #endif
     } else {
-        myMotor[B_AXIS][0] = new Motors::Nullmotor();
-        myMotor[B_AXIS][1] = new Motors::Nullmotor();
+        myMotor[B_AXIS][0] = new Motors::Nullmotor(B_AXIS);
+        myMotor[B_AXIS][1] = new Motors::Nullmotor(B2_AXIS);
     }
 
     if (n_axis >= 6) {
         // this WILL be done better with settings
 #ifdef C_TRINAMIC_DRIVER
         myMotor[C_AXIS][0] = new Motors::TrinamicDriver(
-            C_AXIS, C_STEP_PIN, C_DIRECTION_PIN, C_DISABLE_PIN, C_CS_PIN, C_TRINAMIC_DRIVER, C_RSENSE, get_next_trinamic_driver_index());
+            C_AXIS, C_STEP_PIN, C_DIRECTION_PIN, C_DISABLE_PIN, C_CS_PIN, C_TRINAMIC_DRIVER, C_RSENSE);
 #elif defined(C_SERVO_PIN)
-        myMotor[C_AXIS][0] = new Motors::RcServo(C_AXIS, C_SERVO_PIN, C_SERVO_CAL_MIN, C_SERVO_CAL_MAX);
+        myMotor[C_AXIS][0] = new Motors::RcServo(C_AXIS, C_SERVO_PIN);
 #elif defined(C_UNIPOLAR)
         myMotor[C_AXIS][0] = new Motors::UnipolarMotor(C_AXIS, C_PIN_PHASE_0, C_PIN_PHASE_1, C_PIN_PHASE_2, C_PIN_PHASE_3);
 #elif defined(C_STEP_PIN)
         myMotor[C_AXIS][0] = new Motors::StandardStepper(C_AXIS, C_STEP_PIN, C_DIRECTION_PIN, C_DISABLE_PIN);
+#elif defined(C_DYNAMIXEL_ID)
+        myMotor[C_AXIS][0] = new Motors::Dynamixel2(C_AXIS, C_DYNAMIXEL_ID, DYNAMIXEL_TXD, DYNAMIXEL_RXD, DYNAMIXEL_RTS);
 #else
-        myMotor[C_AXIS][0] = new Motors::Nullmotor();
+        myMotor[C_AXIS][0] = new Motors::Nullmotor(C_AXIS);
 #endif
 
 #ifdef C2_TRINAMIC_DRIVER
         myMotor[C_AXIS][1] = new Motors::TrinamicDriver(
-            C2_AXIS, C2_STEP_PIN, C2_DIRECTION_PIN, C2_DISABLE_PIN, C2_CS_PIN, C2_TRINAMIC_DRIVER, C2_RSENSE, get_next_trinamic_driver_index());
-#elif defined(C2_SERVO_PIN)
-        myMotor[C_AXIS][1] = new Motors::RcServo(C2_AXIS, C2_SERVO_PIN, C2_SERVO_CAL_MIN, C2_SERVO_CAL_MAX);
+            C2_AXIS, C2_STEP_PIN, C2_DIRECTION_PIN, C2_DISABLE_PIN, C2_CS_PIN, C2_TRINAMIC_DRIVER, C2_RSENSE);
 #elif defined(C2_UNIPOLAR)
         myMotor[C_AXIS][1] = new Motors::UnipolarMotor(C2_AXIS, C2_PIN_PHASE_0, C2_PIN_PHASE_1, C2_PIN_PHASE_2, C2_PIN_PHASE_3);
 #elif defined(C2_STEP_PIN)
         myMotor[C_AXIS][1] = new Motors::StandardStepper(C2_AXIS, C2_STEP_PIN, C2_DIRECTION_PIN, C2_DISABLE_PIN);
 #else
-        myMotor[C_AXIS][1] = new Motors::Nullmotor();
+        myMotor[C_AXIS][1] = new Motors::Nullmotor(C2_AXIS);
 #endif
     } else {
-        myMotor[C_AXIS][0] = new Motors::Nullmotor();
-        myMotor[C_AXIS][1] = new Motors::Nullmotor();
+        myMotor[C_AXIS][0] = new Motors::Nullmotor(C_AXIS);
+        myMotor[C_AXIS][1] = new Motors::Nullmotor(C2_AXIS);
     }
 
 #ifdef USE_STEPSTICK
@@ -292,77 +274,9 @@ void init_motors() {
     // certain motors need features to be turned on. Check them here
     for (uint8_t axis = X_AXIS; axis < n_axis; axis++) {
         for (uint8_t gang_index = 0; gang_index < 2; gang_index++) {
-            if (myMotor[axis][gang_index]->type_id == UNIPOLAR_MOTOR) {
-                motor_class_steps = true;
-            }
-
-            // CS Pins of all TMC motors need to be setup before any can be talked to
-            // ...so init cannot be called via the constructors. This inits them all.
-            if (myMotor[axis][gang_index]->type_id == TRINAMIC_SPI_MOTOR) {
-                myMotor[axis][gang_index]->init();
-            }
+            myMotor[axis][gang_index]->init();
         }
     }
-
-    // some motor objects require a step signal
-    motor_class_steps = motors_have_type_id(UNIPOLAR_MOTOR);
-
-    if (motors_have_type_id(TRINAMIC_SPI_MOTOR)) {
-        grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "TMCStepper Library Ver. 0x%06x", TMCSTEPPER_VERSION);
-        xTaskCreatePinnedToCore(readSgTask,    // task
-                                "readSgTask",  // name for task
-                                4096,          // size of task stack
-                                NULL,          // parameters
-                                1,             // priority
-                                &readSgTaskHandle,
-                                0  // core
-        );
-        if (stallguard_debug_mask->get() != 0) {
-            grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Stallguard debug enabled: %d", stallguard_debug_mask->get());
-        }
-    }
-
-    if (motors_have_type_id(RC_SERVO_MOTOR) || motors_have_type_id(DYNAMIXEL2)) {
-        xTaskCreatePinnedToCore(servoUpdateTask,    // task
-                                "servoUpdateTask",  // name for task
-                                4096,               // size of task stack
-                                NULL,               // parameters
-                                1,                  // priority
-                                &servoUpdateTaskHandle,
-                                0  // core
-        );
-    }
-}
-
-void servoUpdateTask(void* pvParameters) {
-    TickType_t       xLastWakeTime;
-    const TickType_t xUpdate = SERVO_TIMER_INTERVAL;  // in ticks (typically ms)
-    auto             n_axis  = number_axis->get();
-
-    xLastWakeTime = xTaskGetTickCount();  // Initialise the xLastWakeTime variable with the current time.
-    vTaskDelay(2000);                     // initial delay
-    while (true) {                        // don't ever return from this or the task dies
-        for (uint8_t axis = X_AXIS; axis < n_axis; axis++) {
-            for (uint8_t gang_index = 0; gang_index < 2; gang_index++) {
-                myMotor[axis][gang_index]->update();
-            }
-        }
-
-        vTaskDelayUntil(&xLastWakeTime, xUpdate);
-    }
-}
-
-// do any motors match the type_id
-bool motors_have_type_id(motor_class_id_t id) {
-    auto n_axis = number_axis->get();
-    for (uint8_t axis = X_AXIS; axis < n_axis; axis++) {
-        for (uint8_t gang_index = 0; gang_index < 2; gang_index++) {
-            if (myMotor[axis][gang_index]->type_id == id) {
-                return true;
-            }
-        }
-    }
-    return false;
 }
 
 void motors_set_disable(bool disable) {
@@ -370,14 +284,14 @@ void motors_set_disable(bool disable) {
 
     //grbl_msg_sendf(CLIENT_SERIAL, MSG_LEVEL_INFO, "Motors disable %d", disable);
 
-/*
+    /*
     if (previous_state == disable) {
         return;
     }
-    previous_state = disable;    
+    previous_state = disable;
 */
 
-    // now loop through all the motors to see if they can individually diable
+    // now loop through all the motors to see if they can individually disable
     auto n_axis = number_axis->get();
     for (uint8_t gang_index = 0; gang_index < MAX_GANGED; gang_index++) {
         for (uint8_t axis = X_AXIS; axis < n_axis; axis++) {
@@ -390,7 +304,6 @@ void motors_set_disable(bool disable) {
         disable = !disable;  // Apply pin invert.
     }
     digitalWrite(STEPPERS_DISABLE_PIN, disable);
-    
 }
 
 void motors_read_settings() {
@@ -405,98 +318,53 @@ void motors_read_settings() {
 
 // use this to tell all the motors what the current homing mode is
 // They can use this to setup things like Stall
-void motors_set_homing_mode(uint8_t homing_mask, bool isHoming) {
+uint8_t motors_set_homing_mode(uint8_t homing_mask, bool isHoming) {
+    uint8_t can_home = 0;
     auto n_axis = number_axis->get();
-    for (uint8_t gang_index = 0; gang_index < 2; gang_index++) {
-        for (uint8_t axis = X_AXIS; axis < n_axis; axis++) {
-            if (bit_istrue(homing_mask, bit(axis)) && (myMotor[axis][gang_index]->is_active)) {
-                myMotor[axis][gang_index]->set_homing_mode(homing_mask, isHoming);
+    for (uint8_t axis = X_AXIS; axis < n_axis; axis++) {
+        if (bitnum_istrue(homing_mask, axis)) {
+            if (myMotor[axis][0]->set_homing_mode(isHoming)) {
+                bitnum_true(can_home, axis);
             }
+            myMotor[axis][1]->set_homing_mode(isHoming);
         }
     }
+    return can_home;
 }
 
-void motors_set_direction_pins(uint8_t onMask) {
-    static uint8_t previous_val = 255;  // should never be this value
-    if (previous_val == onMask) {
-        return;
-    }
-    previous_val = onMask;
-
+void motors_step(uint8_t step_mask, uint8_t dir_mask) {
+    auto n_axis = number_axis->get();
     //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "motors_set_direction_pins:0x%02X", onMask);
 
-    auto n_axis = number_axis->get();
-    for (uint8_t gang_index = 0; gang_index < MAX_GANGED; gang_index++) {
-        for (uint8_t axis = X_AXIS; axis < n_axis; axis++) {
-            myMotor[axis][gang_index]->set_direction_pins(onMask);
+    // Set the direction pins, but optimize for the common
+    // situation where the direction bits haven't changed.
+    static uint8_t previous_dir = 255;  // should never be this value
+    if (dir_mask != previous_dir) {
+        previous_dir = dir_mask;
+
+        for (int axis = X_AXIS; axis < n_axis; axis++) {
+            bool thisDir = bitnum_istrue(dir_mask, axis);
+            myMotor[axis][0]->set_direction(thisDir);
+            myMotor[axis][1]->set_direction(thisDir);
         }
     }
-}
-
-// returns the next spi index. We cannot preassign to axes because ganged (X2 type axes) might
-// need to be inserted into the order of axes.
-uint8_t get_next_trinamic_driver_index() {
-#ifdef TRINAMIC_DAISY_CHAIN
-    static uint8_t index = 1;  // they start at 1
-    return index++;
-#else
-    return -1;
-#endif
-}
-
-// some motor objects, like unipolar need step signals
-void motors_step(uint8_t step_mask, uint8_t dir_mask) {
-    if (motor_class_steps) {  // determined in init_motors if any motors need to handle steps
-        auto n_axis = number_axis->get();
-        for (uint8_t gang_index = 0; gang_index < 2; gang_index++) {
-            for (uint8_t axis = X_AXIS; axis < n_axis; axis++) {
-                myMotor[axis][gang_index]->step(step_mask, dir_mask);
+    // Turn on step pulses for motors that are supposed to step now
+    for (uint8_t axis = X_AXIS; axis < n_axis; axis++) {
+        if (bitnum_istrue(step_mask, axis)) {
+            if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::A)) {
+                myMotor[axis][0]->step();
+            }
+            if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::B)) {
+                myMotor[axis][1]->step();
             }
         }
     }
 }
-
-/*
-	This will print StallGuard data that is useful for tuning.
-*/
-void readSgTask(void* pvParameters) {
-    TickType_t       xLastWakeTime;
-    const TickType_t xreadSg = 200;  // in ticks (typically ms)
-    auto             n_axis  = number_axis->get();
-
-    xLastWakeTime = xTaskGetTickCount();  // Initialise the xLastWakeTime variable with the current time.
-    while (true) {                        // don't ever return from this or the task dies
-        if (motorSettingChanged) {
-            motors_read_settings();
-            motorSettingChanged = false;
-        }
-
-        if (stallguard_debug_mask->get() != 0) {
-            if (sys.state == State::Cycle || sys.state == State::Homing || sys.state == State::Jog) {
-                for (uint8_t axis = X_AXIS; axis < n_axis; axis++) {
-                    if (stallguard_debug_mask->get() & bit(axis)) {
-                        //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "SG:%d", stallguard_debug_mask->get());
-                        for (uint8_t gang_index = 0; gang_index < 2; gang_index++) {
-                            myMotor[axis][gang_index]->debug_message();
-                        }
-                    }
-                }
-            }  // sys.state
-        }      // if mask
-        vTaskDelayUntil(&xLastWakeTime, xreadSg);
+// Turn all stepper pins off
+void motors_unstep() {
+    auto n_axis = number_axis->get();
+    for (uint8_t axis = X_AXIS; axis < n_axis; axis++) {
+        myMotor[axis][0]->unstep();
+        myMotor[axis][1]->unstep();
     }
 }
-
-bool motor_can_home(uint8_t axis) {
-    return myMotor[axis][0]->can_home();
-}
-
-#ifdef USE_I2S_OUT
-//
-// Override default function and insert a short delay
-//
-void TMC2130Stepper::switchCSpin(bool state) {
-    digitalWrite(_pinCS, state);
-    i2s_out_delay();
-}
-#endif
diff --git a/Grbl_Esp32/src/Motors/Motors.h b/Grbl_Esp32/src/Motors/Motors.h
index 70536f99..a6451eff 100644
--- a/Grbl_Esp32/src/Motors/Motors.h
+++ b/Grbl_Esp32/src/Motors/Motors.h
@@ -30,34 +30,17 @@
 */
 
 #include "../Grbl.h"
-#include <TMCStepper.h>  // https://github.com/teemuatlut/TMCStepper
-
-extern uint8_t      rmt_chan_num[MAX_AXES][2];
-extern rmt_item32_t rmtItem[2];
-extern rmt_config_t rmtConfig;
-
-typedef enum {
-    MOTOR,
-    NULL_MOTOR,
-    STANDARD_MOTOR,
-    TRINAMIC_SPI_MOTOR,
-    UNIPOLAR_MOTOR,
-    RC_SERVO_MOTOR,
-    SOLENOID,
-    DYNAMIXEL2
-} motor_class_id_t;
 
 // These are used for setup and to talk to the motors as a group.
 void    init_motors();
 uint8_t get_next_trinamic_driver_index();
-bool    motors_have_type_id(motor_class_id_t id);
 void    readSgTask(void* pvParameters);
 void    motors_read_settings();
-void    motors_set_homing_mode(uint8_t homing_mask, bool isHoming);
-void    motors_set_disable(bool disable);
-void    motors_set_direction_pins(uint8_t onMask);
-void    motors_step(uint8_t step_mask, uint8_t dir_mask);
-void    servoUpdateTask(void* pvParameters);
-bool    motor_can_home(uint8_t index);
 
-extern bool motor_class_steps;  // true if at least one motor class is handling steps
+// The return value is a bitmask of axes that can home
+uint8_t motors_set_homing_mode(uint8_t homing_mask, bool isHoming);
+void    motors_set_disable(bool disable);
+void    motors_step(uint8_t step_mask, uint8_t dir_mask);
+void    motors_unstep();
+
+void servoUpdateTask(void* pvParameters);
diff --git a/Grbl_Esp32/src/Motors/NullMotor.cpp b/Grbl_Esp32/src/Motors/NullMotor.cpp
new file mode 100644
index 00000000..be5b34a2
--- /dev/null
+++ b/Grbl_Esp32/src/Motors/NullMotor.cpp
@@ -0,0 +1,28 @@
+/*
+    NullMotor.cpp
+
+    This is a fake motor that does nothing.
+
+    Part of Grbl_ESP32
+
+    2020 -	Bart Dring
+
+    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 "NullMotor.h"
+
+namespace Motors {
+   Nullmotor::Nullmotor(uint8_t axis_index) :
+       Motor(axis_index)
+   {}
+}
diff --git a/Grbl_Esp32/src/Motors/NullMotor.h b/Grbl_Esp32/src/Motors/NullMotor.h
index e360392d..6e859855 100644
--- a/Grbl_Esp32/src/Motors/NullMotor.h
+++ b/Grbl_Esp32/src/Motors/NullMotor.h
@@ -3,5 +3,9 @@
 #include "Motor.h"
 
 namespace Motors {
-    class Nullmotor : public Motor {};
+    class Nullmotor : public Motor {
+    public:
+        Nullmotor(uint8_t axis_index);
+        bool set_homing_mode(bool isHoming) { return false; }
+    };
 }
diff --git a/Grbl_Esp32/src/Motors/RcServo.cpp b/Grbl_Esp32/src/Motors/RcServo.cpp
index eabcf27b..a5716701 100644
--- a/Grbl_Esp32/src/Motors/RcServo.cpp
+++ b/Grbl_Esp32/src/Motors/RcServo.cpp
@@ -1,7 +1,7 @@
 /*
     RcServo.cpp
 
-    This allows an RcServo to be used like any other motor. Serrvos
+    This allows an RcServo to be used like any other motor. Servos
     do have limitation in travel and speed, so you do need to respect that.
 
     Part of Grbl_ESP32
@@ -10,9 +10,9 @@
 
     The servo's travel will be mapped against the axis with $X/MaxTravel
 
-    The rotation can be inverted with by $Stepper/DirInvert    
+    The rotation can be inverted with by $Stepper/DirInvert
 
-    Homing simply sets the axis Mpos to the endpoint as determined by $Homing/DirInver    
+    Homing simply sets the axis Mpos to the endpoint as determined by $Homing/DirInvert
 
     Calibration is part of the setting (TBD) fixed at 1.00 now
 
@@ -31,45 +31,40 @@
 #include "RcServo.h"
 
 namespace Motors {
-    RcServo::RcServo() {}
-
-    RcServo::RcServo(uint8_t axis_index, uint8_t pwm_pin, float cal_min, float cal_max) {
-        type_id                = RC_SERVO_MOTOR;
-        this->_axis_index      = axis_index % MAX_AXES;
-        this->_dual_axis_index = axis_index < MAX_AXES ? 0 : 1;  // 0 = primary 1 = ganged
-        this->_pwm_pin         = pwm_pin;
-        _cal_min               = cal_min;
-        _cal_max               = cal_max;
-        init();
-    }
+    RcServo::RcServo(uint8_t axis_index, uint8_t pwm_pin) : Servo(axis_index), _pwm_pin(pwm_pin) {}
 
     void RcServo::init() {
+        char* setting_cal_min = (char*)malloc(20);
+        sprintf(setting_cal_min, "%c/RcServo/Cal/Min", report_get_axis_letter(_axis_index));  //
+        rc_servo_cal_min = new FloatSetting(EXTENDED, WG, NULL, setting_cal_min, 1.0, 0.5, 2.0);
+
+        char* setting_cal_max = (char*)malloc(20);
+        sprintf(setting_cal_max, "%c/RcServo/Cal/Max", report_get_axis_letter(_axis_index));  //
+        rc_servo_cal_max = new FloatSetting(EXTENDED, WG, NULL, setting_cal_max, 1.0, 0.5, 2.0);
+
         read_settings();
         _channel_num = sys_get_next_PWM_chan_num();
         ledcSetup(_channel_num, SERVO_PULSE_FREQ, SERVO_PULSE_RES_BITS);
         ledcAttachPin(_pwm_pin, _channel_num);
         _current_pwm_duty = 0;
-        is_active         = true;   // as opposed to NullMotors, this is a real motor
-        _can_home         = false;  // this axis cannot be confensionally homed
 
-        set_axis_name();
         config_message();
+        startUpdateTask();
     }
 
     void RcServo::config_message() {
         grbl_msg_sendf(CLIENT_SERIAL,
                        MsgLevel::Info,
-                       "%s Axis RC Servo Pin:%d Pulse Len(%.0f,%.0f) Limits(%.3f,%.3f)",
-                       _axis_name,
+                       "%s RC Servo Pin:%d Pulse Len(%.0f,%.0f) %s",
+                       reportAxisNameMsg(_axis_index, _dual_axis_index),
                        _pwm_pin,
                        _pwm_pulse_min,
                        _pwm_pulse_max,
-                       _position_min,
-                       _position_max);
+                       reportAxisLimitsMsg(_axis_index));
     }
 
     void RcServo::_write_pwm(uint32_t duty) {
-        // to prevent excessive calls to ledcWrite, make sure duty hass changed
+        // to prevent excessive calls to ledcWrite, make sure duty has changed
         if (duty == _current_pwm_duty) {
             return;
         }
@@ -80,7 +75,10 @@ namespace Motors {
 
     // sets the PWM to zero. This allows most servos to be manually moved
     void RcServo::set_disable(bool disable) {
-        return;
+        if (_disabled == disable) {
+            return;
+        }
+
         _disabled = disable;
         if (_disabled) {
             _write_pwm(0);
@@ -88,15 +86,15 @@ namespace Motors {
     }
 
     // Homing justs sets the new system position and the servo will move there
-    void RcServo::set_homing_mode(uint8_t homing_mask, bool isHoming) {
+    bool RcServo::set_homing_mode(bool isHoming) {
         float home_pos = 0.0;
 
         sys_position[_axis_index] =
             axis_settings[_axis_index]->home_mpos->get() * axis_settings[_axis_index]->steps_per_mm->get();  // convert to steps
 
-        set_location();  // force the PWM to update now
-
+        set_location();   // force the PWM to update now
         vTaskDelay(750);  // give time to move
+        return false;     // Cannot be homed in the conventional way
     }
 
     void RcServo::update() { set_location(); }
@@ -105,42 +103,33 @@ namespace Motors {
         uint32_t servo_pulse_len;
         float    servo_pos, mpos, offset;
 
-        read_settings();
+        if (_disabled)
+            return;
 
         if (sys.state == State::Alarm) {
             set_disable(true);
             return;
         }
 
+        read_settings();
+
         mpos = system_convert_axis_steps_to_mpos(sys_position, _axis_index);  // get the axis machine position in mm
         // TBD working in MPos
         offset    = 0;  // gc_state.coord_system[axis_index] + gc_state.coord_offset[axis_index];  // get the current axis work offset
         servo_pos = mpos - offset;  // determine the current work position
 
         // determine the pulse length
-        servo_pulse_len = (uint32_t)mapConstrain(servo_pos, _position_min, _position_max, _pwm_pulse_min, _pwm_pulse_max);
+        servo_pulse_len = (uint32_t)mapConstrain(
+            servo_pos, limitsMinPosition(_axis_index), limitsMaxPosition(_axis_index), _pwm_pulse_min, _pwm_pulse_max);
 
         _write_pwm(servo_pulse_len);
     }
 
     void RcServo::read_settings() {
-        float travel = axis_settings[_axis_index]->max_travel->get();
-        float mpos   = axis_settings[_axis_index]->home_mpos->get();
-        //float max_mpos, min_mpos;
+        _pwm_pulse_min = SERVO_MIN_PULSE * rc_servo_cal_min->get();
+        _pwm_pulse_max = SERVO_MAX_PULSE * rc_servo_cal_max->get();
 
-        if (bit_istrue(homing_dir_mask->get(), bit(_axis_index))) {
-            _position_min = mpos;
-            _position_max = mpos + travel;
-        } else {
-            _position_min = mpos - travel;
-            _position_max = mpos;
-        }
-
-        _pwm_pulse_min = SERVO_MIN_PULSE * _cal_min;
-        _pwm_pulse_max = SERVO_MAX_PULSE * _cal_max;
-
-        if (bit_istrue(dir_invert_mask->get(), bit(_axis_index)))  // normal direction
+        if (bitnum_istrue(dir_invert_mask->get(), _axis_index))  // normal direction
             swap(_pwm_pulse_min, _pwm_pulse_max);
     }
-
 }
diff --git a/Grbl_Esp32/src/Motors/RcServo.h b/Grbl_Esp32/src/Motors/RcServo.h
index 818b6167..77540891 100644
--- a/Grbl_Esp32/src/Motors/RcServo.h
+++ b/Grbl_Esp32/src/Motors/RcServo.h
@@ -6,7 +6,7 @@
     Part of Grbl_ESP32
 
     2020 -	Bart Dring
-    
+
     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
@@ -21,35 +21,41 @@
 
 #include "Motor.h"
 
+#include "Servo.h"
 #include "RcServoSettings.h"
 
 namespace Motors {
-    class RcServo : public Motor {
+    class RcServo : public Servo {
     public:
-        RcServo();
-        RcServo(uint8_t axis_index, uint8_t pwm_pin, float cal_min, float cal_max);
-        virtual void config_message();
-        virtual void init();
-        void         _write_pwm(uint32_t duty);
-        virtual void set_disable(bool disable);
-        virtual void update();
-        void         read_settings();
-        void         set_homing_mode(uint8_t homing_mask, bool isHoming) override;
+        RcServo(uint8_t axis_index, uint8_t pwm_pin);
+
+        // Overrides for inherited methods
+        void init() override;
+        void read_settings() override;
+        bool set_homing_mode(bool isHoming) override;
+        void set_disable(bool disable) override;
+        void update() override;
+
+        void _write_pwm(uint32_t duty);
+        
 
     protected:
+        void config_message() override;
+
         void set_location();
 
         uint8_t  _pwm_pin;
         uint8_t  _channel_num;
         uint32_t _current_pwm_duty;
-        
 
         float _homing_position;
 
         float _pwm_pulse_min;
         float _pwm_pulse_max;
 
-        float _cal_min = 1.0;
-        float _cal_max = 1.0;
-    };
+        bool _disabled;
+
+        FloatSetting* rc_servo_cal_min;
+        FloatSetting* rc_servo_cal_max;
+        };
 }
diff --git a/Grbl_Esp32/src/Motors/RcServoSettings.h b/Grbl_Esp32/src/Motors/RcServoSettings.h
index 3502f746..a11c20bd 100644
--- a/Grbl_Esp32/src/Motors/RcServoSettings.h
+++ b/Grbl_Esp32/src/Motors/RcServoSettings.h
@@ -20,9 +20,6 @@ const uint16_t SERVO_MAX_PULSE = (uint16_t)(SERVO_MAX_PULSE_SEC / SERVO_TIME_PER
 
 const uint16_t SERVO_PULSE_RANGE = (SERVO_MAX_PULSE - SERVO_MIN_PULSE);
 
-const double SERVO_CAL_MIN = 20.0;   // Percent: the minimum allowable calibration value
-const double SERVO_CAL_MAX = 180.0;  // Percent: the maximum allowable calibration value
-
 const double SERVO_TIMER_INT_FREQ = 50.0;  // Hz This is the task frequency
 
 const int SERVO_FULL_MOVE_TIME = 750;  // rtos ticks
diff --git a/Grbl_Esp32/src/Motors/Servo.cpp b/Grbl_Esp32/src/Motors/Servo.cpp
new file mode 100644
index 00000000..65e213a1
--- /dev/null
+++ b/Grbl_Esp32/src/Motors/Servo.cpp
@@ -0,0 +1,74 @@
+/*
+    Servo.cpp
+
+    This is a base class for servo-type motors - ones that autonomously
+    move to a specified position, instead of being moved incrementally
+    by stepping.  Specific kinds of servo motors inherit from it.
+
+    Part of Grbl_ESP32
+
+    2020 -	Bart Dring
+
+    The servo's travel will be mapped against the axis with $X/MaxTravel
+
+    The rotation can be inverted with by $Stepper/DirInvert
+
+    Homing simply sets the axis Mpos to the endpoint as determined by $Homing/DirInvert
+
+    Calibration is part of the setting (TBD) fixed at 1.00 now
+
+    Grbl_ESP32 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 "Servo.h"
+
+namespace Motors {
+    Servo* Servo::List = NULL;
+
+    Servo::Servo(uint8_t axis_index) : Motor(axis_index) {
+        link = List;
+        List = this;
+    }
+
+    void Servo::startUpdateTask() {
+        if (this == List) {
+            xTaskCreatePinnedToCore(updateTask,         // task
+                                    "servoUpdateTask",  // name for task
+                                    4096,               // size of task stack
+                                    NULL,               // parameters
+                                    1,                  // priority
+                                    NULL,               // handle
+                                    0                   // core
+            );
+        }
+    }
+
+    void Servo::updateTask(void* pvParameters) {
+        TickType_t       xLastWakeTime;
+        const TickType_t xUpdate = SERVO_TIMER_INTERVAL;  // in ticks (typically ms)
+        auto             n_axis  = number_axis->get();
+
+        xLastWakeTime = xTaskGetTickCount();  // Initialise the xLastWakeTime variable with the current time.
+        vTaskDelay(2000);                     // initial delay
+        while (true) {                        // don't ever return from this or the task dies
+            for (Servo* p = List; p; p = p->link) {
+                p->update();
+            }
+
+            vTaskDelayUntil(&xLastWakeTime, xUpdate);
+
+            static UBaseType_t uxHighWaterMark = 0;
+            reportTaskStackSize(uxHighWaterMark);
+        }
+    }
+
+}
diff --git a/Grbl_Esp32/src/Motors/Servo.h b/Grbl_Esp32/src/Motors/Servo.h
new file mode 100644
index 00000000..09b6e35c
--- /dev/null
+++ b/Grbl_Esp32/src/Motors/Servo.h
@@ -0,0 +1,54 @@
+#pragma once
+
+/*
+    Servo.h
+
+    This is a base class for servo-type motors - ones that autonomously
+    move to a specified position, instead of being moved incrementally
+    by stepping.  Specific kinds of servo motors inherit from it.
+
+    Part of Grbl_ESP32
+
+    2020 -	Bart Dring
+
+    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 "Motor.h"
+
+namespace Motors {
+    class Servo : public Motor {
+    public:
+        Servo(uint8_t axis_index);
+#if 0
+        // Overrides for inherited methods
+        void init() override;
+        void read_settings() override;
+        bool set_homing_mode(bool isHoming) override;
+        void set_disable(bool disable) override;
+#endif
+        virtual void update() = 0;  // This must be implemented by derived classes
+
+    protected:
+        // Start the servo update task.  Each derived subclass instance calls this
+        // during init(), which happens after all objects have been constructed.
+        // startUpdateTask() ignores all such calls except for the last one, where
+        // it starts the task.
+        void startUpdateTask();
+
+    private:
+        // Linked list of servo instances, used by the servo task
+        static Servo* List;
+        Servo*        link;
+        static void   updateTask(void*);
+    };
+}
diff --git a/Grbl_Esp32/src/Motors/Solenoid.h b/Grbl_Esp32/src/Motors/Solenoid.h
index fb973083..60b62077 100644
--- a/Grbl_Esp32/src/Motors/Solenoid.h
+++ b/Grbl_Esp32/src/Motors/Solenoid.h
@@ -7,12 +7,13 @@ namespace Motors {
     public:
         Solenoid();
         Solenoid(uint8_t axis_index, gpio_num_t pwm_pin, float transition_poiont);
-        void config_message();
         void set_location();
-        void update();
-        void init();
-        void set_disable(bool disable);
+        void update() override;
+        void init() override;
+        void set_disable(bool disable) override;
 
         float _transition_poiont;
+  protected:
+        void config_message() override;
     };
 }
diff --git a/Grbl_Esp32/src/Motors/StandardStepper.cpp b/Grbl_Esp32/src/Motors/StandardStepper.cpp
index 82b62c1c..d2a2e51a 100644
--- a/Grbl_Esp32/src/Motors/StandardStepper.cpp
+++ b/Grbl_Esp32/src/Motors/StandardStepper.cpp
@@ -24,31 +24,34 @@
 #include "StandardStepper.h"
 
 namespace Motors {
-    StandardStepper::StandardStepper() {}
+    rmt_item32_t StandardStepper::rmtItem[2];
+    rmt_config_t StandardStepper::rmtConfig;
 
-    StandardStepper::StandardStepper(uint8_t axis_index, uint8_t step_pin, uint8_t dir_pin, uint8_t disable_pin) {
-        type_id                = STANDARD_MOTOR;
-        this->_axis_index      = axis_index % MAX_AXES;
-        this->_dual_axis_index = axis_index < MAX_AXES ? 0 : 1;  // 0 = primary 1 = ganged
-        this->step_pin         = step_pin;
-        this->dir_pin          = dir_pin;
-        this->disable_pin      = disable_pin;
-        init();
+    // Get an RMT channel number
+    // returns RMT_CHANNEL_MAX for error
+    rmt_channel_t StandardStepper::get_next_RMT_chan_num() {
+        static uint8_t next_RMT_chan_num = uint8_t(RMT_CHANNEL_0);  // channels 0-7 are valid
+        if (next_RMT_chan_num < RMT_CHANNEL_MAX) {
+            next_RMT_chan_num++;
+        } else {
+            grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Error, "Error: out of RMT channels");
+        }
+        return rmt_channel_t(next_RMT_chan_num);
+    }
+
+    StandardStepper::StandardStepper(uint8_t axis_index, uint8_t step_pin, uint8_t dir_pin, uint8_t disable_pin) :
+        Motor(axis_index), _step_pin(step_pin), _dir_pin(dir_pin), _disable_pin(disable_pin) {
     }
 
     void StandardStepper::init() {
-        _homing_mask = 0;
-        is_active    = true;  // as opposed to NullMotors, this is a real motor
-        set_axis_name();
         init_step_dir_pins();
-        read_settings();
         config_message();
     }
 
     void StandardStepper::init_step_dir_pins() {
-        // TODO Step pin, but RMT complicates things
-        _invert_step_pin = bit_istrue(step_invert_mask->get(), bit(_axis_index));
-        pinMode(dir_pin, OUTPUT);
+        _invert_step_pin = bitnum_istrue(step_invert_mask->get(), _axis_index);
+        _invert_dir_pin  = bitnum_istrue(dir_invert_mask->get(), _axis_index);
+        pinMode(_dir_pin, OUTPUT);
 
 #ifdef USE_RMT_STEPS
         rmtConfig.rmt_mode                       = RMT_MODE_TX;
@@ -71,36 +74,53 @@ namespace Motors {
         rmtItem[1].duration0 = 0;
         rmtItem[1].duration1 = 0;
 
-        rmt_chan_num[_axis_index][_dual_axis_index] = sys_get_next_RMT_chan_num();
-        rmt_set_source_clk((rmt_channel_t)rmt_chan_num[_axis_index][_dual_axis_index], RMT_BASECLK_APB);
-        rmtConfig.channel              = (rmt_channel_t)rmt_chan_num[_axis_index][_dual_axis_index];
+        _rmt_chan_num = get_next_RMT_chan_num();
+        if (_rmt_chan_num == RMT_CHANNEL_MAX) {
+            return;
+        }
+        rmt_set_source_clk(_rmt_chan_num, RMT_BASECLK_APB);
+        rmtConfig.channel              = _rmt_chan_num;
         rmtConfig.tx_config.idle_level = _invert_step_pin ? RMT_IDLE_LEVEL_HIGH : RMT_IDLE_LEVEL_LOW;
-        rmtConfig.gpio_num             = gpio_num_t(step_pin);  // c is a wacky lang
+        rmtConfig.gpio_num             = gpio_num_t(_step_pin);
         rmtItem[0].level0              = rmtConfig.tx_config.idle_level;
         rmtItem[0].level1              = !rmtConfig.tx_config.idle_level;
         rmt_config(&rmtConfig);
         rmt_fill_tx_items(rmtConfig.channel, &rmtItem[0], rmtConfig.mem_block_num, 0);
 
 #else
-        pinMode(step_pin, OUTPUT);
+        pinMode(_step_pin, OUTPUT);
 
 #endif  // USE_RMT_STEPS
-        pinMode(disable_pin, OUTPUT);
+        pinMode(_disable_pin, OUTPUT);
     }
 
     void StandardStepper::config_message() {
         grbl_msg_sendf(CLIENT_SERIAL,
                        MsgLevel::Info,
-                       "%s Axis Standard Stepper Step:%s Dir:%s Disable:%s Limits(%0.3f,%0.3f)",
-                       _axis_name,
-                       pinName(step_pin).c_str(),
-                       pinName(dir_pin).c_str(),
-                       pinName(disable_pin).c_str(),
-                       _position_min,
-                       _position_max);
+                       "%s Standard Stepper Step:%s Dir:%s Disable:%s %s",
+                       reportAxisNameMsg(_axis_index, _dual_axis_index),
+                       pinName(_step_pin).c_str(),
+                       pinName(_dir_pin).c_str(),
+                       pinName(_disable_pin).c_str(),
+                       reportAxisLimitsMsg(_axis_index));
     }
 
-    void StandardStepper::set_direction_pins(uint8_t onMask) { digitalWrite(dir_pin, (onMask & bit(_axis_index))); }
+    void StandardStepper::step() {
+#ifdef USE_RMT_STEPS
+        RMT.conf_ch[_rmt_chan_num].conf1.mem_rd_rst = 1;
+        RMT.conf_ch[_rmt_chan_num].conf1.tx_start   = 1;
+#else
+        digitalWrite(_step_pin, !_invert_step_pin);
+#endif  // USE_RMT_STEPS
+    }
 
-    void StandardStepper::set_disable(bool disable) { digitalWrite(disable_pin, disable); }
+    void StandardStepper::unstep() {
+#ifndef USE_RMT_STEPS
+        digitalWrite(_step_pin, _invert_step_pin);
+#endif  // USE_RMT_STEPS
+    }
+
+    void StandardStepper::set_direction(bool dir) { digitalWrite(_dir_pin, dir ^ _invert_dir_pin); }
+
+    void StandardStepper::set_disable(bool disable) { digitalWrite(_disable_pin, disable); }
 }
diff --git a/Grbl_Esp32/src/Motors/StandardStepper.h b/Grbl_Esp32/src/Motors/StandardStepper.h
index 8a4d7e8c..efc72f66 100644
--- a/Grbl_Esp32/src/Motors/StandardStepper.h
+++ b/Grbl_Esp32/src/Motors/StandardStepper.h
@@ -5,19 +5,34 @@
 namespace Motors {
     class StandardStepper : public Motor {
     public:
-        StandardStepper();
         StandardStepper(uint8_t axis_index, uint8_t step_pin, uint8_t dir_pin, uint8_t disable_pin);
 
-        virtual void config_message();
-        virtual void init();
-        virtual void set_direction_pins(uint8_t onMask);
-        void         init_step_dir_pins();
-        virtual void set_disable(bool disable);
-        uint8_t      step_pin;
+        // Overrides for inherited methods
+        void init() override;
+        // No special action, but return true to say homing is possible
+        bool set_homing_mode(bool isHoming) override { return true; }
+        void set_disable(bool) override;
+        void set_direction(bool) override;
+        void step() override;
+        void unstep() override;
+
+        void init_step_dir_pins();
 
     protected:
+        void config_message() override;
+
+#ifdef USE_RMT_STEPS
+        rmt_channel_t _rmt_chan_num;
+#endif
         bool    _invert_step_pin;
-        uint8_t dir_pin;
-        uint8_t disable_pin;
+        bool    _invert_dir_pin;
+        uint8_t _step_pin;
+        uint8_t _dir_pin;
+        uint8_t _disable_pin;
+
+    private:
+        static rmt_channel_t get_next_RMT_chan_num();
+        static rmt_item32_t  rmtItem[2];
+        static rmt_config_t  rmtConfig;
     };
 }
diff --git a/Grbl_Esp32/src/Motors/TrinamicDriver.cpp b/Grbl_Esp32/src/Motors/TrinamicDriver.cpp
index f785862c..d2c6e595 100644
--- a/Grbl_Esp32/src/Motors/TrinamicDriver.cpp
+++ b/Grbl_Esp32/src/Motors/TrinamicDriver.cpp
@@ -21,7 +21,26 @@
 
 #include <TMCStepper.h>
 
+#ifdef USE_I2S_OUT
+
+// Override default function and insert a short delay
+void TMC2130Stepper::switchCSpin(bool state) {
+    digitalWrite(_pinCS, state);
+    i2s_out_delay();
+}
+#endif
+
 namespace Motors {
+    uint8_t TrinamicDriver::get_next_index() {
+#ifdef TRINAMIC_DAISY_CHAIN
+        static uint8_t index = 1;  // they start at 1
+        return index++;
+#else
+        return -1;
+#endif
+    }
+    TrinamicDriver* TrinamicDriver::List = NULL;
+
     TrinamicDriver::TrinamicDriver(uint8_t  axis_index,
                                    uint8_t  step_pin,
                                    uint8_t  dir_pin,
@@ -29,62 +48,76 @@ namespace Motors {
                                    uint8_t  cs_pin,
                                    uint16_t driver_part_number,
                                    float    r_sense,
-                                   int8_t   spi_index) {
-        type_id                = TRINAMIC_SPI_MOTOR;
-        this->_axis_index      = axis_index % MAX_AXES;
-        this->_dual_axis_index = axis_index < 6 ? 0 : 1;  // 0 = primary 1 = ganged
-        _driver_part_number    = driver_part_number;
-        _r_sense               = r_sense;
-        this->step_pin         = step_pin;
-        this->dir_pin          = dir_pin;
-        this->disable_pin      = disable_pin;
-        this->cs_pin           = cs_pin;
-        this->spi_index        = spi_index;
-
-        _homing_mode = TRINAMIC_HOMING_MODE;
-        _homing_mask = 0;  // no axes homing
-
-        set_axis_name();
-
+                                   int8_t   spi_index) :
+        StandardStepper(axis_index, step_pin, dir_pin, disable_pin),
+        _homing_mode(TRINAMIC_HOMING_MODE), _cs_pin(cs_pin), _driver_part_number(driver_part_number), _r_sense(r_sense),
+        _spi_index(spi_index) {
+        _has_errors = false;
         if (_driver_part_number == 2130) {
-            tmcstepper = new TMC2130Stepper(cs_pin, _r_sense, spi_index);
+            tmcstepper = new TMC2130Stepper(_cs_pin, _r_sense, _spi_index);
         } else if (_driver_part_number == 5160) {
-            tmcstepper = new TMC5160Stepper(cs_pin, _r_sense, spi_index);
+            tmcstepper = new TMC5160Stepper(_cs_pin, _r_sense, _spi_index);
         } else {
-            grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "%s Axis Unsupported Trinamic part number TMC%d", _axis_name, _driver_part_number);
-            has_errors = true;  // as opposed to NullMotors, this is a real motor
+            grbl_msg_sendf(CLIENT_SERIAL,
+                           MsgLevel::Info,
+                           "%s Unsupported Trinamic part number TMC%d",
+                           reportAxisNameMsg(_axis_index, _dual_axis_index),
+                           _driver_part_number);
+            _has_errors = true;  // This motor cannot be used
             return;
         }
 
+        _has_errors = false;
         init_step_dir_pins();  // from StandardStepper
 
-        digitalWrite(cs_pin, HIGH);
-        pinMode(cs_pin, OUTPUT);
+        digitalWrite(_cs_pin, HIGH);
+        pinMode(_cs_pin, OUTPUT);
 
         // use slower speed if I2S
-        if (cs_pin >= I2S_OUT_PIN_BASE) {
+        if (_cs_pin >= I2S_OUT_PIN_BASE) {
             tmcstepper->setSPISpeed(TRINAMIC_SPI_FREQ);
         }
 
-        config_message();
+        link = List;
+        List = this;
 
         // init() must be called later, after all TMC drivers have CS pins setup.
     }
 
     void TrinamicDriver::init() {
-        if (has_errors) {
+        if (_has_errors) {
             return;
         }
 
+        // Display the stepper library version message once, before the first
+        // TMC config message.  Link is NULL for the first TMC instance.
+        if (!link) {
+            grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "TMCStepper Library Ver. 0x%06x", TMCSTEPPER_VERSION);
+        }
+
+        config_message();
+
         SPI.begin();  // this will get called for each motor, but does not seem to hurt anything
 
         tmcstepper->begin();
-        test();           // Try communicating with motor. Prints an error if there is a problem.
+
+        _has_errors = !test();  // Try communicating with motor. Prints an error if there is a problem.
+
         read_settings();  // pull info from settings
         set_mode(false);
 
-        _homing_mask = 0;
-        is_active    = true;
+        // After initializing all of the TMC drivers, create a task to
+        // display StallGuard data.  List == this for the final instance.
+        if (List == this) {
+            xTaskCreatePinnedToCore(readSgTask,    // task
+                                    "readSgTask",  // name for task
+                                    4096,          // size of task stack
+                                    NULL,          // parameters
+                                    1,             // priority
+                                    NULL,
+                                    0  // core
+            );
+        }
     }
 
     /*
@@ -93,28 +126,33 @@ namespace Motors {
     void TrinamicDriver::config_message() {
         grbl_msg_sendf(CLIENT_SERIAL,
                        MsgLevel::Info,
-                       "%s Axis Trinamic TMC%d Step:%s Dir:%s CS:%s Disable:%s Index:%d Limits(%0.3f,%0.3f)",
-                       _axis_name,
+                       "%s Trinamic TMC%d Step:%s Dir:%s CS:%s Disable:%s Index:%d %s",
+                       reportAxisNameMsg(_axis_index, _dual_axis_index),
                        _driver_part_number,
-                       pinName(step_pin).c_str(),
-                       pinName(dir_pin).c_str(),
-                       pinName(cs_pin).c_str(),
-                       pinName(disable_pin).c_str(),
-                       spi_index,
-                       _position_min,
-                       _position_max);
+                       pinName(_step_pin).c_str(),
+                       pinName(_dir_pin).c_str(),
+                       pinName(_cs_pin).c_str(),
+                       pinName(_disable_pin).c_str(),
+                       _spi_index,
+                       reportAxisLimitsMsg(_axis_index));
     }
 
     bool TrinamicDriver::test() {
-        if (has_errors) {
+        if (_has_errors) {
             return false;
         }
         switch (tmcstepper->test_connection()) {
             case 1:
-                grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "%s Trinamic driver test failed. Check connection", _axis_name);
+                grbl_msg_sendf(CLIENT_SERIAL,
+                               MsgLevel::Info,
+                               "%s Trinamic driver test failed. Check connection",
+                               reportAxisNameMsg(_axis_index, _dual_axis_index));
                 return false;
             case 2:
-                grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "%s Trinamic driver test failed. Check motor power", _axis_name);
+                grbl_msg_sendf(CLIENT_SERIAL,
+                               MsgLevel::Info,
+                               "%s Trinamic driver test failed. Check motor power",
+                               reportAxisNameMsg(_axis_index, _dual_axis_index));
                 return false;
             default:
                 // driver responded, so check for other errors from the DRV_STATUS register
@@ -123,12 +161,12 @@ namespace Motors {
                 status.sr = tmcstepper->DRV_STATUS();
 
                 bool err = false;
-                // look for open loan or short 2 ground on a and b
+                // look for open or short to ground on a and b
                 if (status.s2ga || status.s2gb) {
                     grbl_msg_sendf(CLIENT_SERIAL,
                                    MsgLevel::Info,
                                    "%s Motor Short Coil a:%s b:%s",
-                                   _axis_name,
+                                   reportAxisNameMsg(_axis_index, _dual_axis_index),
                                    status.s2ga ? "Y" : "N",
                                    status.s2gb ? "Y" : "N");
                     err = true;
@@ -138,7 +176,7 @@ namespace Motors {
                     grbl_msg_sendf(CLIENT_SERIAL,
                                    MsgLevel::Info,
                                    "%s Driver Temp Warning:%s Fault:%s",
-                                   _axis_name,
+                                   reportAxisNameMsg(_axis_index, _dual_axis_index),
                                    status.otpw ? "Y" : "N",
                                    status.ot ? "Y" : "N");
                     err = true;
@@ -148,7 +186,8 @@ namespace Motors {
                     return false;
                 }
 
-                grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "%s Trinamic driver test passed", _axis_name);
+                grbl_msg_sendf(
+                    CLIENT_SERIAL, MsgLevel::Info, "%s Trinamic driver test passed", reportAxisNameMsg(_axis_index, _dual_axis_index));
                 return true;
         }
     }
@@ -159,9 +198,11 @@ namespace Motors {
     uint16_t run (mA)
     float hold (as a percentage of run)
 */
+
     void TrinamicDriver::read_settings() {
-        if (has_errors)
+        if (_has_errors) {
             return;
+        }
 
         uint16_t run_i_ma = (uint16_t)(axis_settings[_axis_index]->run_current->get() * 1000.0);
         float    hold_i_percent;
@@ -173,15 +214,15 @@ namespace Motors {
             if (hold_i_percent > 1.0)
                 hold_i_percent = 1.0;
         }
-        //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "%s Current run %d hold %f", _axis_name, run_i_ma, hold_i_percent);
+        //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "%s Current run %d hold %f", reportAxisNameMsg(_axis_index, _dual_axis_index), run_i_ma, hold_i_percent);
 
         tmcstepper->microsteps(axis_settings[_axis_index]->microsteps->get());
         tmcstepper->rms_current(run_i_ma, hold_i_percent);
     }
 
-    void TrinamicDriver::set_homing_mode(uint8_t homing_mask, bool isHoming) {
-        _homing_mask = homing_mask;
+    bool TrinamicDriver::set_homing_mode(bool isHoming) {
         set_mode(isHoming);
+        return true;
     }
 
     /*
@@ -190,19 +231,16 @@ namespace Motors {
     Coolstep mode, so it will need to switch to Coolstep when homing
 */
     void TrinamicDriver::set_mode(bool isHoming) {
-        if (has_errors) {
+        if (_has_errors) {
             return;
         }
-        if (isHoming) {
-            _mode = TRINAMIC_HOMING_MODE;
-        } else {
-            _mode = TRINAMIC_RUN_MODE;
-        }
 
-        if (_lastMode == _mode) {
+        TrinamicMode newMode = isHoming ? TRINAMIC_HOMING_MODE : TRINAMIC_RUN_MODE;
+
+        if (newMode == _mode) {
             return;
         }
-        _lastMode = _mode;
+        _mode = newMode;
 
         switch (_mode) {
             case TrinamicMode ::StealthChop:
@@ -211,7 +249,7 @@ namespace Motors {
                 tmcstepper->pwm_autoscale(true);
                 tmcstepper->diag1_stall(false);
                 break;
-            case TrinamicMode :: CoolStep:
+            case TrinamicMode ::CoolStep:
                 //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Coolstep");
                 tmcstepper->en_pwm_mode(false);
                 tmcstepper->pwm_autoscale(false);
@@ -237,7 +275,7 @@ namespace Motors {
     This is the stallguard tuning info. It is call debug, so it could be generic across all classes.
 */
     void TrinamicDriver::debug_message() {
-        if (has_errors) {
+        if (_has_errors) {
             return;
         }
         uint32_t tstep = tmcstepper->TSTEP();
@@ -250,7 +288,7 @@ namespace Motors {
         grbl_msg_sendf(CLIENT_SERIAL,
                        MsgLevel::Info,
                        "%s Stallguard %d   SG_Val: %04d   Rate: %05.0f mm/min SG_Setting:%d",
-                       _axis_name,
+                       reportAxisNameMsg(_axis_index, _dual_axis_index),
                        tmcstepper->stallguard(),
                        tmcstepper->sg_result(),
                        feedrate,
@@ -272,15 +310,17 @@ namespace Motors {
     // this can use the enable feature over SPI. The dedicated pin must be in the enable mode,
     // but that can be hardwired that way.
     void TrinamicDriver::set_disable(bool disable) {
-        if (has_errors)
+        if (_has_errors) {
             return;
+        }
 
-        if (_disabled == disable)
+        if (_disabled == disable) {
             return;
+        }
 
         _disabled = disable;
 
-        digitalWrite(disable_pin, _disabled);
+        digitalWrite(_disable_pin, _disabled);
 
 #ifdef USE_TRINAMIC_ENABLE
         if (_disabled) {
@@ -296,4 +336,34 @@ namespace Motors {
         // the pin based enable could be added here.
         // This would be for individual motors, not the single pin for all motors.
     }
+
+    // Prints StallGuard data that is useful for tuning.
+    void TrinamicDriver::readSgTask(void* pvParameters) {
+        TickType_t       xLastWakeTime;
+        const TickType_t xreadSg = 200;  // in ticks (typically ms)
+        auto             n_axis  = number_axis->get();
+
+        xLastWakeTime = xTaskGetTickCount();  // Initialise the xLastWakeTime variable with the current time.
+        while (true) {                        // don't ever return from this or the task dies
+            if (motorSettingChanged) {
+                motors_read_settings();
+                motorSettingChanged = false;
+            }
+            if (stallguard_debug_mask->get() != 0) {
+                if (sys.state == State::Cycle || sys.state == State::Homing || sys.state == State::Jog) {
+                    for (TrinamicDriver* p = List; p; p = p->link) {
+                        if (bitnum_istrue(stallguard_debug_mask->get(), p->_axis_index)) {
+                            //grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "SG:%d", stallguard_debug_mask->get());
+                            p->debug_message();
+                        }
+                    }
+                }  // sys.state
+            }      // if mask
+
+            vTaskDelayUntil(&xLastWakeTime, xreadSg);
+
+            static UBaseType_t uxHighWaterMark = 0;
+            reportTaskStackSize(uxHighWaterMark);
+        }
+    }
 }
diff --git a/Grbl_Esp32/src/Motors/TrinamicDriver.h b/Grbl_Esp32/src/Motors/TrinamicDriver.h
index d7e7b681..004aa0dd 100644
--- a/Grbl_Esp32/src/Motors/TrinamicDriver.h
+++ b/Grbl_Esp32/src/Motors/TrinamicDriver.h
@@ -63,14 +63,25 @@ const double TRINAMIC_FCLK = 12700000.0;  // Internal clock Approx (Hz) used to
 namespace Motors {
 
     enum class TrinamicMode : uint8_t {
-        None            = 0, // not for machine defs!
-        StealthChop     = 1, 
-        CoolStep        = 2,
-        StallGuard      = 3,
+        None        = 0,  // not for machine defs!
+        StealthChop = 1,
+        CoolStep    = 2,
+        StallGuard  = 3,
     };
 
     class TrinamicDriver : public StandardStepper {
     public:
+        TrinamicDriver(uint8_t  axis_index,
+                       uint8_t  step_pin,
+                       uint8_t  dir_pin,
+                       uint8_t  disable_pin,
+                       uint8_t  cs_pin,
+                       uint16_t driver_part_number,
+                       float    r_sense) :
+            TrinamicDriver(axis_index, step_pin, dir_pin, disable_pin,
+                           cs_pin, driver_part_number, r_sense, get_next_index())
+        {}
+
         TrinamicDriver(uint8_t  axis_index,
                        uint8_t  step_pin,
                        uint8_t  dir_pin,
@@ -80,29 +91,41 @@ namespace Motors {
                        float    r_sense,
                        int8_t   spi_index);
 
-        void config_message();
-        void init();
-        void set_mode(bool isHoming);
-        void read_settings();
-        void trinamic_test_response();
-        void trinamic_stepper_enable(bool enable);
+        // Overrides for inherited methods
+        void init() override;
+        void read_settings() override;
+        bool set_homing_mode(bool ishoming) override;
+        void set_disable(bool disable) override;
+
         void debug_message();
-        void set_homing_mode(uint8_t homing_mask, bool ishoming);
-        void set_disable(bool disable);
-        bool test();
 
     private:
         uint32_t calc_tstep(float speed, float percent);
 
         TMC2130Stepper* tmcstepper;  // all other driver types are subclasses of this one
         TrinamicMode    _homing_mode;
-        uint8_t         cs_pin = UNDEFINED_PIN;  // The chip select pin (can be the same for daisy chain)
-        uint16_t        _driver_part_number;     // example: use 2130 for TMC2130
+        uint8_t         _cs_pin = UNDEFINED_PIN;  // The chip select pin (can be the same for daisy chain)
+        uint16_t        _driver_part_number;      // example: use 2130 for TMC2130
         float           _r_sense;
-        int8_t          spi_index;
+        int8_t          _spi_index;
+        bool            _has_errors;
+        bool            _disabled;
+
+        TrinamicMode _mode = TrinamicMode::None;
+        bool test();
+        void set_mode(bool isHoming);
+        void trinamic_test_response();
+        void trinamic_stepper_enable(bool enable);
+
+        uint8_t get_next_index();
+
+        // Linked list of Trinamic driver instances, used by the
+        // StallGuard reporting task.
+        static TrinamicDriver* List;
+        TrinamicDriver* link;
+        static void readSgTask(void*);
 
     protected:
-        TrinamicMode _mode;
-        TrinamicMode _lastMode = TrinamicMode::None;
+        void config_message() override;
     };
 }
diff --git a/Grbl_Esp32/src/Motors/UnipolarMotor.cpp b/Grbl_Esp32/src/Motors/UnipolarMotor.cpp
index 68e45c3c..d8c844a4 100644
--- a/Grbl_Esp32/src/Motors/UnipolarMotor.cpp
+++ b/Grbl_Esp32/src/Motors/UnipolarMotor.cpp
@@ -1,23 +1,12 @@
 #include "UnipolarMotor.h"
 
 namespace Motors {
-    UnipolarMotor::UnipolarMotor() {}
+    UnipolarMotor::UnipolarMotor(uint8_t axis_index, uint8_t pin_phase0, uint8_t pin_phase1, uint8_t pin_phase2, uint8_t pin_phase3) :
+        Motor(axis_index), _pin_phase0(pin_phase0), _pin_phase1(pin_phase1), _pin_phase2(pin_phase2),
+        _pin_phase3(pin_phase3),
 
-    UnipolarMotor::UnipolarMotor(uint8_t axis_index, uint8_t pin_phase0, uint8_t pin_phase1, uint8_t pin_phase2, uint8_t pin_phase3) {
-        type_id                = UNIPOLAR_MOTOR;
-        this->_axis_index      = axis_index % MAX_AXES;
-        this->_dual_axis_index = axis_index < MAX_AXES ? 0 : 1;  // 0 = primary 1 = ganged
-        _pin_phase0            = pin_phase0;
-        _pin_phase1            = pin_phase1;
-        _pin_phase2            = pin_phase2;
-        _pin_phase3            = pin_phase3;
-
-        _half_step = true;  // TODO read from settings ... microstep > 1 = half step
-
-        set_axis_name();
-        init();
-        config_message();
-    }
+        _half_step(true)  // TODO read from settings ... microstep > 1 = half step
+    {}
 
     void UnipolarMotor::init() {
         pinMode(_pin_phase0, OUTPUT);
@@ -25,19 +14,19 @@ namespace Motors {
         pinMode(_pin_phase2, OUTPUT);
         pinMode(_pin_phase3, OUTPUT);
         _current_phase = 0;
+        config_message();
     }
 
     void UnipolarMotor::config_message() {
         grbl_msg_sendf(CLIENT_SERIAL,
                        MsgLevel::Info,
-                       "%s Axis Unipolar Stepper Ph0:%s Ph1:%s Ph2:%s Ph3:%s Limits(%0.3f,%0.3f)",
-                       _axis_name,
+                       "%s Unipolar Stepper Ph0:%s Ph1:%s Ph2:%s Ph3:%s %s",
+                       reportAxisNameMsg(_axis_index, _dual_axis_index),
                        pinName(_pin_phase0).c_str(),
                        pinName(_pin_phase1).c_str(),
                        pinName(_pin_phase2).c_str(),
                        pinName(_pin_phase3).c_str(),
-                       _position_min,
-                       _position_max);
+                       reportAxisLimitsMsg(_axis_index));
     }
 
     void UnipolarMotor::set_disable(bool disable) {
@@ -50,31 +39,21 @@ namespace Motors {
         _enabled = !disable;
     }
 
-    void UnipolarMotor::step(uint8_t step_mask, uint8_t dir_mask) {
+    void UnipolarMotor::set_direction(bool dir) { _dir = dir; }
+
+    void UnipolarMotor::step() {
         uint8_t _phase[8] = { 0, 0, 0, 0, 0, 0, 0, 0 };  // temporary phase values...all start as off
         uint8_t phase_max;
 
-        if (!(step_mask & bit(_axis_index)))
-            return;  // a step is not required on this interrupt
-
         if (!_enabled)
             return;  // don't do anything, phase is not changed or lost
 
-        if (_half_step)
-            phase_max = 7;
-        else
-            phase_max = 3;
+        phase_max = _half_step ? 7 : 3;
 
-        if (dir_mask & bit(_axis_index)) {  // count up
-            if (_current_phase == phase_max)
-                _current_phase = 0;
-            else
-                _current_phase++;
+        if (_dir) {  // count up
+            _current_phase = _current_phase == phase_max ? 0 : _current_phase + 1;
         } else {  // count down
-            if (_current_phase == 0)
-                _current_phase = phase_max;
-            else
-                _current_phase--;
+            _current_phase = _current_phase == 0 ? phase_max : _current_phase - 1;
         }
         /*
 			8 Step : A – AB – B – BC – C – CD – D – DA
diff --git a/Grbl_Esp32/src/Motors/UnipolarMotor.h b/Grbl_Esp32/src/Motors/UnipolarMotor.h
index d9594815..ba554646 100644
--- a/Grbl_Esp32/src/Motors/UnipolarMotor.h
+++ b/Grbl_Esp32/src/Motors/UnipolarMotor.h
@@ -5,12 +5,14 @@
 namespace Motors {
     class UnipolarMotor : public Motor {
     public:
-        UnipolarMotor();
         UnipolarMotor(uint8_t axis_index, uint8_t pin_phase0, uint8_t pin_phase1, uint8_t pin_phase2, uint8_t pin_phase3);
-        void init();
-        void config_message();
-        void set_disable(bool disable);
-        void step(uint8_t step_mask, uint8_t dir_mask);  // only used on Unipolar right now
+
+        // Overrides for inherited methods
+        void init() override;
+        bool set_homing_mode(bool isHoming) override { return true; }
+        void set_disable(bool disable) override;
+        void set_direction(bool) override;
+        void step() override;
 
     private:
         uint8_t _pin_phase0;
@@ -20,5 +22,9 @@ namespace Motors {
         uint8_t _current_phase;
         bool    _half_step;
         bool    _enabled;
+        bool    _dir;
+
+  protected:
+        void config_message() override;
     };
 }
diff --git a/Grbl_Esp32/src/NutsBolts.cpp b/Grbl_Esp32/src/NutsBolts.cpp
index 1ded98fa..591ef5e3 100644
--- a/Grbl_Esp32/src/NutsBolts.cpp
+++ b/Grbl_Esp32/src/NutsBolts.cpp
@@ -123,7 +123,7 @@ void delay_sec(float seconds, uint8_t mode) {
         } else {  // DELAY_MODE_SYS_SUSPEND
             // Execute rt_system() only to avoid nesting suspend loops.
             protocol_exec_rt_system();
-            if (sys.suspend & SUSPEND_RESTART_RETRACT) {
+            if (sys.suspend.bit.restartRetract) {
                 return;  // Bail, if safety door reopens.
             }
         }
diff --git a/Grbl_Esp32/src/NutsBolts.h b/Grbl_Esp32/src/NutsBolts.h
index 5661c178..185be0a7 100644
--- a/Grbl_Esp32/src/NutsBolts.h
+++ b/Grbl_Esp32/src/NutsBolts.h
@@ -49,6 +49,9 @@ const int GANGED_MOTOR  = 1;
 #define A2_AXIS (A_AXIS + MAX_AXES)
 #define B2_AXIS (B_AXIS + MAX_AXES)
 #define C2_AXIS (C_AXIS + MAX_AXES)
+static inline int toMotor2(int axis) {
+    return axis + MAX_AXES;
+}
 
 // CoreXY motor assignments. DO NOT ALTER.
 // NOTE: If the A and B motor axis bindings are changed, this effects the CoreXY equations.
@@ -58,7 +61,6 @@ const int GANGED_MOTOR  = 1;
 // Conversions
 const double MM_PER_INCH = (25.40);
 const double INCH_PER_MM = (0.0393701);
-#define TICKS_PER_MICROSECOND (F_STEPPER_TIMER / 1000000)  // Different from AVR version
 
 const int DELAY_MODE_DWELL       = 0;
 const int DELAY_MODE_SYS_SUSPEND = 1;
@@ -81,6 +83,8 @@ const int DELAY_MODE_SYS_SUSPEND = 1;
 #define bit_false(x, mask) (x) &= ~(mask)
 #define bit_istrue(x, mask) ((x & mask) != 0)
 #define bit_isfalse(x, mask) ((x & mask) == 0)
+#define bitnum_true(x, num) (x) |= bit(num)
+#define bitnum_istrue(x, num) ((x & bit(num)) != 0)
 
 // Read a floating point value from a string. Line points to the input buffer, char_counter
 // is the indexer pointing to the current character of the line, while float_ptr is
diff --git a/Grbl_Esp32/src/Planner.cpp b/Grbl_Esp32/src/Planner.cpp
index cf3cc927..47920960 100644
--- a/Grbl_Esp32/src/Planner.cpp
+++ b/Grbl_Esp32/src/Planner.cpp
@@ -358,7 +358,7 @@ uint8_t plan_buffer_line(float* target, plan_line_data_t* pl_data) {
         unit_vec[idx] = delta_mm;  // Store unit vector numerator
         // Set direction bits. Bit enabled always means direction is negative.
         if (delta_mm < 0.0) {
-            block->direction_bits |= get_direction_pin_mask(idx);
+            block->direction_bits |= bit(idx);
         }
     }
     // Bail if this is a zero-length block. Highly unlikely to occur.
diff --git a/Grbl_Esp32/src/Probe.cpp b/Grbl_Esp32/src/Probe.cpp
index 058b936e..661c2d84 100644
--- a/Grbl_Esp32/src/Probe.cpp
+++ b/Grbl_Esp32/src/Probe.cpp
@@ -58,8 +58,8 @@ bool probe_get_state() {
 // NOTE: This function must be extremely efficient as to not bog down the stepper ISR.
 void probe_state_monitor() {
     if (probe_get_state() ^ is_probe_away) {
-        sys_probe_state = PROBE_OFF;
+        sys_probe_state = Probe::Off;
         memcpy(sys_probe_position, sys_position, sizeof(sys_position));
-        bit_true(sys_rt_exec_state, EXEC_MOTION_CANCEL);
+        sys_rt_exec_state.bit.motionCancel = true;
     }
 }
diff --git a/Grbl_Esp32/src/Probe.h b/Grbl_Esp32/src/Probe.h
index f1612c93..82e4a196 100644
--- a/Grbl_Esp32/src/Probe.h
+++ b/Grbl_Esp32/src/Probe.h
@@ -24,8 +24,10 @@
 */
 
 // Values that define the probing state machine.
-const int PROBE_OFF    = 0;  // Probing disabled or not in use. (Must be zero.)
-const int PROBE_ACTIVE = 1;  // Actively watching the input pin.
+enum class Probe : uint8_t {
+    Off    = 0,  // Probing disabled or not in use. (Must be zero.)
+    Active = 1,  // Actively watching the input pin.
+};
 
 // Probe pin initialization routine.
 void probe_init();
diff --git a/Grbl_Esp32/src/ProcessSettings.cpp b/Grbl_Esp32/src/ProcessSettings.cpp
index ddae1b22..f1a42673 100644
--- a/Grbl_Esp32/src/ProcessSettings.cpp
+++ b/Grbl_Esp32/src/ProcessSettings.cpp
@@ -33,7 +33,7 @@ void show_setting(const char* name, const char* value, const char* description,
 
 void settings_restore(uint8_t restore_flag) {
 #ifdef WIFI_OR_BLUETOOTH
-    if (restore_flag & SETTINGS_RESTORE_WIFI_SETTINGS) {
+    if (restore_flag & SettingsRestore::Wifi) {
 #    ifdef ENABLE_WIFI
         WebUI::wifi_config.reset_settings();
 #    endif
@@ -42,8 +42,8 @@ void settings_restore(uint8_t restore_flag) {
 #    endif
     }
 #endif
-    if (restore_flag & SETTINGS_RESTORE_DEFAULTS) {
-        bool restore_startup = restore_flag & SETTINGS_RESTORE_STARTUP_LINES;
+    if (restore_flag & SettingsRestore::Defaults) {
+        bool restore_startup = restore_flag & SettingsRestore::StartupLines;
         for (Setting* s = Setting::List; s; s = s->next()) {
             if (!s->getDescription()) {
                 const char* name = s->getName();
@@ -55,16 +55,11 @@ void settings_restore(uint8_t restore_flag) {
             }
         }
     }
-    if (restore_flag & SETTINGS_RESTORE_PARAMETERS) {
+    if (restore_flag & SettingsRestore::Parameters) {
         for (auto idx = CoordIndex::Begin; idx < CoordIndex::End; ++idx) {
             coords[idx]->setDefault();
         }
     }
-    if (restore_flag & SETTINGS_RESTORE_BUILD_INFO) {
-        EEPROM.write(EEPROM_ADDR_BUILD_INFO, 0);
-        EEPROM.write(EEPROM_ADDR_BUILD_INFO + 1, 0);  // Checksum
-        EEPROM.commit();
-    }
 }
 
 // Get settings values from non volatile storage into memory
@@ -159,6 +154,16 @@ Error list_settings(const char* value, WebUI::AuthenticationLevel auth_level, We
     }
     return Error::Ok;
 }
+Error list_changed_settings(const char* value, WebUI::AuthenticationLevel auth_level, WebUI::ESPResponseStream* out) {
+    for (Setting* s = Setting::List; s; s = s->next()) {
+        const char* value = s->getStringValue();
+        if (!auth_failed(s, value, auth_level) && strcmp(value, s->getDefaultString())) {
+            show_setting(s->getName(), value, NULL, out);
+        }
+    }
+    grbl_sendf(out->client(), "(Passwords not shown)\r\n");
+    return Error::Ok;
+}
 Error list_commands(const char* value, WebUI::AuthenticationLevel auth_level, WebUI::ESPResponseStream* out) {
     for (Command* cp = Command::List; cp; cp = cp->next()) {
         const char* name    = cp->getName();
@@ -262,22 +267,15 @@ Error home_c(const char* value, WebUI::AuthenticationLevel auth_level, WebUI::ES
     return home(bit(C_AXIS));
 }
 Error sleep_grbl(const char* value, WebUI::AuthenticationLevel auth_level, WebUI::ESPResponseStream* out) {
-    system_set_exec_state_flag(EXEC_SLEEP);
+    sys_rt_exec_state.bit.sleep = true;
     return Error::Ok;
 }
 Error get_report_build_info(const char* value, WebUI::AuthenticationLevel auth_level, WebUI::ESPResponseStream* out) {
     if (!value) {
-        char line[128];
-        settings_read_build_info(line);
-        report_build_info(line, out->client());
+        report_build_info(build_info->get(), out->client());
         return Error::Ok;
     }
-#ifdef ENABLE_BUILD_INFO_WRITE_COMMAND
-    settings_store_build_info(value);
-    return Error::Ok;
-#else
     return Error::InvalidStatement;
-#endif
 }
 Error report_startup_lines(const char* value, WebUI::AuthenticationLevel auth_level, WebUI::ESPResponseStream* out) {
     report_startup_line(0, startup_line_0->get(), out->client());
@@ -286,16 +284,16 @@ Error report_startup_lines(const char* value, WebUI::AuthenticationLevel auth_le
 }
 
 std::map<const char*, uint8_t, cmp_str> restoreCommands = {
-#ifdef ENABLE_RESTORE_EEPROM_DEFAULT_SETTINGS
-    { "$", SETTINGS_RESTORE_DEFAULTS },      { "settings", SETTINGS_RESTORE_DEFAULTS },
+#ifdef ENABLE_RESTORE_DEFAULT_SETTINGS
+    { "$", SettingsRestore::Defaults },      { "settings", SettingsRestore::Defaults },
 #endif
-#ifdef ENABLE_RESTORE_EEPROM_CLEAR_PARAMETERS
-    { "#", SETTINGS_RESTORE_PARAMETERS },    { "gcode", SETTINGS_RESTORE_PARAMETERS },
+#ifdef ENABLE_RESTORE_CLEAR_PARAMETERS
+    { "#", SettingsRestore::Parameters },    { "gcode", SettingsRestore::Parameters },
 #endif
-#ifdef ENABLE_RESTORE_EEPROM_WIPE_ALL
-    { "*", SETTINGS_RESTORE_ALL },           { "all", SETTINGS_RESTORE_ALL },
+#ifdef ENABLE_RESTORE_WIPE_ALL
+    { "*", SettingsRestore::All },           { "all", SettingsRestore::All },
 #endif
-    { "@", SETTINGS_RESTORE_WIFI_SETTINGS }, { "wifi", SETTINGS_RESTORE_WIFI_SETTINGS },
+    { "@", SettingsRestore::Wifi }, { "wifi", SettingsRestore::Wifi },
 };
 Error restore_settings(const char* value, WebUI::AuthenticationLevel auth_level, WebUI::ESPResponseStream* out) {
     if (!value) {
@@ -383,6 +381,7 @@ void make_grbl_commands() {
     new GrblCommand("+", "ExtendedSettings/List", report_extended_settings, notCycleOrHold);
     new GrblCommand("L", "GrblNames/List", list_grbl_names, notCycleOrHold);
     new GrblCommand("S", "Settings/List", list_settings, notCycleOrHold);
+    new GrblCommand("SC","Settings/ListChanged", list_changed_settings, notCycleOrHold);
     new GrblCommand("CMD", "Commands/List", list_commands, notCycleOrHold);
     new GrblCommand("E", "ErrorCodes/List", listErrorCodes, anyState);
     new GrblCommand("G", "GCode/Modes", report_gcode, anyState);
diff --git a/Grbl_Esp32/src/Protocol.cpp b/Grbl_Esp32/src/Protocol.cpp
index d2d3d0cb..982cecca 100644
--- a/Grbl_Esp32/src/Protocol.cpp
+++ b/Grbl_Esp32/src/Protocol.cpp
@@ -94,7 +94,7 @@ Error execute_line(char* line, uint8_t client, WebUI::AuthenticationLevel auth_l
 bool can_park() {
     return
 #ifdef ENABLE_PARKING_OVERRIDE_CONTROL
-        sys.override_ctrl == OVERRIDE_PARKING_MOTION &&
+        sys.override_ctrl == Override::ParkingMotion &&
 #endif
         homing_enable->get() && !laser_mode->get();
 }
@@ -125,7 +125,7 @@ void protocol_main_loop() {
         // Check if the safety door is open.
         sys.state = State::Idle;
         if (system_check_safety_door_ajar()) {
-            bit_true(sys_rt_exec_state, EXEC_SAFETY_DOOR);
+            sys_rt_exec_state.bit.safetyDoor = true;
             protocol_execute_realtime();  // Enter safety door mode. Should return as IDLE state.
         }
         // All systems go!
@@ -142,7 +142,7 @@ void protocol_main_loop() {
             char fileLine[255];
             if (readFileLine(fileLine, 255)) {
                 SD_ready_next = false;
-                report_status_message(gc_execute_line(fileLine, SD_client), SD_client);
+                report_status_message(execute_line(fileLine, SD_client, SD_auth_level), SD_client);
             } else {
                 char temp[50];
                 sd_get_current_filename(temp);
@@ -222,8 +222,8 @@ void protocol_buffer_synchronize() {
 // is finished, single commands), a command that needs to wait for the motions in the buffer to
 // execute calls a buffer sync, or the planner buffer is full and ready to go.
 void protocol_auto_cycle_start() {
-    if (plan_get_current_block() != NULL) {            // Check if there are any blocks in the buffer.
-        system_set_exec_state_flag(EXEC_CYCLE_START);  // If so, execute them!
+    if (plan_get_current_block() != NULL) {       // Check if there are any blocks in the buffer.
+        sys_rt_exec_state.bit.cycleStart = true;  // If so, execute them!
     }
 }
 
@@ -236,11 +236,11 @@ void protocol_auto_cycle_start() {
 // handles them, removing the need to define more computationally-expensive volatile variables. This
 // also provides a controlled way to execute certain tasks without having two or more instances of
 // the same task, such as the planner recalculating the buffer upon a feedhold or overrides.
-// NOTE: The sys_rt_exec_state variable flags are set by any process, step or serial interrupts, pinouts,
+// NOTE: The sys_rt_exec_state.bit variable flags are set by any process, step or serial interrupts, pinouts,
 // limit switches, or the main program.
 void protocol_execute_realtime() {
     protocol_exec_rt_system();
-    if (sys.suspend) {
+    if (sys.suspend.value) {
         protocol_exec_rt_suspend();
     }
 }
@@ -259,145 +259,156 @@ void protocol_exec_rt_system() {
         // Halt everything upon a critical event flag. Currently hard and soft limits flag this.
         if ((alarm == ExecAlarm::HardLimit) || (alarm == ExecAlarm::SoftLimit)) {
             report_feedback_message(Message::CriticalEvent);
-            system_clear_exec_state_flag(EXEC_RESET);  // Disable any existing reset
+            sys_rt_exec_state.bit.reset = false;  // Disable any existing reset
             do {
                 // Block everything, except reset and status reports, until user issues reset or power
                 // cycles. Hard limits typically occur while unattended or not paying attention. Gives
                 // the user and a GUI time to do what is needed before resetting, like killing the
                 // incoming stream. The same could be said about soft limits. While the position is not
                 // lost, continued streaming could cause a serious crash if by chance it gets executed.
-            } while (bit_isfalse(sys_rt_exec_state, EXEC_RESET));
+            } while (!sys_rt_exec_state.bit.reset);
         }
-        system_clear_exec_alarm();  // Clear alarm
+        sys_rt_exec_alarm = ExecAlarm::None;
     }
-    uint8_t rt_exec = sys_rt_exec_state;  // Copy volatile sys_rt_exec_state.
-    if (rt_exec || cycle_stop) {
+    ExecState rt_exec_state;
+    rt_exec_state.value = sys_rt_exec_state.value;  // Copy volatile sys_rt_exec_state.
+    if (rt_exec_state.value != 0 || cycle_stop) {                 // Test if any bits are on
         // Execute system abort.
-        if (rt_exec & EXEC_RESET) {
+        if (rt_exec_state.bit.reset) {
             sys.abort = true;  // Only place this is set true.
             return;            // Nothing else to do but exit.
         }
         // Execute and serial print status
-        if (rt_exec & EXEC_STATUS_REPORT) {
+        if (rt_exec_state.bit.statusReport) {
             report_realtime_status(CLIENT_ALL);
-            system_clear_exec_state_flag(EXEC_STATUS_REPORT);
+            sys_rt_exec_state.bit.statusReport = false;
         }
         // NOTE: Once hold is initiated, the system immediately enters a suspend state to block all
         // main program processes until either reset or resumed. This ensures a hold completes safely.
-        if (rt_exec & (EXEC_MOTION_CANCEL | EXEC_FEED_HOLD | EXEC_SAFETY_DOOR | EXEC_SLEEP)) {
+        if (rt_exec_state.bit.motionCancel || rt_exec_state.bit.feedHold || rt_exec_state.bit.safetyDoor || rt_exec_state.bit.sleep) {
             // State check for allowable states for hold methods.
             if (!(sys.state == State::Alarm || sys.state == State::CheckMode)) {
                 // If in CYCLE or JOG states, immediately initiate a motion HOLD.
                 if (sys.state == State::Cycle || sys.state == State::Jog) {
-                    if (!(sys.suspend & (SUSPEND_MOTION_CANCEL | SUSPEND_JOG_CANCEL))) {  // Block, if already holding.
-                        st_update_plan_block_parameters();             // Notify stepper module to recompute for hold deceleration.
-                        sys.step_control = STEP_CONTROL_EXECUTE_HOLD;  // Initiate suspend state with active flag.
-                        if (sys.state == State::Jog) {                 // Jog cancelled upon any hold event, except for sleeping.
-                            if (!(rt_exec & EXEC_SLEEP)) {
-                                sys.suspend |= SUSPEND_JOG_CANCEL;
+                    if (!(sys.suspend.bit.motionCancel || sys.suspend.bit.jogCancel)) {  // Block, if already holding.
+                        st_update_plan_block_parameters();  // Notify stepper module to recompute for hold deceleration.
+                        sys.step_control             = {};
+                        sys.step_control.executeHold = true;  // Initiate suspend state with active flag.
+                        if (sys.state == State::Jog) {        // Jog cancelled upon any hold event, except for sleeping.
+                            if (!rt_exec_state.bit.sleep) {
+                                sys.suspend.bit.jogCancel = true;
                             }
                         }
                     }
                 }
                 // If IDLE, Grbl is not in motion. Simply indicate suspend state and hold is complete.
                 if (sys.state == State::Idle) {
-                    sys.suspend = SUSPEND_HOLD_COMPLETE;
+                    sys.suspend.value            = 0;
+                    sys.suspend.bit.holdComplete = true;
                 }
                 // Execute and flag a motion cancel with deceleration and return to idle. Used primarily by probing cycle
                 // to halt and cancel the remainder of the motion.
-                if (rt_exec & EXEC_MOTION_CANCEL) {
+                if (rt_exec_state.bit.motionCancel) {
                     // MOTION_CANCEL only occurs during a CYCLE, but a HOLD and SAFETY_DOOR may been initiated beforehand
                     // to hold the CYCLE. Motion cancel is valid for a single planner block motion only, while jog cancel
                     // will handle and clear multiple planner block motions.
                     if (sys.state != State::Jog) {
-                        sys.suspend |= SUSPEND_MOTION_CANCEL;  // NOTE: State is State::Cycle.
+                        sys.suspend.bit.motionCancel = true;  // NOTE: State is State::Cycle.
                     }
+                    sys_rt_exec_state.bit.motionCancel = false;
                 }
                 // Execute a feed hold with deceleration, if required. Then, suspend system.
-                if (rt_exec & EXEC_FEED_HOLD) {
+                if (rt_exec_state.bit.feedHold) {
                     // Block SAFETY_DOOR, JOG, and SLEEP states from changing to HOLD state.
                     if (!(sys.state == State::SafetyDoor || sys.state == State::Jog || sys.state == State::Sleep)) {
                         sys.state = State::Hold;
                     }
+                    sys_rt_exec_state.bit.feedHold = false;
                 }
                 // Execute a safety door stop with a feed hold and disable spindle/coolant.
                 // NOTE: Safety door differs from feed holds by stopping everything no matter state, disables powered
                 // devices (spindle/coolant), and blocks resuming until switch is re-engaged.
-                if (rt_exec & EXEC_SAFETY_DOOR) {
+                if (rt_exec_state.bit.safetyDoor) {
                     report_feedback_message(Message::SafetyDoorAjar);
                     // If jogging, block safety door methods until jog cancel is complete. Just flag that it happened.
-                    if (!(sys.suspend & SUSPEND_JOG_CANCEL)) {
+                    if (!(sys.suspend.bit.jogCancel)) {
                         // Check if the safety re-opened during a restore parking motion only. Ignore if
                         // already retracting, parked or in sleep state.
                         if (sys.state == State::SafetyDoor) {
-                            if (sys.suspend & SUSPEND_INITIATE_RESTORE) {  // Actively restoring
+                            if (sys.suspend.bit.initiateRestore) {  // Actively restoring
 #ifdef PARKING_ENABLE
                                 // Set hold and reset appropriate control flags to restart parking sequence.
-                                if (sys.step_control & STEP_CONTROL_EXECUTE_SYS_MOTION) {
+                                if (sys.step_control.executeSysMotion) {
                                     st_update_plan_block_parameters();  // Notify stepper module to recompute for hold deceleration.
-                                    sys.step_control = (STEP_CONTROL_EXECUTE_HOLD | STEP_CONTROL_EXECUTE_SYS_MOTION);
-                                    sys.suspend &= ~(SUSPEND_HOLD_COMPLETE);
+                                    sys.step_control                  = {};
+                                    sys.step_control.executeHold      = true;
+                                    sys.step_control.executeSysMotion = true;
+                                    sys.suspend.bit.holdComplete      = false;
                                 }  // else NO_MOTION is active.
 #endif
-                                sys.suspend &= ~(SUSPEND_RETRACT_COMPLETE | SUSPEND_INITIATE_RESTORE | SUSPEND_RESTORE_COMPLETE);
-                                sys.suspend |= SUSPEND_RESTART_RETRACT;
+                                sys.suspend.bit.retractComplete = false;
+                                sys.suspend.bit.initiateRestore = false;
+                                sys.suspend.bit.restoreComplete = false;
+                                sys.suspend.bit.restartRetract  = true;
                             }
                         }
                         if (sys.state != State::Sleep) {
                             sys.state = State::SafetyDoor;
                         }
+                        sys_rt_exec_state.bit.safetyDoor = false;
                     }
                     // NOTE: This flag doesn't change when the door closes, unlike sys.state. Ensures any parking motions
                     // are executed if the door switch closes and the state returns to HOLD.
-                    sys.suspend |= SUSPEND_SAFETY_DOOR_AJAR;
+                    sys.suspend.bit.safetyDoorAjar = true;
                 }
             }
-            if (rt_exec & EXEC_SLEEP) {
+            if (rt_exec_state.bit.sleep) {
                 if (sys.state == State::Alarm) {
-                    sys.suspend |= (SUSPEND_RETRACT_COMPLETE | SUSPEND_HOLD_COMPLETE);
+                    sys.suspend.bit.retractComplete = true;
+                    sys.suspend.bit.holdComplete    = true;
                 }
-                sys.state = State::Sleep;
+                sys.state                   = State::Sleep;
+                sys_rt_exec_state.bit.sleep = false;
             }
-            system_clear_exec_state_flag((EXEC_MOTION_CANCEL | EXEC_FEED_HOLD | EXEC_SAFETY_DOOR | EXEC_SLEEP));
         }
         // Execute a cycle start by starting the stepper interrupt to begin executing the blocks in queue.
-        if (rt_exec & EXEC_CYCLE_START) {
+        if (rt_exec_state.bit.cycleStart) {
             // Block if called at same time as the hold commands: feed hold, motion cancel, and safety door.
             // Ensures auto-cycle-start doesn't resume a hold without an explicit user-input.
-            if (!(rt_exec & (EXEC_FEED_HOLD | EXEC_MOTION_CANCEL | EXEC_SAFETY_DOOR))) {
+            if (!(rt_exec_state.bit.feedHold || rt_exec_state.bit.motionCancel || rt_exec_state.bit.safetyDoor)) {
                 // Resume door state when parking motion has retracted and door has been closed.
-                if ((sys.state == State::SafetyDoor) && !(sys.suspend & SUSPEND_SAFETY_DOOR_AJAR)) {
-                    if (sys.suspend & SUSPEND_RESTORE_COMPLETE) {
+                if (sys.state == State::SafetyDoor && !(sys.suspend.bit.safetyDoorAjar)) {
+                    if (sys.suspend.bit.restoreComplete) {
                         sys.state = State::Idle;  // Set to IDLE to immediately resume the cycle.
-                    } else if (sys.suspend & SUSPEND_RETRACT_COMPLETE) {
+                    } else if (sys.suspend.bit.retractComplete) {
                         // Flag to re-energize powered components and restore original position, if disabled by SAFETY_DOOR.
                         // NOTE: For a safety door to resume, the switch must be closed, as indicated by HOLD state, and
                         // the retraction execution is complete, which implies the initial feed hold is not active. To
                         // restore normal operation, the restore procedures must be initiated by the following flag. Once,
                         // they are complete, it will call CYCLE_START automatically to resume and exit the suspend.
-                        sys.suspend |= SUSPEND_INITIATE_RESTORE;
+                        sys.suspend.bit.initiateRestore = true;
                     }
                 }
                 // Cycle start only when IDLE or when a hold is complete and ready to resume.
-                if ((sys.state == State::Idle) || ((sys.state == State::Hold) && (sys.suspend & SUSPEND_HOLD_COMPLETE))) {
-                    if (sys.state == State::Hold && sys.spindle_stop_ovr) {
-                        sys.spindle_stop_ovr |= SPINDLE_STOP_OVR_RESTORE_CYCLE;  // Set to restore in suspend routine and cycle start after.
+                if (sys.state == State::Idle || (sys.state == State::Hold && sys.suspend.bit.holdComplete)) {
+                    if (sys.state == State::Hold && sys.spindle_stop_ovr.value) {
+                        sys.spindle_stop_ovr.bit.restoreCycle = true;  // Set to restore in suspend routine and cycle start after.
                     } else {
                         // Start cycle only if queued motions exist in planner buffer and the motion is not canceled.
-                        sys.step_control = STEP_CONTROL_NORMAL_OP;  // Restore step control to normal operation
-                        if (plan_get_current_block() && bit_isfalse(sys.suspend, SUSPEND_MOTION_CANCEL)) {
-                            sys.suspend = SUSPEND_DISABLE;  // Break suspend state.
-                            sys.state   = State::Cycle;
+                        sys.step_control = {};  // Restore step control to normal operation
+                        if (plan_get_current_block() && !sys.suspend.bit.motionCancel) {
+                            sys.suspend.value = 0;  // Break suspend state.
+                            sys.state         = State::Cycle;
                             st_prep_buffer();  // Initialize step segment buffer before beginning cycle.
                             st_wake_up();
-                        } else {                            // Otherwise, do nothing. Set and resume IDLE state.
-                            sys.suspend = SUSPEND_DISABLE;  // Break suspend state.
-                            sys.state   = State::Idle;
+                        } else {                    // Otherwise, do nothing. Set and resume IDLE state.
+                            sys.suspend.value = 0;  // Break suspend state.
+                            sys.state         = State::Idle;
                         }
                     }
                 }
             }
-            system_clear_exec_state_flag(EXEC_CYCLE_START);
+            sys_rt_exec_state.bit.cycleStart = false;
         }
         if (cycle_stop) {
             // Reinitializes the cycle plan and stepper system after a feed hold for a resume. Called by
@@ -406,150 +417,95 @@ void protocol_exec_rt_system() {
             // cycle reinitializations. The stepper path should continue exactly as if nothing has happened.
             // NOTE: cycle_stop is set by the stepper subsystem when a cycle or feed hold completes.
             if ((sys.state == State::Hold || sys.state == State::SafetyDoor || sys.state == State::Sleep) && !(sys.soft_limit) &&
-                !(sys.suspend & SUSPEND_JOG_CANCEL)) {
+                !(sys.suspend.bit.jogCancel)) {
                 // Hold complete. Set to indicate ready to resume.  Remain in HOLD or DOOR states until user
                 // has issued a resume command or reset.
                 plan_cycle_reinitialize();
-                if (sys.step_control & STEP_CONTROL_EXECUTE_HOLD) {
-                    sys.suspend |= SUSPEND_HOLD_COMPLETE;
+                if (sys.step_control.executeHold) {
+                    sys.suspend.bit.holdComplete = true;
                 }
-                bit_false(sys.step_control, (STEP_CONTROL_EXECUTE_HOLD | STEP_CONTROL_EXECUTE_SYS_MOTION));
+                sys.step_control.executeHold      = false;
+                sys.step_control.executeSysMotion = false;
             } else {
                 // Motion complete. Includes CYCLE/JOG/HOMING states and jog cancel/motion cancel/soft limit events.
                 // NOTE: Motion and jog cancel both immediately return to idle after the hold completes.
-                if (sys.suspend & SUSPEND_JOG_CANCEL) {  // For jog cancel, flush buffers and sync positions.
-                    sys.step_control = STEP_CONTROL_NORMAL_OP;
+                if (sys.suspend.bit.jogCancel) {  // For jog cancel, flush buffers and sync positions.
+                    sys.step_control = {};
                     plan_reset();
                     st_reset();
                     gc_sync_position();
                     plan_sync_position();
                 }
-                if (sys.suspend & SUSPEND_SAFETY_DOOR_AJAR) {  // Only occurs when safety door opens during jog.
-                    sys.suspend &= ~(SUSPEND_JOG_CANCEL);
-                    sys.suspend |= SUSPEND_HOLD_COMPLETE;
-                    sys.state = State::SafetyDoor;
+                if (sys.suspend.bit.safetyDoorAjar) {  // Only occurs when safety door opens during jog.
+                    sys.suspend.bit.jogCancel    = false;
+                    sys.suspend.bit.holdComplete = true;
+                    sys.state                    = State::SafetyDoor;
                 } else {
-                    sys.suspend = SUSPEND_DISABLE;
-                    sys.state   = State::Idle;
+                    sys.suspend.value = 0;
+                    sys.state         = State::Idle;
                 }
             }
             cycle_stop = false;
         }
     }
     // Execute overrides.
-    rt_exec = sys_rt_exec_motion_override;  // Copy volatile sys_rt_exec_motion_override
-    if (rt_exec) {
-        system_clear_exec_motion_overrides();  // Clear all motion override flags.
-        uint8_t new_f_override = sys.f_override;
-        if (rt_exec & EXEC_FEED_OVR_RESET) {
-            new_f_override = DEFAULT_FEED_OVERRIDE;
-        }
-        if (rt_exec & EXEC_FEED_OVR_COARSE_PLUS) {
-            new_f_override += FEED_OVERRIDE_COARSE_INCREMENT;
-        }
-        if (rt_exec & EXEC_FEED_OVR_COARSE_MINUS) {
-            new_f_override -= FEED_OVERRIDE_COARSE_INCREMENT;
-        }
-        if (rt_exec & EXEC_FEED_OVR_FINE_PLUS) {
-            new_f_override += FEED_OVERRIDE_FINE_INCREMENT;
-        }
-        if (rt_exec & EXEC_FEED_OVR_FINE_MINUS) {
-            new_f_override -= FEED_OVERRIDE_FINE_INCREMENT;
-        }
-        new_f_override         = MIN(new_f_override, MAX_FEED_RATE_OVERRIDE);
-        new_f_override         = MAX(new_f_override, MIN_FEED_RATE_OVERRIDE);
-        uint8_t new_r_override = sys.r_override;
-        if (rt_exec & EXEC_RAPID_OVR_RESET) {
-            new_r_override = DEFAULT_RAPID_OVERRIDE;
-        }
-        if (rt_exec & EXEC_RAPID_OVR_MEDIUM) {
-            new_r_override = RAPID_OVERRIDE_MEDIUM;
-        }
-        if (rt_exec & EXEC_RAPID_OVR_LOW) {
-            new_r_override = RAPID_OVERRIDE_LOW;
-        }
-        if ((new_f_override != sys.f_override) || (new_r_override != sys.r_override)) {
-            sys.f_override         = new_f_override;
-            sys.r_override         = new_r_override;
-            sys.report_ovr_counter = 0;  // Set to report change immediately
-            plan_update_velocity_profile_parameters();
-            plan_cycle_reinitialize();
+    if ((sys_rt_f_override != sys.f_override) || (sys_rt_r_override != sys.r_override)) {
+        sys.f_override         = sys_rt_f_override;
+        sys.r_override         = sys_rt_r_override;
+        sys.report_ovr_counter = 0;  // Set to report change immediately
+        plan_update_velocity_profile_parameters();
+        plan_cycle_reinitialize();
+    }
+
+    // NOTE: Unlike motion overrides, spindle overrides do not require a planner reinitialization.
+    if (sys_rt_s_override != sys.spindle_speed_ovr) {
+        sys.step_control.updateSpindleRpm = true;
+        sys.spindle_speed_ovr             = sys_rt_s_override;
+        sys.report_ovr_counter            = 0;  // Set to report change immediately
+        // If spinlde is on, tell it the rpm has been overridden
+        if (gc_state.modal.spindle != SpindleState::Disable) {
+            spindle->set_rpm(gc_state.spindle_speed);
         }
     }
-    rt_exec = sys_rt_exec_accessory_override;
-    if (rt_exec) {
-        system_clear_exec_accessory_overrides();  // Clear all accessory override flags.
-        // NOTE: Unlike motion overrides, spindle overrides do not require a planner reinitialization.
-        uint8_t last_s_override = sys.spindle_speed_ovr;
-        if (rt_exec & EXEC_SPINDLE_OVR_RESET) {
-            last_s_override = DEFAULT_SPINDLE_SPEED_OVERRIDE;
-        }
-        if (rt_exec & EXEC_SPINDLE_OVR_COARSE_PLUS) {
-            last_s_override += SPINDLE_OVERRIDE_COARSE_INCREMENT;
-        }
-        if (rt_exec & EXEC_SPINDLE_OVR_COARSE_MINUS) {
-            last_s_override -= SPINDLE_OVERRIDE_COARSE_INCREMENT;
-        }
-        if (rt_exec & EXEC_SPINDLE_OVR_FINE_PLUS) {
-            last_s_override += SPINDLE_OVERRIDE_FINE_INCREMENT;
-        }
-        if (rt_exec & EXEC_SPINDLE_OVR_FINE_MINUS) {
-            last_s_override -= SPINDLE_OVERRIDE_FINE_INCREMENT;
-        }
-        last_s_override = MIN(last_s_override, MAX_SPINDLE_SPEED_OVERRIDE);
-        last_s_override = MAX(last_s_override, MIN_SPINDLE_SPEED_OVERRIDE);
-        if (last_s_override != sys.spindle_speed_ovr) {
-            bit_true(sys.step_control, STEP_CONTROL_UPDATE_SPINDLE_RPM);
-            sys.spindle_speed_ovr  = last_s_override;
-            sys.report_ovr_counter = 0;  // Set to report change immediately
-            // If spinlde is on, tell it the rpm has been overridden
-            if (gc_state.modal.spindle != SpindleState::Disable) {
-                spindle->set_rpm(gc_state.spindle_speed);
+
+    if (sys_rt_exec_accessory_override.bit.spindleOvrStop) {
+        sys_rt_exec_accessory_override.bit.spindleOvrStop = false;
+        // Spindle stop override allowed only while in HOLD state.
+        // NOTE: Report counters are set in spindle_set_state() when spindle stop is executed.
+        if (sys.state == State::Hold) {
+            if (sys.spindle_stop_ovr.value == 0) {
+                sys.spindle_stop_ovr.bit.initiate = true;
+            } else if (sys.spindle_stop_ovr.bit.enabled) {
+                sys.spindle_stop_ovr.bit.restore = true;
             }
         }
-        if (rt_exec & EXEC_SPINDLE_OVR_STOP) {
-            // Spindle stop override allowed only while in HOLD state.
-            // NOTE: Report counters are set in spindle_set_state() when spindle stop is executed.
-            if (sys.state == State::Hold) {
-                if (!(sys.spindle_stop_ovr)) {
-                    sys.spindle_stop_ovr = SPINDLE_STOP_OVR_INITIATE;
-                } else if (sys.spindle_stop_ovr & SPINDLE_STOP_OVR_ENABLED) {
-                    sys.spindle_stop_ovr |= SPINDLE_STOP_OVR_RESTORE;
-                }
-            }
-        }
-        // NOTE: Since coolant state always performs a planner sync whenever it changes, the current
-        // run state can be determined by checking the parser state.
-        if (rt_exec & (EXEC_COOLANT_FLOOD_OVR_TOGGLE | EXEC_COOLANT_MIST_OVR_TOGGLE)) {
-            if (sys.state == State::Idle || sys.state == State::Cycle || sys.state == State::Hold) {
-                CoolantState coolant_state = gc_state.modal.coolant;
+    }
+
+    // NOTE: Since coolant state always performs a planner sync whenever it changes, the current
+    // run state can be determined by checking the parser state.
+    if (sys_rt_exec_accessory_override.bit.coolantFloodOvrToggle) {
+        sys_rt_exec_accessory_override.bit.coolantFloodOvrToggle = false;
 #ifdef COOLANT_FLOOD_PIN
-                if (rt_exec & EXEC_COOLANT_FLOOD_OVR_TOGGLE) {
-                    if (coolant_state.Flood) {
-                        coolant_state.Flood = 0;
-                    } else {
-                        coolant_state.Flood = 1;
-                    }
-                }
-#endif
-#ifdef COOLANT_MIST_PIN
-                if (rt_exec & EXEC_COOLANT_MIST_OVR_TOGGLE) {
-                    if (coolant_state.Mist) {
-                        coolant_state.Mist = 0;
-                    } else {
-                        coolant_state.Mist = 1;
-                    }
-                }
-#endif
-                coolant_set_state(coolant_state);  // Report counter set in coolant_set_state().
-                gc_state.modal.coolant = coolant_state;
-            }
+        if (sys.state == State::Idle || sys.state == State::Cycle || sys.state == State::Hold) {
+            gc_state.modal.coolant.Flood = !gc_state.modal.coolant.Flood;
+            coolant_set_state(gc_state.modal.coolant);  // Report counter set in coolant_set_state().
         }
+#endif
     }
+    if (sys_rt_exec_accessory_override.bit.coolantMistOvrToggle) {
+        sys_rt_exec_accessory_override.bit.coolantMistOvrToggle = false;
+#ifdef COOLANT_MIST_PIN
+        if (sys.state == State::Idle || sys.state == State::Cycle || sys.state == State::Hold) {
+            gc_state.modal.coolant.Mist = !gc_state.modal.coolant.Mist;
+            coolant_set_state(gc_state.modal.coolant);  // Report counter set in coolant_set_state().
+        }
+#endif
+    }
+
 #ifdef DEBUG
     if (sys_rt_exec_debug) {
         report_realtime_debug();
-        sys_rt_exec_debug = 0;
+        sys_rt_exec_debug = false;
     }
 #endif
     // Reload step segment buffer
@@ -603,30 +559,30 @@ static void protocol_exec_rt_suspend() {
     }
 #ifdef DISABLE_LASER_DURING_HOLD
     if (laser_mode->get()) {
-        system_set_exec_accessory_override_flag(EXEC_SPINDLE_OVR_STOP);
+        sys_rt_exec_accessory_override.bit.spindleOvrStop = true;
     }
 #endif
 
-    while (sys.suspend) {
+    while (sys.suspend.value) {
         if (sys.abort) {
             return;
         }
         // Block until initial hold is complete and the machine has stopped motion.
-        if (sys.suspend & SUSPEND_HOLD_COMPLETE) {
+        if (sys.suspend.bit.holdComplete) {
             // Parking manager. Handles de/re-energizing, switch state checks, and parking motions for
             // the safety door and sleep states.
             if (sys.state == State::SafetyDoor || sys.state == State::Sleep) {
                 // Handles retraction motions and de-energizing.
-                if (bit_isfalse(sys.suspend, SUSPEND_RETRACT_COMPLETE)) {
+                if (!sys.suspend.bit.retractComplete) {
                     // Ensure any prior spindle stop override is disabled at start of safety door routine.
-                    sys.spindle_stop_ovr = SPINDLE_STOP_OVR_DISABLED;
+                    sys.spindle_stop_ovr.value = 0;  // Disable override
 #ifndef PARKING_ENABLE
                     spindle->set_state(SpindleState::Disable, 0);  // De-energize
                     coolant_off();
 #else
                     // Get current position and store restore location and spindle retract waypoint.
                     system_convert_array_steps_to_mpos(parking_target, sys_position);
-                    if (bit_isfalse(sys.suspend, SUSPEND_RESTART_RETRACT)) {
+                    if (!sys.suspend.bit.restartRetract) {
                         memcpy(restore_target, parking_target, sizeof(parking_target));
                         retract_waypoint += restore_target[PARKING_AXIS];
                         retract_waypoint = MIN(retract_waypoint, PARKING_TARGET);
@@ -667,8 +623,8 @@ static void protocol_exec_rt_suspend() {
                         coolant_off();
                     }
 #endif
-                    sys.suspend &= ~(SUSPEND_RESTART_RETRACT);
-                    sys.suspend |= SUSPEND_RETRACT_COMPLETE;
+                    sys.suspend.bit.restartRetract  = false;
+                    sys.suspend.bit.retractComplete = true;
                 } else {
                     if (sys.state == State::Sleep) {
                         report_feedback_message(Message::SleepMode);
@@ -684,11 +640,11 @@ static void protocol_exec_rt_suspend() {
                     // Allows resuming from parking/safety door. Actively checks if safety door is closed and ready to resume.
                     if (sys.state == State::SafetyDoor) {
                         if (!(system_check_safety_door_ajar())) {
-                            sys.suspend &= ~(SUSPEND_SAFETY_DOOR_AJAR);  // Reset door ajar flag to denote ready to resume.
+                            sys.suspend.bit.safetyDoorAjar = false;  // Reset door ajar flag to denote ready to resume.
                         }
                     }
                     // Handles parking restore and safety door resume.
-                    if (sys.suspend & SUSPEND_INITIATE_RESTORE) {
+                    if (sys.suspend.bit.initiateRestore) {
 #ifdef PARKING_ENABLE
                         // Execute fast restore motion to the pull-out position. Parking requires homing enabled.
                         // NOTE: State is will remain DOOR, until the de-energizing and retract is complete.
@@ -704,10 +660,10 @@ static void protocol_exec_rt_suspend() {
                         // Delayed Tasks: Restart spindle and coolant, delay to power-up, then resume cycle.
                         if (gc_state.modal.spindle != SpindleState::Disable) {
                             // Block if safety door re-opened during prior restore actions.
-                            if (bit_isfalse(sys.suspend, SUSPEND_RESTART_RETRACT)) {
+                            if (!sys.suspend.bit.restartRetract) {
                                 if (laser_mode->get()) {
                                     // When in laser mode, ignore spindle spin-up delay. Set to turn on laser when cycle starts.
-                                    bit_true(sys.step_control, STEP_CONTROL_UPDATE_SPINDLE_RPM);
+                                    sys.step_control.updateSpindleRpm = true;
                                 } else {
                                     spindle->set_state(restore_spindle, (uint32_t)restore_spindle_speed);
                                     delay_sec(SAFETY_DOOR_SPINDLE_DELAY, DELAY_MODE_SYS_SUSPEND);
@@ -716,7 +672,7 @@ static void protocol_exec_rt_suspend() {
                         }
                         if (gc_state.modal.coolant.Flood || gc_state.modal.coolant.Mist) {
                             // Block if safety door re-opened during prior restore actions.
-                            if (bit_isfalse(sys.suspend, SUSPEND_RESTART_RETRACT)) {
+                            if (!sys.suspend.bit.restartRetract) {
                                 // NOTE: Laser mode will honor this delay. An exhaust system is often controlled by this pin.
                                 coolant_set_state(restore_coolant);
                                 delay_sec(SAFETY_DOOR_COOLANT_DELAY, DELAY_MODE_SYS_SUSPEND);
@@ -726,7 +682,7 @@ static void protocol_exec_rt_suspend() {
                         // Execute slow plunge motion from pull-out position to resume position.
                         if (can_park()) {
                             // Block if safety door re-opened during prior restore actions.
-                            if (bit_isfalse(sys.suspend, SUSPEND_RESTART_RETRACT)) {
+                            if (!sys.suspend.bit.restartRetract) {
                                 // Regardless if the retract parking motion was a valid/safe motion or not, the
                                 // restore parking motion should logically be valid, either by returning to the
                                 // original position through valid machine space or by not moving at all.
@@ -738,46 +694,47 @@ static void protocol_exec_rt_suspend() {
                             }
                         }
 #endif
-                        if (bit_isfalse(sys.suspend, SUSPEND_RESTART_RETRACT)) {
-                            sys.suspend |= SUSPEND_RESTORE_COMPLETE;
-                            system_set_exec_state_flag(EXEC_CYCLE_START);  // Set to resume program.
+                        if (!sys.suspend.bit.restartRetract) {
+                            sys.suspend.bit.restoreComplete  = true;
+                            sys_rt_exec_state.bit.cycleStart = true;  // Set to resume program.
                         }
                     }
                 }
             } else {
                 // Feed hold manager. Controls spindle stop override states.
                 // NOTE: Hold ensured as completed by condition check at the beginning of suspend routine.
-                if (sys.spindle_stop_ovr) {
+                if (sys.spindle_stop_ovr.value) {
                     // Handles beginning of spindle stop
-                    if (sys.spindle_stop_ovr & SPINDLE_STOP_OVR_INITIATE) {
+                    if (sys.spindle_stop_ovr.bit.initiate) {
                         if (gc_state.modal.spindle != SpindleState::Disable) {
-                            spindle->set_state(SpindleState::Disable, 0);     // De-energize
-                            sys.spindle_stop_ovr = SPINDLE_STOP_OVR_ENABLED;  // Set stop override state to enabled, if de-energized.
+                            spindle->set_state(SpindleState::Disable, 0);  // De-energize
+                            sys.spindle_stop_ovr.value       = 0;
+                            sys.spindle_stop_ovr.bit.enabled = true;  // Set stop override state to enabled, if de-energized.
                         } else {
-                            sys.spindle_stop_ovr = SPINDLE_STOP_OVR_DISABLED;  // Clear stop override state
+                            sys.spindle_stop_ovr.value = 0;  // Clear stop override state
                         }
                         // Handles restoring of spindle state
-                    } else if (sys.spindle_stop_ovr & (SPINDLE_STOP_OVR_RESTORE | SPINDLE_STOP_OVR_RESTORE_CYCLE)) {
+                    } else if (sys.spindle_stop_ovr.bit.restore || sys.spindle_stop_ovr.bit.restoreCycle) {
                         if (gc_state.modal.spindle != SpindleState::Disable) {
                             report_feedback_message(Message::SpindleRestore);
                             if (laser_mode->get()) {
                                 // When in laser mode, ignore spindle spin-up delay. Set to turn on laser when cycle starts.
-                                bit_true(sys.step_control, STEP_CONTROL_UPDATE_SPINDLE_RPM);
+                                sys.step_control.updateSpindleRpm = true;
                             } else {
                                 spindle->set_state(restore_spindle, (uint32_t)restore_spindle_speed);
                             }
                         }
-                        if (sys.spindle_stop_ovr & SPINDLE_STOP_OVR_RESTORE_CYCLE) {
-                            system_set_exec_state_flag(EXEC_CYCLE_START);  // Set to resume program.
+                        if (sys.spindle_stop_ovr.bit.restoreCycle) {
+                            sys_rt_exec_state.bit.cycleStart = true;  // Set to resume program.
                         }
-                        sys.spindle_stop_ovr = SPINDLE_STOP_OVR_DISABLED;  // Clear stop override state
+                        sys.spindle_stop_ovr.value = 0;  // Clear stop override state
                     }
                 } else {
                     // Handles spindle state during hold. NOTE: Spindle speed overrides may be altered during hold state.
-                    // NOTE: STEP_CONTROL_UPDATE_SPINDLE_RPM is automatically reset upon resume in step generator.
-                    if (bit_istrue(sys.step_control, STEP_CONTROL_UPDATE_SPINDLE_RPM)) {
+                    // NOTE: sys.step_control.updateSpindleRpm is automatically reset upon resume in step generator.
+                    if (sys.step_control.updateSpindleRpm) {
                         spindle->set_state(restore_spindle, (uint32_t)restore_spindle_speed);
-                        bit_false(sys.step_control, STEP_CONTROL_UPDATE_SPINDLE_RPM);
+                        sys.step_control.updateSpindleRpm = false;
                     }
                 }
             }
diff --git a/Grbl_Esp32/src/Protocol.h b/Grbl_Esp32/src/Protocol.h
index 6b91553d..83033064 100644
--- a/Grbl_Esp32/src/Protocol.h
+++ b/Grbl_Esp32/src/Protocol.h
@@ -31,7 +31,7 @@
 // memory space we can invest into here or we re-write the g-code parser not to have this
 // buffer.
 #ifndef LINE_BUFFER_SIZE
-#    define LINE_BUFFER_SIZE 80
+#    define LINE_BUFFER_SIZE 256
 #endif
 
 // Starts Grbl main loop. It handles all incoming characters from the serial port and executes
diff --git a/Grbl_Esp32/src/Report.cpp b/Grbl_Esp32/src/Report.cpp
index e307b05d..27ba9ea8 100644
--- a/Grbl_Esp32/src/Report.cpp
+++ b/Grbl_Esp32/src/Report.cpp
@@ -102,7 +102,7 @@ void grbl_sendf(uint8_t client, const char* format, ...) {
     len = vsnprintf(temp, len + 1, format, arg);
     grbl_send(client, temp);
     va_end(arg);
-    if (len > 64) {
+    if (temp != loc_buf) {
         delete[] temp;
     }
 }
@@ -131,7 +131,7 @@ void grbl_msg_sendf(uint8_t client, MsgLevel level, const char* format, ...) {
     len = vsnprintf(temp, len + 1, format, arg);
     grbl_sendf(client, "[MSG:%s]\r\n", temp);
     va_end(arg);
-    if (len > 100) {
+    if (temp != loc_buf) {
         delete[] temp;
     }
 }
@@ -161,29 +161,29 @@ void grbl_notifyf(const char* title, const char* format, ...) {
     len = vsnprintf(temp, len + 1, format, arg);
     grbl_notify(title, temp);
     va_end(arg);
-    if (len > 64) {
+    if (temp != loc_buf) {
         delete[] temp;
     }
 }
 
 static const int coordStringLen = 20;
-static const int axesStringLen = coordStringLen * MAX_N_AXIS;
+static const int axesStringLen  = coordStringLen * MAX_N_AXIS;
 
 // formats axis values into a string and returns that string in rpt
 // NOTE: rpt should have at least size: axesStringLen
 static void report_util_axis_values(float* axis_value, char* rpt) {
-    uint8_t idx;
-    char    axisVal[coordStringLen];
-    float   unit_conv = 1.0;  // unit conversion multiplier..default is mm
-    const char* format = "%4.3f";  // Default - report mm to 3 decimal places
-    rpt[0]            = '\0';
+    uint8_t     idx;
+    char        axisVal[coordStringLen];
+    float       unit_conv = 1.0;      // unit conversion multiplier..default is mm
+    const char* format    = "%4.3f";  // Default - report mm to 3 decimal places
+    rpt[0]                = '\0';
     if (report_inches->get()) {
         unit_conv = 1.0 / MM_PER_INCH;
-        format = "%4.4f";  // Report inches to 4 decimal places
+        format    = "%4.4f";  // Report inches to 4 decimal places
     }
     auto n_axis = number_axis->get();
     for (idx = 0; idx < n_axis; idx++) {
-        snprintf(axisVal, coordStringLen-1, format, axis_value[idx] * unit_conv);
+        snprintf(axisVal, coordStringLen - 1, format, axis_value[idx] * unit_conv);
         strcat(rpt, axisVal);
         if (idx < (number_axis->get() - 1)) {
             strcat(rpt, ",");
@@ -193,14 +193,14 @@ static void report_util_axis_values(float* axis_value, char* rpt) {
 
 // This version returns the axis values as a String
 static String report_util_axis_values(const float* axis_value) {
-    String rpt = "";
+    String  rpt = "";
     uint8_t idx;
     char    axisVal[coordStringLen];
     float   unit_conv = 1.0;  // unit conversion multiplier..default is mm
-    int decimals = 3;  // Default - report mm to 3 decimal places
+    int     decimals  = 3;    // Default - report mm to 3 decimal places
     if (report_inches->get()) {
         unit_conv = 1.0 / MM_PER_INCH;
-        decimals = 4;  // Report inches to 4 decimal places
+        decimals  = 4;  // Report inches to 4 decimal places
     }
     auto n_axis = number_axis->get();
     for (idx = 0; idx < n_axis; idx++) {
@@ -321,7 +321,7 @@ void report_probe_parameters(uint8_t client) {
 
 // Prints Grbl NGC parameters (coordinate offsets, probing)
 void report_ngc_parameters(uint8_t client) {
-    String  ngc_rpt = "";
+    String ngc_rpt = "";
 
     // Print persistent offsets G54 - G59, G28, and G30
     for (auto coord_select = CoordIndex::Begin; coord_select < CoordIndex::End; ++coord_select) {
@@ -339,7 +339,8 @@ void report_ngc_parameters(uint8_t client) {
     if (report_inches->get()) {
         tlo *= INCH_PER_MM;
     }
-    ngc_rpt += String(tlo, 3);;
+    ngc_rpt += String(tlo, 3);
+    ;
     ngc_rpt += "]\r\n";
     grbl_send(client, ngc_rpt.c_str());
     report_probe_parameters(client);
@@ -487,7 +488,7 @@ void report_gcode_modes(uint8_t client) {
     }
 
 #ifdef ENABLE_PARKING_OVERRIDE_CONTROL
-    if (sys.override_ctrl == OVERRIDE_PARKING_MOTION) {
+    if (sys.override_ctrl == Override::ParkingMotion) {
         strcat(modes_rpt, " M56");
     }
 #endif
@@ -513,69 +514,56 @@ void report_execute_startup_message(const char* line, Error status_code, uint8_t
 }
 
 // Prints build info line
-void report_build_info(char* line, uint8_t client) {
-    char build_info[50];
-    strcpy(build_info, "[VER:");
-    strcat(build_info, GRBL_VERSION);
-    strcat(build_info, ".");
-    strcat(build_info, GRBL_VERSION_BUILD);
-    strcat(build_info, ":");
-    strcat(build_info, line);
-    strcat(build_info, "]\r\n[OPT:");
-    strcat(build_info, "V");  // variable spindle..always on now
-    strcat(build_info, "N");
+void report_build_info(const char* line, uint8_t client) {
+    grbl_sendf(client, "[VER:%s.%s:%s]\r\n[OPT:", GRBL_VERSION, GRBL_VERSION_BUILD, line);
 #ifdef COOLANT_MIST_PIN
-    strcat(build_info, "M");  // TODO Need to deal with M8...it could be disabled
+    grbl_send(client, "M");  // TODO Need to deal with M8...it could be disabled
 #endif
 #ifdef COREXY
-    strcat(build_info, "C");
+    grbl_send(client, "C");
 #endif
 #ifdef PARKING_ENABLE
-    strcat(build_info, "P");
+    grbl_send(client, "P");
 #endif
 #ifdef HOMING_SINGLE_AXIS_COMMANDS
-    strcat(build_info, "H");
+    grbl_send(client, "H");
 #endif
 #ifdef LIMITS_TWO_SWITCHES_ON_AXES
-    strcat(build_info, "L");
+    grbl_send(client, "L");
 #endif
 #ifdef ALLOW_FEED_OVERRIDE_DURING_PROBE_CYCLES
-    strcat(build_info, "A");
+    grbl_send(client, "A");
 #endif
 #ifdef ENABLE_BLUETOOTH
-    strcat(build_info, "B");
+    grbl_send(client, "B");
 #endif
 #ifdef ENABLE_SD_CARD
-    strcat(build_info, "S");
+    grbl_send(client, "S");
 #endif
 #ifdef ENABLE_PARKING_OVERRIDE_CONTROL
-    serial_write('R');
+    grbl_send(client, "R");
 #endif
 #if defined(ENABLE_WIFI)
-    strcat(build_info, "W");
+    grbl_send(client, "W");
 #endif
-#ifndef ENABLE_RESTORE_EEPROM_WIPE_ALL  // NOTE: Shown when disabled.
-    strcat(build_info, "*");
+#ifndef ENABLE_RESTORE_WIPE_ALL  // NOTE: Shown when disabled.
+    grbl_send(client, "*");
 #endif
-#ifndef ENABLE_RESTORE_EEPROM_DEFAULT_SETTINGS  // NOTE: Shown when disabled.
-    strcat(build_info, "$");
+#ifndef ENABLE_RESTORE_DEFAULT_SETTINGS  // NOTE: Shown when disabled.
+    grbl_send(client, "$");
 #endif
-#ifndef ENABLE_RESTORE_EEPROM_CLEAR_PARAMETERS  // NOTE: Shown when disabled.
-    strcat(build_info, "#");
+#ifndef ENABLE_RESTORE_CLEAR_PARAMETERS  // NOTE: Shown when disabled.
+    grbl_send(client, "#");
 #endif
-#ifndef ENABLE_BUILD_INFO_WRITE_COMMAND  // NOTE: Shown when disabled.
-    strcat(build_info, "I");
-#endif
-#ifndef FORCE_BUFFER_SYNC_DURING_EEPROM_WRITE  // NOTE: Shown when disabled.
-    strcat(build_info, "E");
+#ifndef FORCE_BUFFER_SYNC_DURING_NVS_WRITE  // NOTE: Shown when disabled.
+    grbl_send(client, "E");
 #endif
 #ifndef FORCE_BUFFER_SYNC_DURING_WCO_CHANGE  // NOTE: Shown when disabled.
-    strcat(build_info, "W");
+    grbl_send(client, "W");
 #endif
     // NOTE: Compiled values, like override increments/max/min values, may be added at some point later.
     // These will likely have a comma delimiter to separate them.
-    strcat(build_info, "]\r\n");
-    grbl_send(client, build_info);  // ok to send to all
+    grbl_send(client, "]\r\n");
     report_machine_type(client);
 #if defined(ENABLE_WIFI)
     grbl_send(client, (char*)WebUI::wifi_config.info());
@@ -614,13 +602,9 @@ void report_realtime_status(uint8_t client) {
             strcat(status, "Run");
             break;
         case State::Hold:
-            if (!(sys.suspend & SUSPEND_JOG_CANCEL)) {
+            if (!(sys.suspend.bit.jogCancel)) {
                 strcat(status, "Hold:");
-                if (sys.suspend & SUSPEND_HOLD_COMPLETE) {
-                    strcat(status, "0");  // Ready to resume
-                } else {
-                    strcat(status, "1");  // Actively holding
-                }
+                strcat(status, sys.suspend.bit.holdComplete ? "0" : "1");  // Ready to resume
                 break;
             }  // Continues to print jog state during jog cancel.
         case State::Jog:
@@ -637,15 +621,11 @@ void report_realtime_status(uint8_t client) {
             break;
         case State::SafetyDoor:
             strcat(status, "Door:");
-            if (sys.suspend & SUSPEND_INITIATE_RESTORE) {
+            if (sys.suspend.bit.initiateRestore) {
                 strcat(status, "3");  // Restoring
             } else {
-                if (sys.suspend & SUSPEND_RETRACT_COMPLETE) {
-                    if (sys.suspend & SUSPEND_SAFETY_DOOR_AJAR) {
-                        strcat(status, "1");  // Door ajar
-                    } else {
-                        strcat(status, "0");
-                    }
+                if (sys.suspend.bit.retractComplete) {
+                    strcat(status, sys.suspend.bit.safetyDoorAjar ? "1" : "0");  // Door ajar
                     // Door closed and ready to resume
                 } else {
                     strcat(status, "2");  // Retracting
@@ -657,7 +637,7 @@ void report_realtime_status(uint8_t client) {
             break;
     }
     float wco[MAX_N_AXIS];
-    if (bit_isfalse(status_mask->get(), BITFLAG_RT_STATUS_POSITION_TYPE) || (sys.report_wco_counter == 0)) {
+    if (bit_isfalse(status_mask->get(), RtStatus::Position) || (sys.report_wco_counter == 0)) {
         auto n_axis = number_axis->get();
         for (idx = 0; idx < n_axis; idx++) {
             // Apply work coordinate offsets and tool length offset to current position.
@@ -665,13 +645,13 @@ void report_realtime_status(uint8_t client) {
             if (idx == TOOL_LENGTH_OFFSET_AXIS) {
                 wco[idx] += gc_state.tool_length_offset;
             }
-            if (bit_isfalse(status_mask->get(), BITFLAG_RT_STATUS_POSITION_TYPE)) {
+            if (bit_isfalse(status_mask->get(), RtStatus::Position)) {
                 print_position[idx] -= wco[idx];
             }
         }
     }
     // Report machine position
-    if (bit_istrue(status_mask->get(), BITFLAG_RT_STATUS_POSITION_TYPE)) {
+    if (bit_istrue(status_mask->get(), RtStatus::Position)) {
         strcat(status, "|MPos:");
     } else {
 #ifdef USE_FWD_KINEMATICS
@@ -683,7 +663,7 @@ void report_realtime_status(uint8_t client) {
     strcat(status, temp);
     // Returns planner and serial read buffer states.
 #ifdef REPORT_FIELD_BUFFER_STATE
-    if (bit_istrue(status_mask->get(), BITFLAG_RT_STATUS_BUFFER_STATE)) {
+    if (bit_istrue(status_mask->get(), RtStatus::Buffer)) {
         int bufsize = DEFAULTBUFFERSIZE;
 #    if defined(ENABLE_WIFI) && defined(ENABLE_TELNET)
         if (client == CLIENT_TELNET) {
@@ -726,10 +706,10 @@ void report_realtime_status(uint8_t client) {
     strcat(status, temp);
 #endif
 #ifdef REPORT_FIELD_PIN_STATE
-    uint8_t lim_pin_state  = limits_get_state();
-    uint8_t ctrl_pin_state = system_control_get_state();
-    uint8_t prb_pin_state  = probe_get_state();
-    if (lim_pin_state | ctrl_pin_state | prb_pin_state) {
+    AxisMask    lim_pin_state  = limits_get_state();
+    ControlPins ctrl_pin_state = system_control_get_state();
+    bool        prb_pin_state  = probe_get_state();
+    if (lim_pin_state || ctrl_pin_state.value || prb_pin_state) {
         strcat(status, "|Pn:");
         if (prb_pin_state) {
             strcat(status, "P");
@@ -755,21 +735,31 @@ void report_realtime_status(uint8_t client) {
                 strcat(status, "C");
             }
         }
-        if (ctrl_pin_state) {
-#    ifdef ENABLE_SAFETY_DOOR_INPUT_PIN
-            if (bit_istrue(ctrl_pin_state, CONTROL_PIN_INDEX_SAFETY_DOOR)) {
+        if (ctrl_pin_state.value) {
+            if (ctrl_pin_state.bit.safetyDoor) {
                 strcat(status, "D");
             }
-#    endif
-            if (bit_istrue(ctrl_pin_state, CONTROL_PIN_INDEX_RESET)) {
+            if (ctrl_pin_state.bit.reset) {
                 strcat(status, "R");
             }
-            if (bit_istrue(ctrl_pin_state, CONTROL_PIN_INDEX_FEED_HOLD)) {
+            if (ctrl_pin_state.bit.feedHold) {
                 strcat(status, "H");
             }
-            if (bit_istrue(ctrl_pin_state, CONTROL_PIN_INDEX_CYCLE_START)) {
+            if (ctrl_pin_state.bit.cycleStart) {
                 strcat(status, "S");
             }
+            if (ctrl_pin_state.bit.macro0) {
+                strcat(status, "M0");
+            }
+            if (ctrl_pin_state.bit.macro1) {
+                strcat(status, "M1");
+            }
+            if (ctrl_pin_state.bit.macro2) {
+                strcat(status, "M2");
+            }
+            if (ctrl_pin_state.bit.macro3) {
+                strcat(status, "M3");
+            }
         }
     }
 #endif
@@ -930,3 +920,31 @@ char report_get_axis_letter(uint8_t axis) {
             return '?';
     }
 }
+
+char* reportAxisLimitsMsg(uint8_t axis) {
+    static char msg[40];
+    sprintf(msg, "Limits(%0.3f,%0.3f)", limitsMinPosition(axis), limitsMaxPosition(axis));
+    return msg;
+}
+
+char* reportAxisNameMsg(uint8_t axis, uint8_t dual_axis) {
+    static char name[10];
+    sprintf(name, "%c%c Axis", report_get_axis_letter(axis), dual_axis ? '2' : ' ');
+    return name;
+}
+
+char* reportAxisNameMsg(uint8_t axis) {
+    static char name[10];
+    sprintf(name, "%c  Axis", report_get_axis_letter(axis));
+    return name;
+}
+
+void reportTaskStackSize(UBaseType_t& saved) {
+#ifdef DEBUG_REPORT_STACK_FREE
+    UBaseType_t        newHighWater    = uxTaskGetStackHighWaterMark(NULL);
+    if (newHighWater != saved) {
+        saved = newHighWater;
+        grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "%s Min Stack Space: %d", pcTaskGetTaskName(NULL), saved);
+    }
+#endif
+}
diff --git a/Grbl_Esp32/src/Report.h b/Grbl_Esp32/src/Report.h
index 624cb1d9..5b128b5d 100644
--- a/Grbl_Esp32/src/Report.h
+++ b/Grbl_Esp32/src/Report.h
@@ -20,6 +20,12 @@
   along with Grbl.  If not, see <http://www.gnu.org/licenses/>.
 */
 
+// Define status reporting boolean enable bit flags in status_report_mask
+enum RtStatus {
+    Position = bit(0),
+    Buffer   = bit(1),
+};
+
 const char* errorString(Error errorNumber);
 
 // Define Grbl feedback message codes. Valid values (0-255).
@@ -103,7 +109,7 @@ void report_startup_line(uint8_t n, const char* line, uint8_t client);
 void report_execute_startup_message(const char* line, Error status_code, uint8_t client);
 
 // Prints build info and user info
-void report_build_info(char* line, uint8_t client);
+void report_build_info(const char* line, uint8_t client);
 
 void report_gcode_comment(char* comment);
 
@@ -117,3 +123,9 @@ void report_hex_msg(char* buf, const char* prefix, int len);
 void report_hex_msg(uint8_t* buf, const char* prefix, int len);
 
 char report_get_axis_letter(uint8_t axis);
+
+char* reportAxisLimitsMsg(uint8_t axis);
+char* reportAxisNameMsg(uint8_t axis);
+char* reportAxisNameMsg(uint8_t axis, uint8_t dual_axis);
+
+void  reportTaskStackSize(UBaseType_t& saved);
diff --git a/Grbl_Esp32/src/SDCard.cpp b/Grbl_Esp32/src/SDCard.cpp
index 4178cc91..c2d8d3c1 100644
--- a/Grbl_Esp32/src/SDCard.cpp
+++ b/Grbl_Esp32/src/SDCard.cpp
@@ -23,6 +23,7 @@
 File        myFile;
 bool        SD_ready_next = false;  // Grbl has processed a line and is waiting for another
 uint8_t     SD_client     = CLIENT_SERIAL;
+WebUI::AuthenticationLevel SD_auth_level = WebUI::AuthenticationLevel::LEVEL_GUEST;
 uint32_t    sd_current_line_number;     // stores the most recent line number read from the SD
 static char comment[LINE_BUFFER_SIZE];  // Line to be executed. Zero-terminated.
 
@@ -80,6 +81,7 @@ boolean closeFile() {
     SD_ready_next          = false;
     sd_current_line_number = 0;
     myFile.close();
+    SD.end();
     return true;
 }
 
@@ -146,7 +148,7 @@ uint8_t get_sd_state(bool refresh) {
     sd_state = SDCARD_NOT_PRESENT;
     //using default value for speed ? should be parameter
     //refresh content if card was removed
-    if (SD.begin((GRBL_SPI_SS == -1) ? SS : GRBL_SPI_SS, SPI, GRBL_SPI_FREQ)) {
+    if (SD.begin((GRBL_SPI_SS == -1) ? SS : GRBL_SPI_SS, SPI, GRBL_SPI_FREQ, "/sd", 2)) {
         if (SD.cardSize() > 0) {
             sd_state = SDCARD_IDLE;
         }
diff --git a/Grbl_Esp32/src/SDCard.h b/Grbl_Esp32/src/SDCard.h
index 4f19f8aa..a64a4e7d 100644
--- a/Grbl_Esp32/src/SDCard.h
+++ b/Grbl_Esp32/src/SDCard.h
@@ -31,6 +31,7 @@ const int SDCARD_BUSY_PARSING   = 8;
 
 extern bool    SD_ready_next;  // Grbl has processed a line and is waiting for another
 extern uint8_t SD_client;
+extern WebUI::AuthenticationLevel SD_auth_level;
 
 //bool sd_mount();
 uint8_t  get_sd_state(bool refresh);
diff --git a/Grbl_Esp32/src/Serial.cpp b/Grbl_Esp32/src/Serial.cpp
index e79516c3..caa78bb6 100644
--- a/Grbl_Esp32/src/Serial.cpp
+++ b/Grbl_Esp32/src/Serial.cpp
@@ -68,16 +68,39 @@ uint8_t serial_get_rx_buffer_available(uint8_t client) {
     return client_buffer[client].availableforwrite();
 }
 
+void heapCheckTask(void* pvParameters) {
+    static uint32_t heapSize = 0;
+    while (true) {
+        uint32_t newHeapSize = xPortGetFreeHeapSize();
+        if (newHeapSize != heapSize) {
+            heapSize = newHeapSize;
+            grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "heap %d", heapSize);
+        }
+        vTaskDelay(3000 / portTICK_RATE_MS);  // Yield to other tasks
+
+        static UBaseType_t uxHighWaterMark = 0;
+        reportTaskStackSize(uxHighWaterMark);
+    }
+}
+
 void serial_init() {
+#ifdef DEBUG_REPORT_HEAP_SIZE
+    // For a 2000-word stack, uxTaskGetStackHighWaterMark reports 288 words available
+    xTaskCreatePinnedToCore(heapCheckTask, "heapTask", 2000, NULL, 1, NULL, 1);
+#endif
+
     Serial.begin(BAUD_RATE);
+    Serial.setRxBufferSize(256);
     // reset all buffers
     serial_reset_read_buffer(CLIENT_ALL);
     grbl_send(CLIENT_SERIAL, "\r\n");  // create some white space after ESP32 boot info
     serialCheckTaskHandle = 0;
     // create a task to check for incoming data
+    // For a 4096-word stack, uxTaskGetStackHighWaterMark reports 244 words available
+    // after WebUI attaches.
     xTaskCreatePinnedToCore(serialCheckTask,    // task
                             "serialCheckTask",  // name for task
-                            8192,               // size of task stack
+                            4096,               // size of task stack
                             NULL,               // parameters
                             1,                  // priority
                             &serialCheckTaskHandle,
@@ -88,9 +111,10 @@ void serial_init() {
 // this task runs and checks for data on all interfaces
 // REaltime stuff is acted upon, then characters are added to the appropriate buffer
 void serialCheckTask(void* pvParameters) {
-    uint8_t data   = 0;
-    uint8_t client = CLIENT_ALL;  // who sent the data
-    while (true) {                // run continuously
+    uint8_t            data            = 0;
+    uint8_t            client          = CLIENT_ALL;  // who sent the data
+    static UBaseType_t uxHighWaterMark = 0;
+    while (true) {  // run continuously
         while (any_client_has_data()) {
             if (Serial.available()) {
                 client = CLIENT_SERIAL;
@@ -104,7 +128,8 @@ void serialCheckTask(void* pvParameters) {
                 if (WebUI::SerialBT.hasClient() && WebUI::SerialBT.available()) {
                     client = CLIENT_BT;
                     data   = WebUI::SerialBT.read();
-                    //Serial.write(data);  // echo all data to serial
+
+                    // Serial.write(data);  // echo all data to serial.
                 } else {
 #endif
 #if defined(ENABLE_WIFI) && defined(ENABLE_HTTP) && defined(ENABLE_SERIAL2SOCKET_IN)
@@ -129,7 +154,7 @@ void serialCheckTask(void* pvParameters) {
             // Pick off realtime command characters directly from the serial stream. These characters are
             // not passed into the main buffer, but these set system state flag bits for realtime execution.
             if (is_realtime_command(data)) {
-                execute_realtime_command(data, client);
+                execute_realtime_command(static_cast<Cmd>(data), client);
             } else {
                 vTaskEnterCritical(&myMutex);
                 client_buffer[client].write(data);
@@ -147,7 +172,10 @@ void serialCheckTask(void* pvParameters) {
         WebUI::Serial2Socket.handle_flush();
 #endif
         vTaskDelay(1 / portTICK_RATE_MS);  // Yield to other tasks
-    }                                      // while(true)
+
+        static UBaseType_t uxHighWaterMark = 0;
+        reportTaskStackSize(uxHighWaterMark);
+    }  // while(true)
 }
 
 void serial_reset_read_buffer(uint8_t client) {
@@ -194,91 +222,115 @@ bool any_client_has_data() {
 
 // checks to see if a character is a realtime character
 bool is_realtime_command(uint8_t data) {
-    return data == CMD_RESET || data == CMD_STATUS_REPORT || data == CMD_CYCLE_START || data == CMD_FEED_HOLD || data > 0x7F;
+    if (data >= 0x80) {
+        return true;
+    }
+    auto cmd = static_cast<Cmd>(data);
+    return cmd == Cmd::Reset || cmd == Cmd::StatusReport || cmd == Cmd::CycleStart || cmd == Cmd::FeedHold;
 }
 
 // Act upon a realtime character
-void execute_realtime_command(uint8_t command, uint8_t client) {
+void execute_realtime_command(Cmd command, uint8_t client) {
     switch (command) {
-        case CMD_RESET:
+        case Cmd::Reset:
             mc_reset();  // Call motion control reset routine.
             break;
-        case CMD_STATUS_REPORT:
+        case Cmd::StatusReport:
             report_realtime_status(client);  // direct call instead of setting flag
             break;
-        case CMD_CYCLE_START:
-            system_set_exec_state_flag(EXEC_CYCLE_START);  // Set as true
+        case Cmd::CycleStart:
+            sys_rt_exec_state.bit.cycleStart = true;
             break;
-        case CMD_FEED_HOLD:
-            system_set_exec_state_flag(EXEC_FEED_HOLD);  // Set as true
+        case Cmd::FeedHold:
+            sys_rt_exec_state.bit.feedHold = true;
             break;
-        case CMD_SAFETY_DOOR:
-            system_set_exec_state_flag(EXEC_SAFETY_DOOR);
-            break;  // Set as true
-        case CMD_JOG_CANCEL:
+        case Cmd::SafetyDoor:
+            sys_rt_exec_state.bit.safetyDoor = true;
+            break;
+        case Cmd::JogCancel:
             if (sys.state == State::Jog) {  // Block all other states from invoking motion cancel.
-                system_set_exec_state_flag(EXEC_MOTION_CANCEL);
+                sys_rt_exec_state.bit.motionCancel = true;
             }
             break;
+        case Cmd::DebugReport:
 #ifdef DEBUG
-        case CMD_DEBUG_REPORT: {
-            uint8_t sreg = SREG;
-            cli();
-            bit_true(sys_rt_exec_debug, EXEC_DEBUG_REPORT);
-            SREG = sreg;
-        } break;
+            sys_rt_exec_debug = true;
 #endif
-        case CMD_FEED_OVR_RESET:
-            system_set_exec_motion_override_flag(EXEC_FEED_OVR_RESET);
             break;
-        case CMD_FEED_OVR_COARSE_PLUS:
-            system_set_exec_motion_override_flag(EXEC_FEED_OVR_COARSE_PLUS);
+        case Cmd::SpindleOvrStop:
+            sys_rt_exec_accessory_override.bit.spindleOvrStop = 1;
             break;
-        case CMD_FEED_OVR_COARSE_MINUS:
-            system_set_exec_motion_override_flag(EXEC_FEED_OVR_COARSE_MINUS);
+        case Cmd::FeedOvrReset:
+            sys_rt_f_override = FeedOverride::Default;
             break;
-        case CMD_FEED_OVR_FINE_PLUS:
-            system_set_exec_motion_override_flag(EXEC_FEED_OVR_FINE_PLUS);
+        case Cmd::FeedOvrCoarsePlus:
+            sys_rt_f_override += FeedOverride::CoarseIncrement;
+            if (sys_rt_f_override > FeedOverride::Max) {
+                sys_rt_f_override = FeedOverride::Max;
+            }
             break;
-        case CMD_FEED_OVR_FINE_MINUS:
-            system_set_exec_motion_override_flag(EXEC_FEED_OVR_FINE_MINUS);
+        case Cmd::FeedOvrCoarseMinus:
+            sys_rt_f_override -= FeedOverride::CoarseIncrement;
+            if (sys_rt_f_override < FeedOverride::Min) {
+                sys_rt_f_override = FeedOverride::Min;
+            }
             break;
-        case CMD_RAPID_OVR_RESET:
-            system_set_exec_motion_override_flag(EXEC_RAPID_OVR_RESET);
+        case Cmd::FeedOvrFinePlus:
+            sys_rt_f_override += FeedOverride::FineIncrement;
+            if (sys_rt_f_override > FeedOverride::Max) {
+                sys_rt_f_override = FeedOverride::Max;
+            }
             break;
-        case CMD_RAPID_OVR_MEDIUM:
-            system_set_exec_motion_override_flag(EXEC_RAPID_OVR_MEDIUM);
+        case Cmd::FeedOvrFineMinus:
+            sys_rt_f_override -= FeedOverride::FineIncrement;
+            if (sys_rt_f_override < FeedOverride::Min) {
+                sys_rt_f_override = FeedOverride::Min;
+            }
             break;
-        case CMD_RAPID_OVR_LOW:
-            system_set_exec_motion_override_flag(EXEC_RAPID_OVR_LOW);
+        case Cmd::RapidOvrReset:
+            sys_rt_r_override = RapidOverride::Default;
             break;
-        case CMD_SPINDLE_OVR_RESET:
-            system_set_exec_accessory_override_flag(EXEC_SPINDLE_OVR_RESET);
+        case Cmd::RapidOvrMedium:
+            sys_rt_r_override = RapidOverride::Medium;
             break;
-        case CMD_SPINDLE_OVR_COARSE_PLUS:
-            system_set_exec_accessory_override_flag(EXEC_SPINDLE_OVR_COARSE_PLUS);
+        case Cmd::RapidOvrLow:
+            sys_rt_r_override = RapidOverride::Low;
             break;
-        case CMD_SPINDLE_OVR_COARSE_MINUS:
-            system_set_exec_accessory_override_flag(EXEC_SPINDLE_OVR_COARSE_MINUS);
+        case Cmd::RapidOvrExtraLow:
+            sys_rt_r_override = RapidOverride::ExtraLow;
             break;
-        case CMD_SPINDLE_OVR_FINE_PLUS:
-            system_set_exec_accessory_override_flag(EXEC_SPINDLE_OVR_FINE_PLUS);
+        case Cmd::SpindleOvrReset:
+            sys_rt_s_override = SpindleSpeedOverride::Default;
             break;
-        case CMD_SPINDLE_OVR_FINE_MINUS:
-            system_set_exec_accessory_override_flag(EXEC_SPINDLE_OVR_FINE_MINUS);
+        case Cmd::SpindleOvrCoarsePlus:
+            sys_rt_s_override += SpindleSpeedOverride::CoarseIncrement;
+            if (sys_rt_s_override > SpindleSpeedOverride::Max) {
+                sys_rt_s_override = SpindleSpeedOverride::Max;
+            }
             break;
-        case CMD_SPINDLE_OVR_STOP:
-            system_set_exec_accessory_override_flag(EXEC_SPINDLE_OVR_STOP);
+        case Cmd::SpindleOvrCoarseMinus:
+            sys_rt_s_override -= SpindleSpeedOverride::CoarseIncrement;
+            if (sys_rt_s_override < SpindleSpeedOverride::Min) {
+                sys_rt_s_override = SpindleSpeedOverride::Min;
+            }
             break;
-#ifdef COOLANT_FLOOD_PIN
-        case CMD_COOLANT_FLOOD_OVR_TOGGLE:
-            system_set_exec_accessory_override_flag(EXEC_COOLANT_FLOOD_OVR_TOGGLE);
+        case Cmd::SpindleOvrFinePlus:
+            sys_rt_s_override += SpindleSpeedOverride::FineIncrement;
+            if (sys_rt_s_override > SpindleSpeedOverride::Max) {
+                sys_rt_s_override = SpindleSpeedOverride::Max;
+            }
             break;
-#endif
-#ifdef COOLANT_MIST_PIN
-        case CMD_COOLANT_MIST_OVR_TOGGLE:
-            system_set_exec_accessory_override_flag(EXEC_COOLANT_MIST_OVR_TOGGLE);
+        case Cmd::SpindleOvrFineMinus:
+            sys_rt_s_override -= SpindleSpeedOverride::FineIncrement;
+            if (sys_rt_s_override < SpindleSpeedOverride::Min) {
+                sys_rt_s_override = SpindleSpeedOverride::Min;
+            }
+            break;
+        case Cmd::CoolantFloodOvrToggle:
+            sys_rt_exec_accessory_override.bit.coolantFloodOvrToggle = 1;
+            break;
+        case Cmd::CoolantMistOvrToggle:
+            sys_rt_exec_accessory_override.bit.coolantMistOvrToggle = 1;
             break;
-#endif
     }
 }
diff --git a/Grbl_Esp32/src/Serial.h b/Grbl_Esp32/src/Serial.h
index 598c6431..7cca0d94 100644
--- a/Grbl_Esp32/src/Serial.h
+++ b/Grbl_Esp32/src/Serial.h
@@ -23,7 +23,7 @@
 #include "Grbl.h"
 
 #ifndef RX_BUFFER_SIZE
-#    define RX_BUFFER_SIZE 128
+#    define RX_BUFFER_SIZE 256
 #endif
 #ifndef TX_BUFFER_SIZE
 #    ifdef USE_LINE_NUMBERS
@@ -51,6 +51,6 @@ void serial_reset_read_buffer(uint8_t client);
 // Returns the number of bytes available in the RX serial buffer.
 uint8_t serial_get_rx_buffer_available(uint8_t client);
 
-void execute_realtime_command(uint8_t command, uint8_t client);
+void execute_realtime_command(Cmd command, uint8_t client);
 bool any_client_has_data();
 bool is_realtime_command(uint8_t data);
diff --git a/Grbl_Esp32/src/Settings.cpp b/Grbl_Esp32/src/Settings.cpp
index e53c313c..5e356d99 100644
--- a/Grbl_Esp32/src/Settings.cpp
+++ b/Grbl_Esp32/src/Settings.cpp
@@ -129,6 +129,12 @@ Error IntSetting::setStringValue(char* s) {
     return Error::Ok;
 }
 
+const char* IntSetting::getDefaultString() {
+    static char strval[32];
+    sprintf(strval, "%d", _defaultValue);
+    return strval;
+}
+
 const char* IntSetting::getStringValue() {
     static char strval[32];
 
@@ -226,10 +232,8 @@ const char* AxisMaskSetting::getCompatibleValue() {
     return strval;
 }
 
-const char* AxisMaskSetting::getStringValue() {
-    static char strval[32];
+static char* maskToString(uint32_t mask, char* strval) {
     char*       s    = strval;
-    uint32_t    mask = get();
     for (int i = 0; i < MAX_N_AXIS; i++) {
         if (mask & bit(i)) {
             *s++ = "XYZABC"[i];
@@ -239,6 +243,16 @@ const char* AxisMaskSetting::getStringValue() {
     return strval;
 }
 
+const char* AxisMaskSetting::getDefaultString() {
+    static char strval[32];
+    return maskToString(_defaultValue, strval);
+}
+
+const char* AxisMaskSetting::getStringValue() {
+    static char strval[32];
+    return maskToString(get(), strval);
+}
+
 void AxisMaskSetting::addWebui(WebUI::JSONencoder* j) {
     if (getDescription()) {
         j->begin_webui(getName(), getDescription(), "I", getStringValue(), 0, (1 << MAX_N_AXIS) - 1);
@@ -312,6 +326,12 @@ Error FloatSetting::setStringValue(char* s) {
     return Error::Ok;
 }
 
+const char* FloatSetting::getDefaultString() {
+    static char strval[32];
+    (void)sprintf(strval, "%.3f", _defaultValue);
+    return strval;
+}
+
 const char* FloatSetting::getStringValue() {
     static char strval[32];
     (void)sprintf(strval, "%.3f", get());
@@ -395,17 +415,21 @@ Error StringSetting::setStringValue(char* s) {
     return Error::Ok;
 }
 
-const char* StringSetting::getStringValue() {
-    // If the string is a password do not display it
-    if (_checker && (
+static bool isPassword(bool (*_checker)(char*)) {
 #ifdef ENABLE_WIFI
-                        _checker == (bool (*)(char*))WebUI::WiFiConfig::isPasswordValid ||
-#endif
-                        _checker == (bool (*)(char*))WebUI::COMMANDS::isLocalPasswordValid)) {
-        return "******";
-    } else {
-        return _currentValue.c_str();
+    if (_checker == (bool (*)(char*))WebUI::WiFiConfig::isPasswordValid) {
+        return true;
     }
+#endif
+    return _checker == (bool (*)(char*))WebUI::COMMANDS::isLocalPasswordValid;
+}
+
+const char* StringSetting::getDefaultString() {
+    // If the string is a password do not display it
+    return (_checker && isPassword(_checker)) ? "******" : _defaultValue.c_str();
+}
+const char* StringSetting::getStringValue() {
+    return (_checker && isPassword(_checker)) ? "******" : get();
 }
 
 void StringSetting::addWebui(WebUI::JSONencoder* j) {
@@ -485,14 +509,20 @@ Error EnumSetting::setStringValue(char* s) {
     return Error::Ok;
 }
 
-const char* EnumSetting::getStringValue() {
+const char* EnumSetting::enumToString(int8_t value) {
     for (enum_opt_t::iterator it = _options->begin(); it != _options->end(); it++) {
-        if (it->second == _currentValue) {
+        if (it->second == value) {
             return it->first;
         }
     }
     return "???";
 }
+const char* EnumSetting::getDefaultString() {
+    return enumToString(_defaultValue);
+}
+const char* EnumSetting::getStringValue() {
+    return enumToString(get());
+}
 
 void EnumSetting::addWebui(WebUI::JSONencoder* j) {
     if (!getDescription()) {
@@ -557,6 +587,9 @@ Error FlagSetting::setStringValue(char* s) {
     }
     return Error::Ok;
 }
+const char* FlagSetting::getDefaultString() {
+    return _defaultValue ? "On" : "Off";
+}
 const char* FlagSetting::getStringValue() {
     return get() ? "On" : "Off";
 }
@@ -635,10 +668,14 @@ Error IPaddrSetting::setStringValue(char* s) {
     return Error::Ok;
 }
 
+const char* IPaddrSetting::getDefaultString() {
+    static String s;
+    s = IPAddress(_defaultValue).toString();
+    return s.c_str();
+}
 const char* IPaddrSetting::getStringValue() {
     static String s;
-    IPAddress     ipaddr(get());
-    s = ipaddr.toString();
+    s = IPAddress(get()).toString();
     return s.c_str();
 }
 
@@ -681,7 +718,7 @@ bool Coordinates::load() {
 
 void Coordinates::set(float value[MAX_N_AXIS]) {
     memcpy(&_currentValue, value, sizeof(_currentValue));
-#ifdef FORCE_BUFFER_SYNC_DURING_EEPROM_WRITE
+#ifdef FORCE_BUFFER_SYNC_DURING_NVS_WRITE
     protocol_buffer_synchronize();
 #endif
     nvs_set_blob(Setting::_handle, _name, _currentValue, sizeof(_currentValue));
diff --git a/Grbl_Esp32/src/Settings.h b/Grbl_Esp32/src/Settings.h
index 5b91c80e..c513b8b4 100644
--- a/Grbl_Esp32/src/Settings.h
+++ b/Grbl_Esp32/src/Settings.h
@@ -5,6 +5,22 @@
 #include <nvs.h>
 #include "WebUI/ESPResponse.h"
 
+// Initialize the configuration subsystem
+void settings_init();
+
+// Define settings restore bitflags.
+enum SettingsRestore {
+    Defaults = bit(0),
+    Parameters = bit(1),
+    StartupLines = bit(2),
+    // BuildInfo = bit(3), // Obsolete
+    Wifi = bit(4),
+    All = 0xff,
+};
+
+// Restore subsets of settings to default values
+void settings_restore(uint8_t restore_flag);
+
 // Command::List is a linked list of all settings,
 // so common code can enumerate them.
 class Command;
@@ -134,6 +150,7 @@ public:
     Error               setStringValue(String s) { return setStringValue(s.c_str()); }
     virtual const char* getStringValue() = 0;
     virtual const char* getCompatibleValue() { return getStringValue(); }
+    virtual const char* getDefaultString() = 0;
 };
 
 class IntSetting : public Setting {
@@ -173,6 +190,7 @@ public:
     void        addWebui(WebUI::JSONencoder*);
     Error       setStringValue(char* value);
     const char* getStringValue();
+    const char* getDefaultString();
 
     int32_t get() { return _currentValue; }
 };
@@ -202,6 +220,7 @@ public:
     Error       setStringValue(char* value);
     const char* getCompatibleValue();
     const char* getStringValue();
+    const char* getDefaultString();
 
     int32_t get() { return _currentValue; }
 };
@@ -263,6 +282,7 @@ public:
     void        addWebui(WebUI::JSONencoder*) {}
     Error       setStringValue(char* value);
     const char* getStringValue();
+    const char* getDefaultString();
 
     float get() { return _currentValue; }
 };
@@ -296,6 +316,7 @@ public:
     void        addWebui(WebUI::JSONencoder*);
     Error       setStringValue(char* value);
     const char* getStringValue();
+    const char* getDefaultString();
 
     const char* get() { return _currentValue.c_str(); }
 };
@@ -310,6 +331,7 @@ private:
     int8_t                                  _storedValue;
     int8_t                                  _currentValue;
     std::map<const char*, int8_t, cmp_str>* _options;
+    const char*                             enumToString(int8_t value);
 
 public:
     EnumSetting(const char*   description,
@@ -328,6 +350,7 @@ public:
     void        addWebui(WebUI::JSONencoder*);
     Error       setStringValue(char* value);
     const char* getStringValue();
+    const char* getDefaultString();
 
     int8_t get() { return _currentValue; }
 };
@@ -357,6 +380,7 @@ public:
     Error       setStringValue(char* value);
     const char* getCompatibleValue();
     const char* getStringValue();
+    const char* getDefaultString();
 
     bool get() { return _currentValue; }
 };
@@ -388,6 +412,7 @@ public:
     void        addWebui(WebUI::JSONencoder*);
     Error       setStringValue(char* value);
     const char* getStringValue();
+    const char* getDefaultString();
 
     uint32_t get() { return _currentValue; }
 };
diff --git a/Grbl_Esp32/src/SettingsDefinitions.cpp b/Grbl_Esp32/src/SettingsDefinitions.cpp
index 5c74e2f2..ef5f0f5a 100644
--- a/Grbl_Esp32/src/SettingsDefinitions.cpp
+++ b/Grbl_Esp32/src/SettingsDefinitions.cpp
@@ -359,6 +359,7 @@ void make_settings() {
     hard_limits = new FlagSetting(GRBL, WG, "21", "Limits/Hard", DEFAULT_HARD_LIMIT_ENABLE);
     soft_limits = new FlagSetting(GRBL, WG, "20", "Limits/Soft", DEFAULT_SOFT_LIMIT_ENABLE, NULL);
 
+    build_info    = new StringSetting(EXTENDED, WG, NULL, "Firmware/Build", "");
     report_inches = new FlagSetting(GRBL, WG, "13", "Report/Inches", DEFAULT_REPORT_INCHES);
     // TODO Settings - also need to clear, but not set, soft_limits
     arc_tolerance      = new FloatSetting(GRBL, WG, "12", "GCode/ArcTolerance", DEFAULT_ARC_TOLERANCE, 0, 1);
diff --git a/Grbl_Esp32/src/SettingsStorage.cpp b/Grbl_Esp32/src/SettingsStorage.cpp
deleted file mode 100644
index 5c17d8ba..00000000
--- a/Grbl_Esp32/src/SettingsStorage.cpp
+++ /dev/null
@@ -1,76 +0,0 @@
-/*
-  SettingsStorage.cpp - EEPROM configuration handling
-  Part of Grbl
-
-  Copyright (c) 2011-2016 Sungeun K. Jeon for Gnea Research LLC
-  Copyright (c) 2009-2011 Simen Svale Skogsrud
-
-	2018 -	Bart Dring This file was modifed for use on the ESP32
-					CPU. Do not use this with Grbl for atMega328P
-
-  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 "Grbl.h"
-
-// Read selected coordinate data from EEPROM. Updates pointed coord_data value.
-// This is now a compatibility routine that is used to propagate coordinate data
-// in the old EEPROM format to the new tagged NVS format.
-bool old_settings_read_coord_data(uint8_t coord_select, float* coord_data) {
-    uint32_t addr = coord_select * (sizeof(float) * N_AXIS + 1) + EEPROM_ADDR_PARAMETERS;
-    if (!(memcpy_from_eeprom_with_old_checksum((char*)coord_data, addr, sizeof(float) * N_AXIS)) &&
-        !(memcpy_from_eeprom_with_checksum((char*)coord_data, addr, sizeof(float) * MAX_N_AXIS))) {
-        // Reset with default zero vector
-        clear_vector_float(coord_data);
-        // The old code used to rewrite the zeroed data but now that is done
-        // elsewhere, in a different format.
-        return false;
-    }
-    return true;
-}
-
-// Method to store build info into EEPROM
-// NOTE: This function can only be called in IDLE state.
-void settings_store_build_info(const char* line) {
-    // Build info can only be stored when state is IDLE.
-    memcpy_to_eeprom_with_checksum(EEPROM_ADDR_BUILD_INFO, (char*)line, LINE_BUFFER_SIZE);
-}
-
-// Reads startup line from EEPROM. Updated pointed line string data.
-uint8_t settings_read_build_info(char* line) {
-    if (!(memcpy_from_eeprom_with_checksum((char*)line, EEPROM_ADDR_BUILD_INFO, LINE_BUFFER_SIZE))) {
-        // Reset line with default value
-        line[0] = 0;  // Empty line
-        settings_store_build_info(line);
-        return false;
-    }
-    return true;
-}
-
-// Returns step pin mask according to Grbl internal axis indexing.
-uint8_t get_step_pin_mask(uint8_t axis_idx) {
-    // todo clean this up further up stream
-    return bit(axis_idx);
-}
-
-// Returns direction pin mask according to Grbl internal axis indexing.
-uint8_t get_direction_pin_mask(uint8_t axis_idx) {
-    return bit(axis_idx);
-}
-
-// Allow iteration over CoordIndex values
-CoordIndex& operator ++ (CoordIndex& i) {
-    i = static_cast<CoordIndex>(static_cast<uint8_t>(i) + 1);
-    return i;
-}
diff --git a/Grbl_Esp32/src/SettingsStorage.h b/Grbl_Esp32/src/SettingsStorage.h
deleted file mode 100644
index 4df9870a..00000000
--- a/Grbl_Esp32/src/SettingsStorage.h
+++ /dev/null
@@ -1,92 +0,0 @@
-#pragma once
-
-/*
-  SettingsStorage.h - eeprom configuration handling
-  Part of Grbl
-
-  Copyright (c) 2011-2016 Sungeun K. Jeon for Gnea Research LLC
-  Copyright (c) 2009-2011 Simen Svale Skogsrud
-
-	2018 -	Bart Dring This file was modifed for use on the ESP32
-					CPU. Do not use this with Grbl for atMega328P
-
-  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 "Grbl.h"
-
-// Define status reporting boolean enable bit flags in status_report_mask
-#define BITFLAG_RT_STATUS_POSITION_TYPE bit(0)
-#define BITFLAG_RT_STATUS_BUFFER_STATE bit(1)
-
-// Define settings restore bitflags.
-#define SETTINGS_RESTORE_DEFAULTS bit(0)
-#define SETTINGS_RESTORE_PARAMETERS bit(1)
-#define SETTINGS_RESTORE_STARTUP_LINES bit(2)
-#define SETTINGS_RESTORE_BUILD_INFO bit(3)
-#define SETTINGS_RESTORE_WIFI_SETTINGS bit(4)
-#ifndef SETTINGS_RESTORE_ALL
-#    define SETTINGS_RESTORE_ALL 0xFF  // All bitflags
-#endif
-
-// Define EEPROM memory address location values for Grbl settings and parameters
-// NOTE: The Atmega328p has 1KB EEPROM. The upper half is reserved for parameters and
-// the startup script. The lower half contains the global settings and space for future
-// developments.
-const int EEPROM_SIZE            = 1024U;
-const int EEPROM_ADDR_PARAMETERS = 512U;
-const int EEPROM_ADDR_BUILD_INFO = 942U;
-
-// Initialize the configuration subsystem (load settings from EEPROM)
-void settings_init();
-void settings_restore(uint8_t restore_flag);
-void write_global_settings();
-
-uint8_t settings_read_build_info(char* line);
-void    settings_store_build_info(const char* line);
-
-// Reads selected coordinate data from EEPROM
-bool old_settings_read_coord_data(uint8_t coord_select, float* coord_data);
-
-// Returns the step pin mask according to Grbl's internal axis numbering
-uint8_t get_step_pin_mask(uint8_t i);
-
-// Returns the direction pin mask according to Grbl's internal axis numbering
-uint8_t get_direction_pin_mask(uint8_t i);
-
-// Various places in the code access saved coordinate system data
-// by a small integer index according to the values below.
-enum CoordIndex : uint8_t{
-    Begin = 0,
-    G54 = Begin,
-    G55,
-    G56,
-    G57,
-    G58,
-    G59,
-    // To support 9 work coordinate systems it would be necessary to define
-    // the following 3 and modify GCode.cpp to support G59.1, G59.2, G59.3
-    // G59_1,
-    // G59_2,
-    // G59_3,
-    NWCSystems,
-    G28 = NWCSystems,
-    G30,
-    // G92_2,
-    // G92_3,
-    End,
-};
-// Allow iteration over CoordIndex values
-CoordIndex& operator ++ (CoordIndex& i);
-
diff --git a/Grbl_Esp32/src/Spindles/H2ASpindle.md b/Grbl_Esp32/src/Spindles/H2ASpindle.md
index ce5d07da..8edefebe 100644
--- a/Grbl_Esp32/src/Spindles/H2ASpindle.md
+++ b/Grbl_Esp32/src/Spindles/H2ASpindle.md
@@ -75,7 +75,8 @@ like to have. For example:
 If you want to query the current [running] status, that's 
 command 0x3000, and the status is 1 byte, so you might as 
 well add `0001` as parameter. There are exceptions here,
-obviously when writing data to the EEPROM or the speed. 
+such as when writing data to non-volatile storage or
+changing the speed.
 
 I hereby list the most important command sequences, and how
 they work:
diff --git a/Grbl_Esp32/src/Spindles/Laser.cpp b/Grbl_Esp32/src/Spindles/Laser.cpp
index 4500600d..b9fa0c72 100644
--- a/Grbl_Esp32/src/Spindles/Laser.cpp
+++ b/Grbl_Esp32/src/Spindles/Laser.cpp
@@ -31,11 +31,11 @@ namespace Spindles {
     void Laser::config_message() {
         grbl_msg_sendf(CLIENT_SERIAL,
                        MsgLevel::Info,
-                       "Laser spindle on Pin:%s, Freq:%.2fHz, Res:%dbits Laser mode:$32=%d",
+                       "Laser spindle on Pin:%s, Freq:%dHz, Res:%dbits Laser mode:%s",
                        pinName(_output_pin).c_str(),
                        _pwm_freq,
                        _pwm_precision,
-                       isRateAdjusted());  // the current mode
+                       laser_mode->getStringValue());  // the current mode
 
         use_delays = false;  // this will override the value set in Spindle::PWM::init()
     }
diff --git a/Grbl_Esp32/src/Spindles/VFDSpindle.cpp b/Grbl_Esp32/src/Spindles/VFDSpindle.cpp
index 7df1f4ae..3a1305a6 100644
--- a/Grbl_Esp32/src/Spindles/VFDSpindle.cpp
+++ b/Grbl_Esp32/src/Spindles/VFDSpindle.cpp
@@ -185,6 +185,9 @@ namespace Spindles {
                     // Wait a bit before we retry. Set the delay to poll-rate. Not sure
                     // if we should use a different value...
                     vTaskDelay(VFD_RS485_POLL_RATE);
+
+                    static UBaseType_t uxHighWaterMark = 0;
+                    reportTaskStackSize(uxHighWaterMark);
                 }
             }
 
@@ -193,7 +196,7 @@ namespace Spindles {
                     grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Spindle RS485 Unresponsive %d", next_cmd.rx_length);
                     if (next_cmd.critical) {
                         grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Critical Spindle RS485 Unresponsive");
-                        system_set_exec_alarm(ExecAlarm::SpindleControl);
+                        sys_rt_exec_alarm = ExecAlarm::SpindleControl;
                     }
                     unresponsive = true;
                 }
diff --git a/Grbl_Esp32/src/Stepper.cpp b/Grbl_Esp32/src/Stepper.cpp
index 858e15c2..82c4a921 100644
--- a/Grbl_Esp32/src/Stepper.cpp
+++ b/Grbl_Esp32/src/Stepper.cpp
@@ -45,14 +45,10 @@ static st_block_t st_block_buffer[SEGMENT_BUFFER_SIZE - 1];
 // the planner, where the remaining planner block steps still can.
 typedef struct {
     uint16_t n_step;           // Number of step events to be executed for this segment
-    uint16_t cycles_per_tick;  // Step distance traveled per ISR tick, aka step rate.
+    uint16_t isrPeriod;        // Time to next ISR tick, in units of timer ticks
     uint8_t  st_block_index;   // Stepper block data index. Uses this information to execute this segment.
-#ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING
-    uint8_t amass_level;  // Indicates AMASS level for the ISR to execute this segment
-#else
-    uint8_t prescaler;  // Without AMASS, a prescaler is required to adjust for slow timing.
-#endif
-    uint16_t spindle_rpm;  // TODO get rid of this.
+    uint8_t  amass_level;      // AMASS level for the ISR to execute this segment
+    uint16_t spindle_rpm;      // TODO get rid of this.
 } segment_t;
 static segment_t segment_buffer[SEGMENT_BUFFER_SIZE];
 
@@ -60,7 +56,7 @@ static segment_t segment_buffer[SEGMENT_BUFFER_SIZE];
 typedef struct {
     // Used by the bresenham line algorithm
 
-    uint32_t counter_x, counter_y, counter_z, counter_a, counter_b, counter_c;  // Counter variables for the bresenham line tracer
+    uint32_t counter[MAX_N_AXIS];  // Counter variables for the bresenham line tracer
 
 #ifdef STEP_PULSE_DELAY
     uint8_t step_bits;  // Stores out_bits output to complete the step pulse delay
@@ -70,9 +66,7 @@ typedef struct {
     uint8_t step_pulse_time;  // Step pulse reset time after step rise
     uint8_t step_outbits;     // The next stepping-bits to be output
     uint8_t dir_outbits;
-#ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING
     uint32_t steps[MAX_N_AXIS];
-#endif
 
     uint16_t    step_count;        // Steps remaining in line segment motion
     uint8_t     exec_block_index;  // Tracks the current st_block index. Change indicates new block.
@@ -94,7 +88,7 @@ static volatile uint8_t busy;
 static plan_block_t* pl_block;       // Pointer to the planner block being prepped
 static st_block_t*   st_prep_block;  // Pointer to the stepper block data being prepped
 
-// esp32 work around for diable in main loop
+// esp32 work around for disable in main loop
 uint64_t stepper_idle_counter;  // used to count down until time to disable stepper drivers
 bool     stepper_idle;
 
@@ -139,15 +133,6 @@ const char* stepper_names[] = {
     "I2S Steps, Static",
 };
 
-#ifndef DEFAULT_STEPPER
-#    if defined(USE_I2S_STEPS)
-#        define DEFAULT_STEPPER ST_I2S_STREAM
-#    elif defined(USE_RMT_STEPS)
-#        define DEFAULT_STEPPER ST_RMT
-#    else
-#        define DEFAULT_STEPPER ST_TIMED
-#    endif
-#endif
 stepper_id_t current_stepper = DEFAULT_STEPPER;
 
 /* "The Stepper Driver Interrupt" - This timer interrupt is the workhorse of Grbl. Grbl employs
@@ -205,9 +190,6 @@ stepper_id_t current_stepper = DEFAULT_STEPPER;
 
 
 */
-#ifdef USE_RMT_STEPS
-inline IRAM_ATTR static void stepperRMT_Outputs();
-#endif
 
 static void stepper_pulse_func();
 
@@ -239,17 +221,15 @@ void IRAM_ATTR onStepperDriverTimer(
 static void stepper_pulse_func() {
     auto n_axis = number_axis->get();
 
-    motors_set_direction_pins(st.dir_outbits);
-#ifdef USE_RMT_STEPS
-    stepperRMT_Outputs();
-#else
-    set_stepper_pins_on(st.step_outbits);
-    uint64_t step_pulse_start_time = esp_timer_get_time();
-#endif
-
-    // some motor objects, like unipolar, handle steps themselves
     motors_step(st.step_outbits, st.dir_outbits);
 
+    // If we are using GPIO stepping as opposed to RMT, record the
+    // time that we turned on the step pins so we can turn them off
+    // at the end of this routine without incurring another interrupt.
+    // This is unnecessary with RMT and I2S stepping since both of
+    // those methods time the turn off automatically.
+    uint64_t step_pulse_start_time = esp_timer_get_time();
+
     // If there is no step segment, attempt to pop one from the stepper buffer
     if (st.exec_segment == NULL) {
         // Anything in the buffer? If so, load and initialize next step segment.
@@ -257,7 +237,7 @@ static void stepper_pulse_func() {
             // Initialize new step segment and load number of steps to execute
             st.exec_segment = &segment_buffer[segment_buffer_tail];
             // Initialize step segment timing per step and load number of steps to execute.
-            Stepper_Timer_WritePeriod(st.exec_segment->cycles_per_tick);
+            Stepper_Timer_WritePeriod(st.exec_segment->isrPeriod);
             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.
@@ -265,26 +245,17 @@ static void stepper_pulse_func() {
                 st.exec_block_index = st.exec_segment->st_block_index;
                 st.exec_block       = &st_block_buffer[st.exec_block_index];
                 // Initialize Bresenham line and distance counters
-                st.counter_x = st.counter_y = st.counter_z = (st.exec_block->step_event_count >> 1);
+                // XXX the original code only inits X, Y, Z here, instead of n_axis.  Is that correct?
+                for (int axis = 0; axis < 3; axis++) {
+                    st.counter[axis] = (st.exec_block->step_event_count >> 1);
+                }
                 // TODO ABC
             }
-            st.dir_outbits = st.exec_block->direction_bits ^ dir_invert_mask->get();
-#ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING
-            // With AMASS enabled, adjust Bresenham axis increment counters according to AMASS level.
-            st.steps[X_AXIS] = st.exec_block->steps[X_AXIS] >> st.exec_segment->amass_level;
-            st.steps[Y_AXIS] = st.exec_block->steps[Y_AXIS] >> st.exec_segment->amass_level;
-            st.steps[Z_AXIS] = st.exec_block->steps[Z_AXIS] >> st.exec_segment->amass_level;
-
-            if (n_axis > A_AXIS) {
-                st.steps[A_AXIS] = st.exec_block->steps[A_AXIS] >> st.exec_segment->amass_level;
-                if (n_axis > B_AXIS) {
-                    st.steps[B_AXIS] = st.exec_block->steps[B_AXIS] >> st.exec_segment->amass_level;
-                    if (n_axis > C_AXIS) {
-                        st.steps[C_AXIS] = st.exec_block->steps[C_AXIS] >> st.exec_segment->amass_level;
-                    }
-                }
+            st.dir_outbits = st.exec_block->direction_bits;
+            // Adjust Bresenham axis increment counters according to AMASS level.
+            for (int axis = 0; axis < n_axis; axis++) {
+                st.steps[axis] = st.exec_block->steps[axis] >> st.exec_segment->amass_level;
             }
-#endif
             // Set real-time spindle output as segment is loaded, just prior to the first step.
             spindle->set_rpm(st.exec_segment->spindle_rpm);
         } else {
@@ -301,102 +272,22 @@ static void stepper_pulse_func() {
         }
     }
     // Check probing state.
-    if (sys_probe_state == PROBE_ACTIVE) {
+    if (sys_probe_state == Probe::Active) {
         probe_state_monitor();
     }
     // Reset step out bits.
     st.step_outbits = 0;
-    // Execute step displacement profile by Bresenham line algorithm
-#ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING
-    st.counter_x += st.steps[X_AXIS];
-#else
-    st.counter_x += st.exec_block->steps[X_AXIS];
-#endif
-    if (st.counter_x > st.exec_block->step_event_count) {
-        st.step_outbits |= bit(X_AXIS);
-        st.counter_x -= st.exec_block->step_event_count;
-        if (st.exec_block->direction_bits & bit(X_AXIS)) {
-            sys_position[X_AXIS]--;
-        } else {
-            sys_position[X_AXIS]++;
-        }
-    }
-#ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING
-    st.counter_y += st.steps[Y_AXIS];
-#else
-    st.counter_y += st.exec_block->steps[Y_AXIS];
-#endif
-    if (st.counter_y > st.exec_block->step_event_count) {
-        st.step_outbits |= bit(Y_AXIS);
-        st.counter_y -= st.exec_block->step_event_count;
-        if (st.exec_block->direction_bits & bit(Y_AXIS)) {
-            sys_position[Y_AXIS]--;
-        } else {
-            sys_position[Y_AXIS]++;
-        }
-    }
-#ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING
-    st.counter_z += st.steps[Z_AXIS];
-#else
-    st.counter_z += st.exec_block->steps[Z_AXIS];
-#endif
-    if (st.counter_z > st.exec_block->step_event_count) {
-        st.step_outbits |= bit(Z_AXIS);
-        st.counter_z -= st.exec_block->step_event_count;
-        if (st.exec_block->direction_bits & bit(Z_AXIS)) {
-            sys_position[Z_AXIS]--;
-        } else {
-            sys_position[Z_AXIS]++;
-        }
-    }
 
-    if (n_axis > A_AXIS) {
-#ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING
-        st.counter_a += st.steps[A_AXIS];
-#else
-        st.counter_a += st.exec_block->steps[A_AXIS];
-#endif
-        if (st.counter_a > st.exec_block->step_event_count) {
-            st.step_outbits |= bit(A_AXIS);
-            st.counter_a -= st.exec_block->step_event_count;
-            if (st.exec_block->direction_bits & bit(A_AXIS)) {
-                sys_position[A_AXIS]--;
+    for (int axis = 0; axis < n_axis; axis++) {
+        // Execute step displacement profile by Bresenham line algorithm
+        st.counter[axis] += st.steps[axis];
+        if (st.counter[axis] > st.exec_block->step_event_count) {
+            st.step_outbits |= bit(axis);
+            st.counter[axis] -= st.exec_block->step_event_count;
+            if (st.exec_block->direction_bits & bit(axis)) {
+                sys_position[axis]--;
             } else {
-                sys_position[A_AXIS]++;
-            }
-        }
-
-        if (n_axis > B_AXIS) {
-#ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING
-            st.counter_b += st.steps[B_AXIS];
-#else
-            st.counter_b += st.exec_block->steps[B_AXIS];
-#endif
-            if (st.counter_b > st.exec_block->step_event_count) {
-                st.step_outbits |= bit(B_AXIS);
-                st.counter_b -= st.exec_block->step_event_count;
-                if (st.exec_block->direction_bits & bit(B_AXIS)) {
-                    sys_position[B_AXIS]--;
-                } else {
-                    sys_position[B_AXIS]++;
-                }
-            }
-
-            if (n_axis > C_AXIS) {
-#ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING
-                st.counter_c += st.steps[C_AXIS];
-#else
-                st.counter_c += st.exec_block->steps[C_AXIS];
-#endif
-                if (st.counter_c > st.exec_block->step_event_count) {
-                    st.step_outbits |= bit(C_AXIS);
-                    st.counter_c -= st.exec_block->step_event_count;
-                    if (st.exec_block->direction_bits & bit(C_AXIS)) {
-                        sys_position[C_AXIS]--;
-                    } else {
-                        sys_position[C_AXIS]++;
-                    }
-                }
+                sys_position[axis]++;
             }
         }
     }
@@ -414,26 +305,23 @@ static void stepper_pulse_func() {
         }
     }
 
-#ifdef USE_I2S_STEPS
-    if (current_stepper == ST_I2S_STREAM) {
-        //
-        // Generate pulse (at least one pulse)
-        // The pulse resolution is limited by I2S_OUT_USEC_PER_PULSE
-        //
-        i2s_out_push_sample(pulse_microseconds->get() / I2S_OUT_USEC_PER_PULSE);
-        set_stepper_pins_on(0);  // turn all off
-        return;
+    switch (current_stepper) {
+        case ST_I2S_STREAM:
+            // Generate the number of pulses needed to span pulse_microseconds
+            i2s_out_push_sample(pulse_microseconds->get());
+            motors_unstep();
+            break;
+        case ST_I2S_STATIC:
+        case ST_TIMED:
+            // wait for step pulse time to complete...some time expired during code above
+            while (esp_timer_get_time() - step_pulse_start_time < pulse_microseconds->get()) {
+                NOP();  // spin here until time to turn off step
+            }
+            motors_unstep();
+            break;
+        case ST_RMT:
+            break;
     }
-#endif
-#ifdef USE_RMT_STEPS
-    return;
-#else
-    // wait for step pulse time to complete...some of it should have expired during code above
-    while (esp_timer_get_time() - step_pulse_start_time < pulse_microseconds->get()) {
-        NOP();  // spin here until time to turn off step
-    }
-    set_stepper_pins_on(0);  // turn all off
-#endif
 }
 
 void stepper_init() {
@@ -457,11 +345,10 @@ void stepper_switch(stepper_id_t new_stepper) {
 #ifdef USE_I2S_STEPS
     if (current_stepper == ST_I2S_STREAM) {
         if (i2s_out_get_pulser_status() != PASSTHROUGH) {
-            // This switching function should not be called during streaming.
-            // However, if it is called, it will stop streaming.
-            grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Warning, "Stop the I2S streaming and switch to the passthrough mode.");
+            // Called during streaming. Stop streaming.
+            grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Debug, "Stop the I2S streaming and switch to the passthrough mode.");
             i2s_out_set_passthrough();
-            i2s_out_delay();
+            i2s_out_delay();  // Wait for a change in mode.
         }
     }
 #endif
@@ -479,7 +366,7 @@ void st_wake_up() {
     // Step pulse delay handling is not require with ESP32...the RMT function does it.
 #else  // Normal operation
     // Set step pulse time. Ad hoc computation from oscilloscope. Uses two's complement.
-    st.step_pulse_time = -(((pulse_microseconds->get() - 2) * TICKS_PER_MICROSECOND) >> 3);
+    st.step_pulse_time = -(((pulse_microseconds->get() - 2) * ticksPerMicrosecond) >> 3);
 #endif
     // Enable Stepper Driver Interrupt
     Stepper_Timer_Start();
@@ -507,202 +394,10 @@ void st_reset() {
     segment_next_head   = 1;
     busy                = false;
     st.step_outbits     = 0;
-    st.dir_outbits      = dir_invert_mask->get();  // Initialize direction bits to default.
+    st.dir_outbits      = 0;  // Initialize direction bits to default.
     // TODO do we need to turn step pins off?
 }
 
-void set_stepper_pins_on(uint8_t onMask) {
-    onMask ^= step_invert_mask->get();  // invert pins as required by invert mask
-#ifdef X_STEP_PIN
-#    ifndef X2_STEP_PIN  // if not a ganged axis
-    digitalWrite(X_STEP_PIN, (onMask & bit(X_AXIS)));
-#    else  // is a ganged axis
-    if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::A)) {
-        digitalWrite(X_STEP_PIN, (onMask & bit(X_AXIS)));
-    }
-    if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::B)) {
-        digitalWrite(X2_STEP_PIN, (onMask & bit(X_AXIS)));
-    }
-#    endif
-#endif
-#ifdef Y_STEP_PIN
-#    ifndef Y2_STEP_PIN  // if not a ganged axis
-    digitalWrite(Y_STEP_PIN, (onMask & bit(Y_AXIS)));
-#    else  // is a ganged axis
-    if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::A)) {
-        digitalWrite(Y_STEP_PIN, (onMask & bit(Y_AXIS)));
-    }
-    if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::B)) {
-        digitalWrite(Y2_STEP_PIN, (onMask & bit(Y_AXIS)));
-    }
-#    endif
-#endif
-
-#ifdef Z_STEP_PIN
-#    ifndef Z2_STEP_PIN  // if not a ganged axis
-    digitalWrite(Z_STEP_PIN, (onMask & bit(Z_AXIS)));
-#    else  // is a ganged axis
-    if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::A)) {
-        digitalWrite(Z_STEP_PIN, (onMask & bit(Z_AXIS)));
-    }
-    if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::B)) {
-        digitalWrite(Z2_STEP_PIN, (onMask & bit(Z_AXIS)));
-    }
-#    endif
-#endif
-
-#ifdef A_STEP_PIN
-#    ifndef A2_STEP_PIN  // if not a ganged axis
-    digitalWrite(A_STEP_PIN, (onMask & bit(A_AXIS)));
-#    else  // is a ganged axis
-    if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::A)) {
-        digitalWrite(A_STEP_PIN, (onMask & bit(A_AXIS)));
-    }
-    if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::B)) {
-        digitalWrite(A2_STEP_PIN, (onMask & bit(A_AXIS)));
-    }
-#    endif
-#endif
-
-#ifdef B_STEP_PIN
-#    ifndef B2_STEP_PIN  // if not a ganged axis
-    digitalWrite(B_STEP_PIN, (onMask & bit(B_AXIS)));
-#    else  // is a ganged axis
-    if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::A)) {
-        digitalWrite(B_STEP_PIN, (onMask & bit(B_AXIS)));
-    }
-    if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::B)) {
-        digitalWrite(B2_STEP_PIN, (onMask & bit(B_AXIS)));
-    }
-#    endif
-#endif
-
-#ifdef C_STEP_PIN
-#    ifndef C2_STEP_PIN  // if not a ganged axis
-    digitalWrite(C_STEP_PIN, (onMask & bit(C_AXIS)));
-#    else  // is a ganged axis
-    if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::A)) {
-        digitalWrite(C_STEP_PIN, (onMask & bit(C_AXIS)));
-    }
-    if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::B)) {
-        digitalWrite(C2_STEP_PIN, (onMask & bit(C_AXIS)));
-    }
-#    endif
-#endif
-}
-//#endif
-
-#ifdef USE_RMT_STEPS
-inline IRAM_ATTR static void stepperRMT_Outputs() {
-#    ifdef X_STEP_PIN
-    if (st.step_outbits & bit(X_AXIS)) {
-#        ifndef X2_STEP_PIN  // if not a ganged axis
-        RMT.conf_ch[rmt_chan_num[X_AXIS][PRIMARY_MOTOR]].conf1.mem_rd_rst = 1;
-        RMT.conf_ch[rmt_chan_num[X_AXIS][PRIMARY_MOTOR]].conf1.tx_start   = 1;
-#        else  // it is a ganged axis
-        if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::A)) {
-            RMT.conf_ch[rmt_chan_num[X_AXIS][PRIMARY_MOTOR]].conf1.mem_rd_rst = 1;
-            RMT.conf_ch[rmt_chan_num[X_AXIS][PRIMARY_MOTOR]].conf1.tx_start   = 1;
-        }
-        if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::B)) {
-            RMT.conf_ch[rmt_chan_num[X_AXIS][GANGED_MOTOR]].conf1.mem_rd_rst = 1;
-            RMT.conf_ch[rmt_chan_num[X_AXIS][GANGED_MOTOR]].conf1.tx_start   = 1;
-        }
-#        endif
-    }
-#    endif
-#    ifdef Y_STEP_PIN
-    if (st.step_outbits & bit(Y_AXIS)) {
-#        ifndef Y2_STEP_PIN  // if not a ganged axis
-        RMT.conf_ch[rmt_chan_num[Y_AXIS][PRIMARY_MOTOR]].conf1.mem_rd_rst = 1;
-        RMT.conf_ch[rmt_chan_num[Y_AXIS][PRIMARY_MOTOR]].conf1.tx_start   = 1;
-#        else  // it is a ganged axis
-        if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::A)) {
-            RMT.conf_ch[rmt_chan_num[Y_AXIS][PRIMARY_MOTOR]].conf1.mem_rd_rst = 1;
-            RMT.conf_ch[rmt_chan_num[Y_AXIS][PRIMARY_MOTOR]].conf1.tx_start   = 1;
-        }
-        if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::B)) {
-            RMT.conf_ch[rmt_chan_num[Y_AXIS][GANGED_MOTOR]].conf1.mem_rd_rst = 1;
-            RMT.conf_ch[rmt_chan_num[Y_AXIS][GANGED_MOTOR]].conf1.tx_start   = 1;
-        }
-#        endif
-    }
-#    endif
-
-#    ifdef Z_STEP_PIN
-    if (st.step_outbits & bit(Z_AXIS)) {
-#        ifndef Z2_STEP_PIN  // if not a ganged axis
-        RMT.conf_ch[rmt_chan_num[Z_AXIS][PRIMARY_MOTOR]].conf1.mem_rd_rst = 1;
-        RMT.conf_ch[rmt_chan_num[Z_AXIS][PRIMARY_MOTOR]].conf1.tx_start   = 1;
-#        else  // it is a ganged axis
-        if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::A)) {
-            RMT.conf_ch[rmt_chan_num[Z_AXIS][PRIMARY_MOTOR]].conf1.mem_rd_rst = 1;
-            RMT.conf_ch[rmt_chan_num[Z_AXIS][PRIMARY_MOTOR]].conf1.tx_start   = 1;
-        }
-        if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::B)) {
-            RMT.conf_ch[rmt_chan_num[Z_AXIS][GANGED_MOTOR]].conf1.mem_rd_rst = 1;
-            RMT.conf_ch[rmt_chan_num[Z_AXIS][GANGED_MOTOR]].conf1.tx_start   = 1;
-        }
-#        endif
-    }
-#    endif
-
-#    ifdef A_STEP_PIN
-    if (st.step_outbits & bit(A_AXIS)) {
-#        ifndef A2_STEP_PIN  // if not a ganged axis
-        RMT.conf_ch[rmt_chan_num[A_AXIS][PRIMARY_MOTOR]].conf1.mem_rd_rst = 1;
-        RMT.conf_ch[rmt_chan_num[A_AXIS][PRIMARY_MOTOR]].conf1.tx_start   = 1;
-#        else  // it is a ganged axis
-        if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::A)) {
-            RMT.conf_ch[rmt_chan_num[A_AXIS][PRIMARY_MOTOR]].conf1.mem_rd_rst = 1;
-            RMT.conf_ch[rmt_chan_num[A_AXIS][PRIMARY_MOTOR]].conf1.tx_start   = 1;
-        }
-        if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::B)) {
-            RMT.conf_ch[rmt_chan_num[A_AXIS][GANGED_MOTOR]].conf1.mem_rd_rst = 1;
-            RMT.conf_ch[rmt_chan_num[A_AXIS][GANGED_MOTOR]].conf1.tx_start   = 1;
-        }
-#        endif
-    }
-#    endif
-
-#    ifdef B_STEP_PIN
-    if (st.step_outbits & bit(B_AXIS)) {
-#        ifndef Z2_STEP_PIN  // if not a ganged axis
-        RMT.conf_ch[rmt_chan_num[B_AXIS][PRIMARY_MOTOR]].conf1.mem_rd_rst = 1;
-        RMT.conf_ch[rmt_chan_num[B_AXIS][PRIMARY_MOTOR]].conf1.tx_start   = 1;
-#        else  // it is a ganged axis
-        if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::A)) {
-            RMT.conf_ch[rmt_chan_num[B_AXIS][PRIMARY_MOTOR]].conf1.mem_rd_rst = 1;
-            RMT.conf_ch[rmt_chan_num[B_AXIS][PRIMARY_MOTOR]].conf1.tx_start   = 1;
-        }
-        if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::B)) {
-            RMT.conf_ch[rmt_chan_num[B_AXIS][GANGED_MOTOR]].conf1.mem_rd_rst = 1;
-            RMT.conf_ch[rmt_chan_num[B_AXIS][GANGED_MOTOR]].conf1.tx_start   = 1;
-        }
-#        endif
-    }
-#    endif
-
-#    ifdef C_STEP_PIN
-    if (st.step_outbits & bit(C_AXIS)) {
-#        ifndef Z2_STEP_PIN  // if not a ganged axis
-        RMT.conf_ch[rmt_chan_num[C_AXIS][PRIMARY_MOTOR]].conf1.mem_rd_rst = 1;
-        RMT.conf_ch[rmt_chan_num[C_AXIS][PRIMARY_MOTOR]].conf1.tx_start   = 1;
-#        else  // it is a ganged axis
-        if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::A)) {
-            RMT.conf_ch[rmt_chan_num[C_AXIS][PRIMARY_MOTOR]].conf1.mem_rd_rst = 1;
-            RMT.conf_ch[rmt_chan_num[C_AXIS][PRIMARY_MOTOR]].conf1.tx_start   = 1;
-        }
-        if ((ganged_mode == SquaringMode::Dual) || (ganged_mode == SquaringMode::B)) {
-            RMT.conf_ch[rmt_chan_num[C_AXIS][GANGED_MOTOR]].conf1.mem_rd_rst = 1;
-            RMT.conf_ch[rmt_chan_num[C_AXIS][GANGED_MOTOR]].conf1.tx_start   = 1;
-        }
-#        endif
-    }
-#    endif
-}
-#endif
-
 // Stepper shutdown
 void st_go_idle() {
     // Disable Stepper Driver Interrupt. Allow Stepper Port Reset Interrupt to finish, if active.
@@ -726,7 +421,7 @@ void st_go_idle() {
         motors_set_disable(false);
     }
 
-    set_stepper_pins_on(0);
+    motors_unstep();
     st.step_outbits = 0;
 }
 
@@ -796,7 +491,7 @@ static uint8_t st_next_block_index(uint8_t block_index) {
 */
 void st_prep_buffer() {
     // Block step prep buffer, while in a suspend state and there is no suspend motion to execute.
-    if (bit_istrue(sys.step_control, STEP_CONTROL_END_MOTION)) {
+    if (sys.step_control.endMotion) {
         return;
     }
 
@@ -804,7 +499,7 @@ void st_prep_buffer() {
         // Determine if we need to load a new planner block or if the block needs to be recomputed.
         if (pl_block == NULL) {
             // Query planner for a queued block
-            if (sys.step_control & STEP_CONTROL_EXECUTE_SYS_MOTION) {
+            if (sys.step_control.executeSysMotion) {
                 pl_block = plan_get_system_motion_block();
             } else {
                 pl_block = plan_get_current_block();
@@ -835,26 +530,21 @@ void st_prep_buffer() {
                 st_prep_block->direction_bits = pl_block->direction_bits;
                 uint8_t idx;
                 auto    n_axis = number_axis->get();
-#ifndef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING
-                for (idx = 0; idx < n_axis; idx++) {
-                    st_prep_block->steps[idx] = pl_block->steps[idx];
-                }
-                st_prep_block->step_event_count = pl_block->step_event_count;
-#else
-                // With AMASS enabled, simply bit-shift multiply all Bresenham data by the max AMASS
-                // level, such that we never divide beyond the original data anywhere in the algorithm.
+
+                // Bit-shift multiply all Bresenham data by the max AMASS level so that
+                // we never divide beyond the original data anywhere in the algorithm.
                 // If the original data is divided, we can lose a step from integer roundoff.
                 for (idx = 0; idx < n_axis; idx++) {
-                    st_prep_block->steps[idx] = pl_block->steps[idx] << MAX_AMASS_LEVEL;
+                    st_prep_block->steps[idx] = pl_block->steps[idx] << maxAmassLevel;
                 }
-                st_prep_block->step_event_count = pl_block->step_event_count << MAX_AMASS_LEVEL;
-#endif
+                st_prep_block->step_event_count = pl_block->step_event_count << maxAmassLevel;
+
                 // Initialize segment buffer data for generating the segments.
                 prep.steps_remaining  = (float)pl_block->step_event_count;
                 prep.step_per_mm      = prep.steps_remaining / pl_block->millimeters;
                 prep.req_mm_increment = REQ_MM_INCREMENT_SCALAR / prep.step_per_mm;
                 prep.dt_remainder     = 0.0;  // Reset for new segment block
-                if ((sys.step_control & STEP_CONTROL_EXECUTE_HOLD) || prep.recalculate_flag.decelOverride) {
+                if ((sys.step_control.executeHold) || prep.recalculate_flag.decelOverride) {
                     // New block loaded mid-hold. Override planner block entry speed to enforce deceleration.
                     prep.current_speed                  = prep.exit_speed;
                     pl_block->entry_speed_sqr           = prep.exit_speed * prep.exit_speed;
@@ -879,7 +569,7 @@ void st_prep_buffer() {
             */
             prep.mm_complete  = 0.0;  // Default velocity profile complete at 0.0mm from end of block.
             float inv_2_accel = 0.5 / pl_block->acceleration;
-            if (sys.step_control & STEP_CONTROL_EXECUTE_HOLD) {  // [Forced Deceleration to Zero Velocity]
+            if (sys.step_control.executeHold) {  // [Forced Deceleration to Zero Velocity]
                 // Compute velocity profile parameters for a feed hold in-progress. This profile overrides
                 // the planner block profile, enforcing a deceleration to zero speed.
                 prep.ramp_type = RAMP_DECEL;
@@ -898,7 +588,7 @@ void st_prep_buffer() {
                 prep.accelerate_until = pl_block->millimeters;
                 float exit_speed_sqr;
                 float nominal_speed;
-                if (sys.step_control & STEP_CONTROL_EXECUTE_SYS_MOTION) {
+                if (sys.step_control.executeSysMotion) {
                     prep.exit_speed = exit_speed_sqr = 0.0;  // Enforce stop at end of system motion.
                 } else {
                     exit_speed_sqr  = plan_get_exec_block_exit_speed_sqr();
@@ -956,7 +646,7 @@ void st_prep_buffer() {
                 }
             }
 
-            bit_true(sys.step_control, STEP_CONTROL_UPDATE_SPINDLE_RPM);  // Force update whenever updating block.
+            sys.step_control.updateSpindleRpm = true;  // Force update whenever updating block.
         }
 
         // Initialize new segment
@@ -1075,7 +765,7 @@ void st_prep_buffer() {
         /* -----------------------------------------------------------------------------------
           Compute spindle speed PWM output for step segment
         */
-        if (st_prep_block->is_pwm_rate_adjusted || (sys.step_control & STEP_CONTROL_UPDATE_SPINDLE_RPM)) {
+        if (st_prep_block->is_pwm_rate_adjusted || sys.step_control.updateSpindleRpm) {
             if (pl_block->spindle != SpindleState::Disable) {
                 float rpm = pl_block->spindle_speed;
                 // NOTE: Feed and rapid overrides are independent of PWM value and do not alter laser power/rate.
@@ -1092,7 +782,7 @@ void st_prep_buffer() {
                 sys.spindle_speed        = 0.0;
                 prep.current_spindle_rpm = 0.0;
             }
-            bit_false(sys.step_control, STEP_CONTROL_UPDATE_SPINDLE_RPM);
+            sys.step_control.updateSpindleRpm = false;
         }
         prep_segment->spindle_rpm = prep.current_spindle_rpm;  // Reload segment PWM value
 
@@ -1113,10 +803,10 @@ void st_prep_buffer() {
 
         // Bail if we are at the end of a feed hold and don't have a step to execute.
         if (prep_segment->n_step == 0) {
-            if (sys.step_control & STEP_CONTROL_EXECUTE_HOLD) {
+            if (sys.step_control.executeHold) {
                 // Less than one step to decelerate to zero speed, but already very close. AMASS
                 // requires full steps to execute. So, just bail.
-                bit_true(sys.step_control, STEP_CONTROL_END_MOTION);
+                sys.step_control.endMotion = true;
 #ifdef PARKING_ENABLE
                 if (!(prep.recalculate_flag.parking)) {
                     prep.recalculate_flag.holdPartialBlock = 1;
@@ -1136,49 +826,28 @@ void st_prep_buffer() {
         // outputs the exact acceleration and velocity profiles as computed by the planner.
 
         dt += prep.dt_remainder;                                               // Apply previous segment partial step execute time
+        // dt is in minutes so inv_rate is in minutes
         float inv_rate = dt / (last_n_steps_remaining - step_dist_remaining);  // Compute adjusted step rate inverse
 
         // Compute CPU cycles per step for the prepped segment.
-        uint32_t cycles = ceil((TICKS_PER_MICROSECOND * 1000000 * 60) * inv_rate);  // (cycles/step)
+        // fStepperTimer is in units of timerTicks/sec, so the dimensional analysis is
+        // timerTicks/sec * 60 sec/minute * minutes = timerTicks
+        uint32_t timerTicks = ceil((fStepperTimer * 60) * inv_rate);  // (timerTicks/step)
+        int level;
 
-#ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING
         // Compute step timing and multi-axis smoothing level.
-        // NOTE: AMASS overdrives the timer with each level, so only one prescalar is required.
-        if (cycles < AMASS_LEVEL1) {
-            prep_segment->amass_level = 0;
-        } else {
-            if (cycles < AMASS_LEVEL2) {
-                prep_segment->amass_level = 1;
-            } else if (cycles < AMASS_LEVEL3) {
-                prep_segment->amass_level = 2;
-            } else {
-                prep_segment->amass_level = 3;
+        for (level = 0; level < maxAmassLevel; level++) {
+            if (timerTicks < amassThreshold) {
+                break;
             }
-            cycles >>= prep_segment->amass_level;
-            prep_segment->n_step <<= prep_segment->amass_level;
+            timerTicks >>= 1;
         }
-        if (cycles < (1UL << 16)) {
-            prep_segment->cycles_per_tick = cycles;  // < 65536 (4.1ms @ 16MHz)
-        } else {
-            prep_segment->cycles_per_tick = 0xffff;  // Just set the slowest speed possible.
-        }
-#else
-        // Compute step timing and timer prescalar for normal step generation.
-        if (cycles < (1UL << 16)) {             // < 65536  (4.1ms @ 16MHz)
-            prep_segment->prescaler       = 1;  // prescaler: 0
-            prep_segment->cycles_per_tick = cycles;
-        } else if (cycles < (1UL << 19)) {      // < 524288 (32.8ms@16MHz)
-            prep_segment->prescaler       = 2;  // prescaler: 8
-            prep_segment->cycles_per_tick = cycles >> 3;
-        } else {
-            prep_segment->prescaler = 3;  // prescaler: 64
-            if (cycles < (1UL << 22)) {   // < 4194304 (262ms@16MHz)
-                prep_segment->cycles_per_tick = cycles >> 6;
-            } else {  // Just set the slowest speed possible. (Around 4 step/sec.)
-                prep_segment->cycles_per_tick = 0xffff;
-            }
-        }
-#endif
+        prep_segment->amass_level = level;
+        prep_segment->n_step <<= level;
+        // isrPeriod is stored as 16 bits, so limit timerTicks to the
+        // largest value that will fit in a uint16_t.
+        prep_segment->isrPeriod = timerTicks > 0xffff ? 0xffff : timerTicks;
+
         // Segment complete! Increment segment buffer indices, so stepper ISR can immediately execute it.
         segment_buffer_head = segment_next_head;
         if (++segment_next_head == SEGMENT_BUFFER_SIZE) {
@@ -1195,7 +864,7 @@ void st_prep_buffer() {
                 // Reset prep parameters for resuming and then bail. Allow the stepper ISR to complete
                 // the segment queue, where realtime protocol will set new state upon receiving the
                 // cycle stop flag from the ISR. Prep_segment is blocked until then.
-                bit_true(sys.step_control, STEP_CONTROL_END_MOTION);
+                sys.step_control.endMotion = true;
 #ifdef PARKING_ENABLE
                 if (!(prep.recalculate_flag.parking)) {
                     prep.recalculate_flag.holdPartialBlock = 1;
@@ -1204,8 +873,8 @@ void st_prep_buffer() {
                 return;  // Bail!
             } else {     // End of planner block
                 // The planner block is complete. All steps are set to be executed in the segment buffer.
-                if (sys.step_control & STEP_CONTROL_EXECUTE_SYS_MOTION) {
-                    bit_true(sys.step_control, STEP_CONTROL_END_MOTION);
+                if (sys.step_control.executeSysMotion) {
+                    sys.step_control.endMotion = true;
                     return;
                 }
                 pl_block = NULL;  // Set pointer to indicate check and load next planner block.
@@ -1232,21 +901,23 @@ float st_get_realtime_rate() {
     }
 }
 
-void IRAM_ATTR Stepper_Timer_WritePeriod(uint64_t alarm_val) {
+// The argument is in units of ticks of the timer that generates ISRs
+void IRAM_ATTR Stepper_Timer_WritePeriod(uint16_t timerTicks) {
     if (current_stepper == ST_I2S_STREAM) {
 #ifdef USE_I2S_STEPS
-        // 1 tick = F_TIMERS / F_STEPPER_TIMER
+        // 1 tick = fTimers / fStepperTimer
         // Pulse ISR is called for each tick of alarm_val.
-        i2s_out_set_pulse_period(alarm_val);
+        // The argument to i2s_out_set_pulse_period is in units of microseconds
+        i2s_out_set_pulse_period(((uint32_t)timerTicks) / ticksPerMicrosecond);
 #endif
     } else {
-        timer_set_alarm_value(STEP_TIMER_GROUP, STEP_TIMER_INDEX, alarm_val);
+        timer_set_alarm_value(STEP_TIMER_GROUP, STEP_TIMER_INDEX, (uint64_t)timerTicks);
     }
 }
 
 void IRAM_ATTR Stepper_Timer_Init() {
     timer_config_t config;
-    config.divider     = F_TIMERS / F_STEPPER_TIMER;
+    config.divider     = fTimers / fStepperTimer;
     config.counter_dir = TIMER_COUNT_UP;
     config.counter_en  = TIMER_PAUSE;
     config.alarm_en    = TIMER_ALARM_EN;
diff --git a/Grbl_Esp32/src/Stepper.h b/Grbl_Esp32/src/Stepper.h
index b7f36265..5bd04482 100644
--- a/Grbl_Esp32/src/Stepper.h
+++ b/Grbl_Esp32/src/Stepper.h
@@ -46,25 +46,23 @@ struct PrepFlag {
     uint8_t decelOverride : 1;
 };
 
+// fStepperTimer should be an integer divisor of the bus speed, i.e. of fTimers
+const uint32_t fStepperTimer = 20000000; // frequency of step pulse timer
+const int ticksPerMicrosecond = fStepperTimer / 1000000;
+
 // Define Adaptive Multi-Axis Step-Smoothing(AMASS) levels and cutoff frequencies. The highest level
 // frequency bin starts at 0Hz and ends at its cutoff frequency. The next lower level frequency bin
 // starts at the next higher cutoff frequency, and so on. The cutoff frequencies for each level must
 // be considered carefully against how much it over-drives the stepper ISR, the accuracy of the 16-bit
 // timer, and the CPU overhead. Level 0 (no AMASS, normal operation) frequency bin starts at the
 // Level 1 cutoff frequency and up to as fast as the CPU allows (over 30kHz in limited testing).
+// For efficient computation, each cutoff frequency is twice the previous one.
 // NOTE: AMASS cutoff frequency multiplied by ISR overdrive factor must not exceed maximum step frequency.
 // NOTE: Current settings are set to overdrive the ISR to no more than 16kHz, balancing CPU overhead
 // and timer accuracy.  Do not alter these settings unless you know what you are doing.
-///#ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING
-#define MAX_AMASS_LEVEL 3
-// AMASS_LEVEL0: Normal operation. No AMASS. No upper cutoff frequency. Starts at LEVEL1 cutoff frequency.
-// Note ESP32 use F_STEPPER_TIMER rather than the AVR F_CPU
-const int AMASS_LEVEL1 = (F_STEPPER_TIMER / 8000);  // Over-drives ISR (x2). Defined as F_CPU/(Cutoff frequency in Hz)
-const int AMASS_LEVEL2 = (F_STEPPER_TIMER / 4000);  // Over-drives ISR (x4)
-const int AMASS_LEVEL3 = (F_STEPPER_TIMER / 2000);  // Over-drives ISR (x8)
 
-static_assert(MAX_AMASS_LEVEL >= 1, "AMASS must have 1 or more levels to operate correctly.");
-//#endif
+const uint32_t amassThreshold = fStepperTimer / 8000;
+const int maxAmassLevel = 3;  // Each level increase doubles the threshold
 
 const timer_group_t STEP_TIMER_GROUP = TIMER_GROUP_0;
 const timer_idx_t   STEP_TIMER_INDEX = TIMER_0;
@@ -81,6 +79,17 @@ enum stepper_id_t {
     ST_I2S_STREAM,
     ST_I2S_STATIC,
 };
+
+#ifndef DEFAULT_STEPPER
+#    if defined(USE_I2S_STEPS)
+#        define DEFAULT_STEPPER ST_I2S_STREAM
+#    elif defined(USE_RMT_STEPS)
+#        define DEFAULT_STEPPER ST_RMT
+#    else
+#        define DEFAULT_STEPPER ST_TIMED
+#    endif
+#endif
+
 extern const char*  stepper_names[];
 extern stepper_id_t current_stepper;
 
@@ -121,7 +130,7 @@ bool get_stepper_disable();  // returns the state of the pin
 void set_stepper_pins_on(uint8_t onMask);
 void set_direction_pins_on(uint8_t onMask);
 
-void Stepper_Timer_WritePeriod(uint64_t alarm_val);
+void Stepper_Timer_WritePeriod(uint16_t timerTicks);
 void Stepper_Timer_Init();
 void Stepper_Timer_Start();
 void Stepper_Timer_Stop();
diff --git a/Grbl_Esp32/src/System.cpp b/Grbl_Esp32/src/System.cpp
index 7c434ec3..d8d18d00 100644
--- a/Grbl_Esp32/src/System.cpp
+++ b/Grbl_Esp32/src/System.cpp
@@ -22,18 +22,20 @@
 #include "Config.h"
 
 // Declare system global variable structure
-system_t           sys;
-int32_t            sys_position[MAX_N_AXIS];        // Real-time machine (aka home) position vector in steps.
-int32_t            sys_probe_position[MAX_N_AXIS];  // Last probe position in machine coordinates and steps.
-volatile uint8_t   sys_probe_state;                 // Probing state value.  Used to coordinate the probing cycle with stepper ISR.
-volatile uint8_t   sys_rt_exec_state;               // Global realtime executor bitflag variable for state management. See EXEC bitmasks.
-volatile ExecAlarm sys_rt_exec_alarm;               // Global realtime executor bitflag variable for setting various alarms.
-volatile uint8_t   sys_rt_exec_motion_override;     // Global realtime executor bitflag variable for motion-based overrides.
-volatile uint8_t   sys_rt_exec_accessory_override;  // Global realtime executor bitflag variable for spindle/coolant overrides.
-volatile bool      cycle_stop;                      // For state transitions, instead of bitflag
+system_t               sys;
+int32_t                sys_position[MAX_N_AXIS];        // Real-time machine (aka home) position vector in steps.
+int32_t                sys_probe_position[MAX_N_AXIS];  // Last probe position in machine coordinates and steps.
+volatile Probe         sys_probe_state;                 // Probing state value.  Used to coordinate the probing cycle with stepper ISR.
+volatile ExecState     sys_rt_exec_state;  // Global realtime executor bitflag variable for state management. See EXEC bitmasks.
+volatile ExecAlarm     sys_rt_exec_alarm;  // Global realtime executor bitflag variable for setting various alarms.
+volatile ExecAccessory sys_rt_exec_accessory_override;  // Global realtime executor bitflag variable for spindle/coolant overrides.
+volatile bool          cycle_stop;                      // For state transitions, instead of bitflag
 #ifdef DEBUG
-volatile uint8_t sys_rt_exec_debug;
+volatile bool sys_rt_exec_debug;
 #endif
+volatile Percent sys_rt_f_override;  // Global realtime executor feedrate override percentage
+volatile Percent sys_rt_r_override;  // Global realtime executor rapid override percentage
+volatile Percent sys_rt_s_override;  // Global realtime executor spindle override percentage
 
 UserOutput::AnalogOutput*  myAnalogOutputs[MaxUserDigitalPin];
 UserOutput::DigitalOutput* myDigitalOutputs[MaxUserDigitalPin];
@@ -116,11 +118,14 @@ void controlCheckTask(void* pvParameters) {
         int evt;
         xQueueReceive(control_sw_queue, &evt, portMAX_DELAY);  // block until receive queue
         vTaskDelay(CONTROL_SW_DEBOUNCE_PERIOD);                // delay a while
-        uint8_t pin = system_control_get_state();
-        if (pin) {
-            system_exec_control_pin(pin);
+        ControlPins pins = system_control_get_state();
+        if (pins.value) {
+            system_exec_control_pin(pins);
         }
         debouncing = false;
+
+        static UBaseType_t uxHighWaterMark = 0;
+        reportTaskStackSize(uxHighWaterMark);
     }
 }
 #endif
@@ -134,77 +139,20 @@ void IRAM_ATTR isr_control_inputs() {
         xQueueSendFromISR(control_sw_queue, &evt, NULL);
     }
 #else
-    uint8_t pin = system_control_get_state();
-    system_exec_control_pin(pin);
+    ControlPins pins = system_control_get_state();
+    system_exec_control_pin(pins);
 #endif
 }
 
 // Returns if safety door is ajar(T) or closed(F), based on pin state.
 uint8_t system_check_safety_door_ajar() {
 #ifdef ENABLE_SAFETY_DOOR_INPUT_PIN
-    return system_control_get_state() & CONTROL_PIN_INDEX_SAFETY_DOOR;
+    return system_control_get_state().bit.safetyDoor;
 #else
     return false;  // Input pin not enabled, so just return that it's closed.
 #endif
 }
 
-// Special handlers for setting and clearing Grbl's real-time execution flags.
-void system_set_exec_state_flag(uint8_t mask) {
-    // TODO uint8_t sreg = SREG;
-    // TODO cli();
-    sys_rt_exec_state |= (mask);
-    // TODO SREG = sreg;
-}
-
-void system_clear_exec_state_flag(uint8_t mask) {
-    //uint8_t sreg = SREG;
-    //cli();
-    sys_rt_exec_state &= ~(mask);
-    //SREG = sreg;
-}
-
-void system_set_exec_alarm(ExecAlarm code) {
-    //uint8_t sreg = SREG;
-    //cli();
-    sys_rt_exec_alarm = code;
-    //SREG = sreg;
-}
-
-void system_clear_exec_alarm() {
-    //uint8_t sreg = SREG;
-    //cli();
-    sys_rt_exec_alarm = ExecAlarm::None;
-    //SREG = sreg;
-}
-
-void system_set_exec_motion_override_flag(uint8_t mask) {
-    //uint8_t sreg = SREG;
-    //cli();
-    sys_rt_exec_motion_override |= (mask);
-    //SREG = sreg;
-}
-
-void system_set_exec_accessory_override_flag(uint8_t mask) {
-    //uint8_t sreg = SREG;
-    //cli();
-    sys_rt_exec_accessory_override |= (mask);
-    //SREG = sreg;
-}
-
-void system_clear_exec_motion_overrides() {
-    //uint8_t sreg = SREG;
-    //cli();
-    sys_rt_exec_motion_override = 0;
-    //SREG = sreg;
-}
-
-void system_clear_exec_accessory_overrides() {
-    //uint8_t sreg = SREG;
-    //cli();
-    sys_rt_exec_accessory_override = 0;
-    //SREG = sreg;
-}
-
 void system_flag_wco_change() {
 #ifdef FORCE_BUFFER_SYNC_DURING_WCO_CHANGE
     protocol_buffer_synchronize();
@@ -241,124 +189,90 @@ void system_convert_array_steps_to_mpos(float* position, int32_t* steps) {
     return;
 }
 
-// Checks and reports if target array exceeds machine travel limits.
-// Return true if exceeding limits
-uint8_t system_check_travel_limits(float* target) {
-    uint8_t idx;
-    auto    n_axis = number_axis->get();
-    for (idx = 0; idx < n_axis; idx++) {
-        float travel = axis_settings[idx]->max_travel->get();
-        float mpos   = axis_settings[idx]->home_mpos->get();
-        float max_mpos, min_mpos;
-
-        if (bit_istrue(homing_dir_mask->get(), bit(idx))) {
-            min_mpos = mpos;
-            max_mpos = mpos + travel;
-        } else {
-            min_mpos = mpos - travel;
-            max_mpos = mpos;
-        }
-
-        if (target[idx] < min_mpos || target[idx] > max_mpos)
-            return true;
-    }
-    return false;
-}
-
 // Returns control pin state as a uint8 bitfield. Each bit indicates the input pin state, where
 // triggered is 1 and not triggered is 0. Invert mask is applied. Bitfield organization is
-// defined by the CONTROL_PIN_INDEX in the header file.
-uint8_t system_control_get_state() {
-    uint8_t defined_pin_mask = 0;  // a mask of defined pins
-    uint8_t control_state    = 0;
+// defined by the ControlPin in System.h.
+ControlPins system_control_get_state() {
+    ControlPins defined_pins;
+    defined_pins.value = 0;
+
+    ControlPins pin_states;
+    pin_states.value = 0;
 
 #ifdef CONTROL_SAFETY_DOOR_PIN
-    defined_pin_mask |= CONTROL_PIN_INDEX_SAFETY_DOOR;
+    defined_pins.bit.safetyDoor = true;
     if (digitalRead(CONTROL_SAFETY_DOOR_PIN)) {
-        control_state |= CONTROL_PIN_INDEX_SAFETY_DOOR;
+        pin_states.bit.safetyDoor = true;
     }
 #endif
 #ifdef CONTROL_RESET_PIN
-    defined_pin_mask |= CONTROL_PIN_INDEX_RESET;
+    defined_pins.bit.reset = true;
     if (digitalRead(CONTROL_RESET_PIN)) {
-        control_state |= CONTROL_PIN_INDEX_RESET;
+        pin_states.bit.reset = true;
     }
 #endif
 #ifdef CONTROL_FEED_HOLD_PIN
-    defined_pin_mask |= CONTROL_PIN_INDEX_FEED_HOLD;
+    defined_pins.bit.feedHold = true;
     if (digitalRead(CONTROL_FEED_HOLD_PIN)) {
-        control_state |= CONTROL_PIN_INDEX_FEED_HOLD;
+        pin_states.bit.feedHold = true;
     }
 #endif
 #ifdef CONTROL_CYCLE_START_PIN
-    defined_pin_mask |= CONTROL_PIN_INDEX_CYCLE_START;
+    defined_pins.bit.cycleStart = true;
     if (digitalRead(CONTROL_CYCLE_START_PIN)) {
-        control_state |= CONTROL_PIN_INDEX_CYCLE_START;
+        pin_states.bit.cycleStart = true;
     }
 #endif
 #ifdef MACRO_BUTTON_0_PIN
-    defined_pin_mask |= CONTROL_PIN_INDEX_MACRO_0;
+    defined_pins.bit.macro0 = true;
     if (digitalRead(MACRO_BUTTON_0_PIN)) {
-        control_state |= CONTROL_PIN_INDEX_MACRO_0;
+        pin_states.bit.macro0 = true;
     }
 #endif
 #ifdef MACRO_BUTTON_1_PIN
-    defined_pin_mask |= CONTROL_PIN_INDEX_MACRO_1;
+    defined_pins.bit.macro1 = true;
     if (digitalRead(MACRO_BUTTON_1_PIN)) {
-        control_state |= CONTROL_PIN_INDEX_MACRO_1;
+        pin_states.bit.macro1 = true;
     }
 #endif
 #ifdef MACRO_BUTTON_2_PIN
-    defined_pin_mask |= CONTROL_PIN_INDEX_MACRO_2;
+    defined_pins.bit.macro2 = true;
     if (digitalRead(MACRO_BUTTON_2_PIN)) {
-        control_state |= CONTROL_PIN_INDEX_MACRO_2;
+        pin_states.bit.macro2 = true;
     }
 #endif
 #ifdef MACRO_BUTTON_3_PIN
-    defined_pin_mask |= CONTROL_PIN_INDEX_MACRO_3;
+    defined_pins.bit.macro3 = true;
     if (digitalRead(MACRO_BUTTON_3_PIN)) {
-        control_state |= CONTROL_PIN_INDEX_MACRO_3;
+        pin_states.bit.macro3 = true;
     }
 #endif
 #ifdef INVERT_CONTROL_PIN_MASK
-    control_state ^= (INVERT_CONTROL_PIN_MASK & defined_pin_mask);
+    pin_states.value ^= (INVERT_CONTROL_PIN_MASK & defined_pins.value);
 #endif
-
-    return control_state;
+    return pin_states;
 }
 
 // execute the function of the control pin
-void system_exec_control_pin(uint8_t pin) {
-    if (bit_istrue(pin, CONTROL_PIN_INDEX_RESET)) {
+void system_exec_control_pin(ControlPins pins) {
+    if (pins.bit.reset) {
         grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Info, "Reset via control pin");
         mc_reset();
-    } else if (bit_istrue(pin, CONTROL_PIN_INDEX_CYCLE_START)) {
-        bit_true(sys_rt_exec_state, EXEC_CYCLE_START);
-    } else if (bit_istrue(pin, CONTROL_PIN_INDEX_FEED_HOLD)) {
-        bit_true(sys_rt_exec_state, EXEC_FEED_HOLD);
-    } else if (bit_istrue(pin, CONTROL_PIN_INDEX_SAFETY_DOOR)) {
-        bit_true(sys_rt_exec_state, EXEC_SAFETY_DOOR);
+    } else if (pins.bit.cycleStart) {
+        sys_rt_exec_state.bit.cycleStart = true;
+    } else if (pins.bit.feedHold) {
+        sys_rt_exec_state.bit.feedHold = true;
+    } else if (pins.bit.safetyDoor) {
+        sys_rt_exec_state.bit.safetyDoor = true;
+    } else if (pins.bit.macro0) {
+        user_defined_macro(0);  // function must be implemented by user
+    } else if (pins.bit.macro1) {
+        user_defined_macro(1);  // function must be implemented by user
+    } else if (pins.bit.macro2) {
+        user_defined_macro(2);  // function must be implemented by user
+    } else if (pins.bit.macro3) {
+        user_defined_macro(3);  // function must be implemented by user
     }
-#ifdef MACRO_BUTTON_0_PIN
-    else if (bit_istrue(pin, CONTROL_PIN_INDEX_MACRO_0)) {
-        user_defined_macro(CONTROL_PIN_INDEX_MACRO_0);  // function must be implemented by user
-    }
-#endif
-#ifdef MACRO_BUTTON_1_PIN
-    else if (bit_istrue(pin, CONTROL_PIN_INDEX_MACRO_1)) {
-        user_defined_macro(CONTROL_PIN_INDEX_MACRO_1);  // function must be implemented by user
-    }
-#endif
-#ifdef MACRO_BUTTON_2_PIN
-    else if (bit_istrue(pin, CONTROL_PIN_INDEX_MACRO_2)) {
-        user_defined_macro(CONTROL_PIN_INDEX_MACRO_2);  // function must be implemented by user
-    }
-#endif
-#ifdef MACRO_BUTTON_3_PIN
-    else if (bit_istrue(pin, CONTROL_PIN_INDEX_MACRO_3)) {
-        user_defined_macro(CONTROL_PIN_INDEX_MACRO_3);  // function must be implemented by user
-    }
-#endif
 }
 
 // CoreXY calculation only. Returns x or y-axis "steps" based on CoreXY motor steps.
@@ -402,18 +316,6 @@ bool sys_pwm_control(uint8_t io_num_mask, float duty, bool synchronized) {
     return cmd_ok;
 }
 
-// Call this function to get an RMT channel number
-// returns -1 for error
-int8_t sys_get_next_RMT_chan_num() {
-    static uint8_t next_RMT_chan_num = 0;  // channels 0-7 are valid
-    if (next_RMT_chan_num < 8) {           // 7 is the max PWM channel number
-        return next_RMT_chan_num++;
-    } else {
-        grbl_msg_sendf(CLIENT_SERIAL, MsgLevel::Error, "Error: out of RMT channels");
-        return -1;
-    }
-}
-
 /*
     This returns an unused pwm channel.
     The 8 channels share 4 timers, so pairs 0,1 & 2,3 , etc
@@ -449,3 +351,4 @@ uint8_t sys_calc_pwm_precision(uint32_t freq) {
 
     return precision - 1;
 }
+void __attribute__((weak)) user_defined_macro(uint8_t index);
diff --git a/Grbl_Esp32/src/System.h b/Grbl_Esp32/src/System.h
index 4568980d..e1ea051a 100644
--- a/Grbl_Esp32/src/System.h
+++ b/Grbl_Esp32/src/System.h
@@ -6,7 +6,7 @@
   Copyright (c) 2014-2016 Sungeun K. Jeon for Gnea Research LLC
 
 	2018 -	Bart Dring This file was modifed for use on the ESP32
-					CPU. Do not use this with Grbl for atMega328P
+		CPU. Do not use this with Grbl for atMega328P
 
   Grbl is free software: you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
@@ -20,8 +20,6 @@
   along with Grbl.  If not, see <http://www.gnu.org/licenses/>.
 */
 
-#include "Grbl.h"
-
 // System states. The state variable primarily tracks the individual functions
 // of Grbl to manage each without overlapping. It is also used as a messaging flag for
 // critical events.
@@ -37,43 +35,98 @@ enum class State : uint8_t {
     Sleep,       // Sleep state.
 };
 
-// Define global system variables
-typedef struct {
-    volatile State   state;      // Tracks the current system state of Grbl.
-    uint8_t abort;               // System abort flag. Forces exit back to main loop for reset.
-    uint8_t suspend;             // System suspend bitflag variable that manages holds, cancels, and safety door.
-    uint8_t soft_limit;          // Tracks soft limit errors for the state machine. (boolean)
-    uint8_t step_control;        // Governs the step segment generator depending on system state.
-    uint8_t probe_succeeded;     // Tracks if last probing cycle was successful.
-    uint8_t homing_axis_lock;    // Locks axes when limits engage. Used as an axis motion mask in the stepper ISR.
-    uint8_t f_override;          // Feed rate override value in percent
-    uint8_t r_override;          // Rapids override value in percent
-    uint8_t spindle_speed_ovr;   // Spindle speed value in percent
-    uint8_t spindle_stop_ovr;    // Tracks spindle stop override states
-    uint8_t report_ovr_counter;  // Tracks when to add override data to status reports.
-    uint8_t report_wco_counter;  // Tracks when to add work coordinate offset data to status reports.
-#ifdef ENABLE_PARKING_OVERRIDE_CONTROL
-    uint8_t override_ctrl;  // Tracks override control states.
+// Step segment generator state flags.
+struct StepControl {
+    uint8_t endMotion : 1;
+    uint8_t executeHold : 1;
+    uint8_t executeSysMotion : 1;
+    uint8_t updateSpindleRpm : 1;
+};
+
+// System suspend flags. Used in various ways to manage suspend states and procedures.
+struct SuspendBits {
+    uint8_t holdComplete : 1;     // Indicates initial feed hold is complete.
+    uint8_t restartRetract : 1;   // Flag to indicate a retract from a restore parking motion.
+    uint8_t retractComplete : 1;  // (Safety door only) Indicates retraction and de-energizing is complete.
+    uint8_t initiateRestore : 1;  // (Safety door only) Flag to initiate resume procedures from a cycle start.
+    uint8_t restoreComplete : 1;  // (Safety door only) Indicates ready to resume normal operation.
+    uint8_t safetyDoorAjar : 1;   // Tracks safety door state for resuming.
+    uint8_t motionCancel : 1;     // Indicates a canceled resume motion. Currently used by probing routine.
+    uint8_t jogCancel : 1;        // Indicates a jog cancel in process and to reset buffers when complete.
+};
+union Suspend {
+    uint8_t     value;
+    SuspendBits bit;
+};
+
+typedef uint8_t AxisMask;  // Bits indexed by axis number
+typedef uint8_t Percent;   // Integer percent
+typedef uint8_t Counter;   // Report interval
+
+enum class Override : uint8_t {
+#ifdef DEACTIVATE_PARKING_UPON_INIT
+    Disabled      = 0,  // (Default: Must be zero)
+    ParkingMotion = 1,  // M56
+#else
+    ParkingMotion = 0,  // M56 (Default: Must be zero)
+    Disabled      = 1,  // Parking disabled.
 #endif
+};
 
+// Spindle stop override control states.
+struct SpindleStopBits {
+    uint8_t enabled : 1;
+    uint8_t initiate : 1;
+    uint8_t restore : 1;
+    uint8_t restoreCycle : 1;
+};
+union SpindleStop {
+    uint8_t         value;
+    SpindleStopBits bit;
+};
+
+// Global system variables
+typedef struct {
+    volatile State state;               // Tracks the current system state of Grbl.
+    bool           abort;               // System abort flag. Forces exit back to main loop for reset.
+    Suspend        suspend;             // System suspend bitflag variable that manages holds, cancels, and safety door.
+    bool           soft_limit;          // Tracks soft limit errors for the state machine. (boolean)
+    StepControl    step_control;        // Governs the step segment generator depending on system state.
+    bool           probe_succeeded;     // Tracks if last probing cycle was successful.
+    AxisMask       homing_axis_lock;    // Locks axes when limits engage. Used as an axis motion mask in the stepper ISR.
+    Percent        f_override;          // Feed rate override value in percent
+    Percent        r_override;          // Rapids override value in percent
+    Percent        spindle_speed_ovr;   // Spindle speed value in percent
+    SpindleStop    spindle_stop_ovr;    // Tracks spindle stop override states
+    Counter        report_ovr_counter;  // Tracks when to add override data to status reports.
+    Counter        report_wco_counter;  // Tracks when to add work coordinate offset data to status reports.
+#ifdef ENABLE_PARKING_OVERRIDE_CONTROL
+    Override override_ctrl;  // Tracks override control states.
+#endif
     uint32_t spindle_speed;
-
 } system_t;
 extern system_t sys;
 
-// Define system executor bit map. Used internally by realtime protocol as realtime command flags,
+// System executor bit map. Used internally by realtime protocol as realtime command flags,
 // which notifies the main program to execute the specified realtime command asynchronously.
 // NOTE: The system executor uses an unsigned 8-bit volatile variable (8 flag limit.) The default
 // flags are always false, so the realtime protocol only needs to check for a non-zero value to
 // know when there is a realtime command to execute.
-#define EXEC_STATUS_REPORT bit(0)  // bitmask 00000001
-#define EXEC_CYCLE_START bit(1)    // bitmask 00000010
-// #define EXEC_CYCLE_STOP bit(2)  // bitmask 00000100 moved to cycle_stop
-#define EXEC_FEED_HOLD bit(3)      // bitmask 00001000
-#define EXEC_RESET bit(4)          // bitmask 00010000
-#define EXEC_SAFETY_DOOR bit(5)    // bitmask 00100000
-#define EXEC_MOTION_CANCEL bit(6)  // bitmask 01000000
-#define EXEC_SLEEP bit(7)          // bitmask 10000000
+struct ExecStateBits {
+    uint8_t statusReport : 1;
+    uint8_t cycleStart : 1;
+    uint8_t cycleStop : 1;  // Unused, per cycle_stop variable
+    uint8_t feedHold : 1;
+    uint8_t reset : 1;
+    uint8_t safetyDoor : 1;
+    uint8_t motionCancel : 1;
+    uint8_t sleep : 1;
+};
+
+union ExecState {
+    uint8_t       value;
+    ExecStateBits bit;
+};
 
 // Alarm executor codes. Valid values (1-255). Zero is reserved.
 enum class ExecAlarm : uint8_t {
@@ -92,105 +145,61 @@ enum class ExecAlarm : uint8_t {
 
 // Override bit maps. Realtime bitflags to control feed, rapid, spindle, and coolant overrides.
 // Spindle/coolant and feed/rapids are separated into two controlling flag variables.
-#define EXEC_FEED_OVR_RESET bit(0)
-#define EXEC_FEED_OVR_COARSE_PLUS bit(1)
-#define EXEC_FEED_OVR_COARSE_MINUS bit(2)
-#define EXEC_FEED_OVR_FINE_PLUS bit(3)
-#define EXEC_FEED_OVR_FINE_MINUS bit(4)
-#define EXEC_RAPID_OVR_RESET bit(5)
-#define EXEC_RAPID_OVR_MEDIUM bit(6)
-#define EXEC_RAPID_OVR_LOW bit(7)
-// #define EXEC_RAPID_OVR_EXTRA_LOW   bit(*) // *NOT SUPPORTED*
 
-#define EXEC_SPINDLE_OVR_RESET bit(0)
-#define EXEC_SPINDLE_OVR_COARSE_PLUS bit(1)
-#define EXEC_SPINDLE_OVR_COARSE_MINUS bit(2)
-#define EXEC_SPINDLE_OVR_FINE_PLUS bit(3)
-#define EXEC_SPINDLE_OVR_FINE_MINUS bit(4)
-#define EXEC_SPINDLE_OVR_STOP bit(5)
-#define EXEC_COOLANT_FLOOD_OVR_TOGGLE bit(6)
-#define EXEC_COOLANT_MIST_OVR_TOGGLE bit(7)
+struct ExecAccessoryBits {
+    uint8_t spindleOvrStop : 1;
+    uint8_t coolantFloodOvrToggle : 1;
+    uint8_t coolantMistOvrToggle : 1;
+};
 
-// Define system suspend flags. Used in various ways to manage suspend states and procedures.
-#define SUSPEND_DISABLE 0                // Must be zero.
-#define SUSPEND_HOLD_COMPLETE bit(0)     // Indicates initial feed hold is complete.
-#define SUSPEND_RESTART_RETRACT bit(1)   // Flag to indicate a retract from a restore parking motion.
-#define SUSPEND_RETRACT_COMPLETE bit(2)  // (Safety door only) Indicates retraction and de-energizing is complete.
-#define SUSPEND_INITIATE_RESTORE bit(3)  // (Safety door only) Flag to initiate resume procedures from a cycle start.
-#define SUSPEND_RESTORE_COMPLETE bit(4)  // (Safety door only) Indicates ready to resume normal operation.
-#define SUSPEND_SAFETY_DOOR_AJAR bit(5)  // Tracks safety door state for resuming.
-#define SUSPEND_MOTION_CANCEL bit(6)     // Indicates a canceled resume motion. Currently used by probing routine.
-#define SUSPEND_JOG_CANCEL bit(7)        // Indicates a jog cancel in process and to reset buffers when complete.
+union ExecAccessory {
+    uint8_t           value;
+    ExecAccessoryBits bit;
+};
 
-// Define step segment generator state flags.
-#define STEP_CONTROL_NORMAL_OP 0  // Must be zero.
-#define STEP_CONTROL_END_MOTION bit(0)
-#define STEP_CONTROL_EXECUTE_HOLD bit(1)
-#define STEP_CONTROL_EXECUTE_SYS_MOTION bit(2)
-#define STEP_CONTROL_UPDATE_SPINDLE_RPM bit(3)
-
-// Define control pin index for Grbl internal use. Pin maps may change, but these values don't.
-//#ifdef ENABLE_SAFETY_DOOR_INPUT_PIN
-#define N_CONTROL_PIN 4
-#define CONTROL_PIN_INDEX_SAFETY_DOOR bit(0)
-#define CONTROL_PIN_INDEX_RESET bit(1)
-#define CONTROL_PIN_INDEX_FEED_HOLD bit(2)
-#define CONTROL_PIN_INDEX_CYCLE_START bit(3)
-#define CONTROL_PIN_INDEX_MACRO_0 bit(4)
-#define CONTROL_PIN_INDEX_MACRO_1 bit(5)
-#define CONTROL_PIN_INDEX_MACRO_2 bit(6)
-#define CONTROL_PIN_INDEX_MACRO_3 bit(7)
-//#else
-//#define N_CONTROL_PIN 3
-//#define CONTROL_PIN_INDEX_RESET         bit(0)
-//#define CONTROL_PIN_INDEX_FEED_HOLD     bit(1)
-//#define CONTROL_PIN_INDEX_CYCLE_START   bit(2)
-//#endif
-
-// Define spindle stop override control states.
-#define SPINDLE_STOP_OVR_DISABLED 0  // Must be zero.
-#define SPINDLE_STOP_OVR_ENABLED bit(0)
-#define SPINDLE_STOP_OVR_INITIATE bit(1)
-#define SPINDLE_STOP_OVR_RESTORE bit(2)
-#define SPINDLE_STOP_OVR_RESTORE_CYCLE bit(3)
+// Control pin states
+struct ControlPinBits {
+    uint8_t safetyDoor : 1;
+    uint8_t reset : 1;
+    uint8_t feedHold : 1;
+    uint8_t cycleStart : 1;
+    uint8_t macro0 : 1;
+    uint8_t macro1 : 1;
+    uint8_t macro2 : 1;
+    uint8_t macro3 : 1;
+};
+union ControlPins {
+    uint8_t        value;
+    ControlPinBits bit;
+};
 
 // NOTE: These position variables may need to be declared as volatiles, if problems arise.
 extern int32_t sys_position[MAX_N_AXIS];        // Real-time machine (aka home) position vector in steps.
 extern int32_t sys_probe_position[MAX_N_AXIS];  // Last probe position in machine coordinates and steps.
 
-extern volatile uint8_t   sys_probe_state;    // Probing state value.  Used to coordinate the probing cycle with stepper ISR.
-extern volatile uint8_t   sys_rt_exec_state;  // Global realtime executor bitflag variable for state management. See EXEC bitmasks.
-extern volatile ExecAlarm sys_rt_exec_alarm;  // Global realtime executor bitflag variable for setting various alarms.
-extern volatile uint8_t   sys_rt_exec_motion_override;     // Global realtime executor bitflag variable for motion-based overrides.
-extern volatile uint8_t   sys_rt_exec_accessory_override;  // Global realtime executor bitflag variable for spindle/coolant overrides.
-extern volatile bool      cycle_stop;
-
+extern volatile Probe         sys_probe_state;    // Probing state value.  Used to coordinate the probing cycle with stepper ISR.
+extern volatile ExecState     sys_rt_exec_state;  // Global realtime executor bitflag variable for state management. See EXEC bitmasks.
+extern volatile ExecAlarm     sys_rt_exec_alarm;  // Global realtime executor bitflag variable for setting various alarms.
+extern volatile ExecAccessory sys_rt_exec_accessory_override;  // Global realtime executor bitflag variable for spindle/coolant overrides.
+extern volatile Percent       sys_rt_f_override;               // Feed override value in percent
+extern volatile Percent       sys_rt_r_override;               // Rapid feed override value in percent
+extern volatile Percent       sys_rt_s_override;               // Spindle override value in percent
+extern volatile bool          cycle_stop;
 #ifdef DEBUG
-#    define EXEC_DEBUG_REPORT bit(0)
-extern volatile uint8_t sys_rt_exec_debug;
+extern volatile bool sys_rt_exec_debug;
 #endif
 
 void system_ini();  // Renamed from system_init() due to conflict with esp32 files
 
 // Returns bitfield of control pin states, organized by CONTROL_PIN_INDEX. (1=triggered, 0=not triggered).
-uint8_t system_control_get_state();
+ControlPins system_control_get_state();
 
 // Returns if safety door is ajar(T) or closed(F), based on pin state.
 uint8_t system_check_safety_door_ajar();
 
 void isr_control_inputs();
 
-// Special handlers for setting and clearing Grbl's real-time execution flags.
-void system_set_exec_state_flag(uint8_t mask);
-void system_clear_exec_state_flag(uint8_t mask);
-void system_set_exec_alarm(ExecAlarm code);
-void system_clear_exec_alarm();
-void system_set_exec_motion_override_flag(uint8_t mask);
-void system_set_exec_accessory_override_flag(uint8_t mask);
-void system_clear_exec_motion_overrides();
-void system_clear_exec_accessory_overrides();
-
-// Execute the startup script lines stored in EEPROM upon initialization
+// Execute the startup script lines stored in non-volatile storage upon initialization
 void  system_execute_startup(char* line);
 Error execute_line(char* line, uint8_t client, WebUI::AuthenticationLevel auth_level);
 Error system_execute_line(char* line, WebUI::ESPResponseStream*, WebUI::AuthenticationLevel);
@@ -204,30 +213,15 @@ float system_convert_axis_steps_to_mpos(int32_t* steps, uint8_t idx);
 // Updates a machine 'position' array based on the 'step' array sent.
 void system_convert_array_steps_to_mpos(float* position, int32_t* steps);
 
-// Checks and reports if target array exceeds machine travel limits.
-uint8_t system_check_travel_limits(float* target);
-
-// Special handlers for setting and clearing Grbl's real-time execution flags.
-void system_set_exec_state_flag(uint8_t mask);
-void system_clear_exec_state_flag(uint8_t mask);
-void system_set_exec_alarm(ExecAlarm code);
-void system_clear_exec_alarm();
-void system_set_exec_motion_override_flag(uint8_t mask);
-void system_set_exec_accessory_override_flag(uint8_t mask);
-void system_clear_exec_motion_overrides();
-void system_clear_exec_accessory_overrides();
-
 int32_t system_convert_corexy_to_x_axis_steps(int32_t* steps);
 int32_t system_convert_corexy_to_y_axis_steps(int32_t* steps);
 
 // A task that runs after a control switch interrupt for debouncing.
 void controlCheckTask(void* pvParameters);
-void system_exec_control_pin(uint8_t pin);
+void system_exec_control_pin(ControlPins pins);
 
 bool sys_io_control(uint8_t io_num_mask, bool turnOn, bool synchronized);
 bool sys_pwm_control(uint8_t io_num_mask, float duty, bool synchronized);
 
-int8_t sys_get_next_RMT_chan_num();
-
 int8_t  sys_get_next_PWM_chan_num();
 uint8_t sys_calc_pwm_precision(uint32_t freq);
diff --git a/Grbl_Esp32/src/WebUI/ESPResponse.cpp b/Grbl_Esp32/src/WebUI/ESPResponse.cpp
index 483b0252..ce128152 100644
--- a/Grbl_Esp32/src/WebUI/ESPResponse.cpp
+++ b/Grbl_Esp32/src/WebUI/ESPResponse.cpp
@@ -96,9 +96,6 @@ namespace WebUI {
             return;
         }
 #endif
-        if (_client == CLIENT_WEBUI) {
-            return;  //this is sanity check
-        }
         grbl_send(_client, data);
     }
 
diff --git a/Grbl_Esp32/src/WebUI/InputBuffer.h b/Grbl_Esp32/src/WebUI/InputBuffer.h
index 4eec0ca6..da25b6c5 100644
--- a/Grbl_Esp32/src/WebUI/InputBuffer.h
+++ b/Grbl_Esp32/src/WebUI/InputBuffer.h
@@ -49,7 +49,7 @@ namespace WebUI {
         ~InputBuffer();
 
     private:
-        static const int RXBUFFERSIZE = 128;
+        static const int RXBUFFERSIZE = 256;
 
         uint8_t  _RXbuffer[RXBUFFERSIZE];
         uint16_t _RXbufferSize;
diff --git a/Grbl_Esp32/src/WebUI/Serial2Socket.h b/Grbl_Esp32/src/WebUI/Serial2Socket.h
index 7d337520..9babdf95 100644
--- a/Grbl_Esp32/src/WebUI/Serial2Socket.h
+++ b/Grbl_Esp32/src/WebUI/Serial2Socket.h
@@ -28,7 +28,7 @@ class WebSocketsServer;
 namespace WebUI {
     class Serial_2_Socket : public Print {
         static const int TXBUFFERSIZE = 1200;
-        static const int RXBUFFERSIZE = 128;
+        static const int RXBUFFERSIZE = 256;
         static const int FLUSHTIMEOUT = 500;
 
     public:
diff --git a/Grbl_Esp32/src/WebUI/WebServer.cpp b/Grbl_Esp32/src/WebUI/WebServer.cpp
index 495d5017..f9ef698d 100644
--- a/Grbl_Esp32/src/WebUI/WebServer.cpp
+++ b/Grbl_Esp32/src/WebUI/WebServer.cpp
@@ -430,36 +430,6 @@ namespace WebUI {
     }
 #    endif
 
-    bool Web_Server::is_realtime_cmd(char c) {
-        switch (c) {
-            case CMD_STATUS_REPORT:
-            case CMD_CYCLE_START:
-            case CMD_RESET:
-            case CMD_FEED_HOLD:
-            case CMD_SAFETY_DOOR:
-            case CMD_JOG_CANCEL:
-            case CMD_DEBUG_REPORT:
-            case CMD_FEED_OVR_RESET:
-            case CMD_FEED_OVR_COARSE_PLUS:
-            case CMD_FEED_OVR_COARSE_MINUS:
-            case CMD_FEED_OVR_FINE_PLUS:
-            case CMD_FEED_OVR_FINE_MINUS:
-            case CMD_RAPID_OVR_RESET:
-            case CMD_RAPID_OVR_MEDIUM:
-            case CMD_RAPID_OVR_LOW:
-            case CMD_SPINDLE_OVR_COARSE_PLUS:
-            case CMD_SPINDLE_OVR_COARSE_MINUS:
-            case CMD_SPINDLE_OVR_FINE_PLUS:
-            case CMD_SPINDLE_OVR_FINE_MINUS:
-            case CMD_SPINDLE_OVR_STOP:
-            case CMD_COOLANT_FLOOD_OVR_TOGGLE:
-            case CMD_COOLANT_MIST_OVR_TOGGLE:
-                return true;
-            default:
-                return false;
-        }
-    }
-
     void Web_Server::_handle_web_command(bool silent) {
         //to save time if already disconnected
         //if (_webserver->hasArg ("PAGEID") ) {
@@ -529,7 +499,7 @@ namespace WebUI {
                 }
                 if (scmd.length() > 1) {
                     scmd += "\n";
-                } else if (!is_realtime_cmd(scmd[0])) {
+                } else if (!is_realtime_command(scmd[0])) {
                     scmd += "\n";
                 }
                 if (!Serial2Socket.push(scmd.c_str())) {
@@ -1348,6 +1318,7 @@ namespace WebUI {
             s += path;
             s += " does not exist on SD Card\"}";
             _webserver->send(200, "application/json", s);
+            SD.end();
             return;
         }
         if (list_files) {
@@ -1426,6 +1397,7 @@ namespace WebUI {
         _webserver->send(200, "application/json", jsonfile);
         _upload_status = UploadStatusType::NONE;
         set_sd_state(SDCARD_IDLE);
+        SD.end();
     }
 
     //SD File upload with direct access to SD///////////////////////////////
@@ -1543,6 +1515,7 @@ namespace WebUI {
                     if (sdUploadFile) {
                         sdUploadFile.close();
                     }
+                    SD.end();
                     return;
                 }
             }
diff --git a/Grbl_Esp32/src/WebUI/WebServer.h b/Grbl_Esp32/src/WebUI/WebServer.h
index 9d8eca30..c4bd8745 100644
--- a/Grbl_Esp32/src/WebUI/WebServer.h
+++ b/Grbl_Esp32/src/WebUI/WebServer.h
@@ -87,7 +87,6 @@ namespace WebUI {
         static void handleFileList();
         static void handleUpdate();
         static void WebUpdateUpload();
-        static bool is_realtime_cmd(char c);
         static void pushError(int code, const char* st, bool web_error = 500, uint16_t timeout = 1000);
         static void cancelUpload();
 #ifdef ENABLE_SD_CARD
diff --git a/Grbl_Esp32/src/WebUI/WebSettings.cpp b/Grbl_Esp32/src/WebUI/WebSettings.cpp
index d795660a..09539a77 100644
--- a/Grbl_Esp32/src/WebUI/WebSettings.cpp
+++ b/Grbl_Esp32/src/WebUI/WebSettings.cpp
@@ -288,7 +288,11 @@ namespace WebUI {
         return Error::Ok;
     }
 
-    static Error runFile(char* parameter, AuthenticationLevel auth_level) {  // ESP700
+    static Error runLocalFile(char* parameter, AuthenticationLevel auth_level) {  // ESP700
+        if (sys.state != State::Idle) {
+            webPrintln("Busy");
+            return Error::IdleError;
+        }
         String path = trim(parameter);
         if ((path.length() > 0) && (path[0] != '/')) {
             path = "/" + path;
@@ -304,14 +308,13 @@ namespace WebUI {
         //until no line in file
         Error err;
         Error accumErr = Error::Ok;
+        uint8_t client = (espresponse) ? espresponse->client() : CLIENT_ALL;
         while (currentfile.available()) {
             String currentline = currentfile.readStringUntil('\n');
             if (currentline.length() > 0) {
                 byte line[256];
                 currentline.getBytes(line, 255);
-                // TODO Settings - feed into command interpreter
-                // while accumulating error codes
-                err = execute_line((char*)line, CLIENT_WEBUI, auth_level);
+                err = execute_line((char*)line, client, auth_level);
                 if (err != Error::Ok) {
                     accumErr = err;
                 }
@@ -322,6 +325,33 @@ namespace WebUI {
         return accumErr;
     }
 
+    static Error showLocalFile(char* parameter, AuthenticationLevel auth_level) {  // ESP701
+        if (sys.state != State::Idle && sys.state != State::Alarm) {
+            return Error::IdleError;
+        }
+        String path = trim(parameter);
+        if ((path.length() > 0) && (path[0] != '/')) {
+            path = "/" + path;
+        }
+        if (!SPIFFS.exists(path)) {
+            webPrintln("Error: No such file!");
+            return Error::SdFileNotFound;
+        }
+        File currentfile = SPIFFS.open(path, FILE_READ);
+        if (!currentfile) {
+            return Error::SdFailedOpenFile;
+        }
+        while (currentfile.available()) {
+            // String currentline = currentfile.readStringUntil('\n');
+            //            if (currentline.length() > 0) {
+            //                webPrintln(currentline);
+            //            }
+            webPrintln(currentfile.readStringUntil('\n'));
+        }
+        currentfile.close();
+        return Error::Ok;
+    }
+
 #ifdef ENABLE_NOTIFICATIONS
     static Error showSetNotification(char* parameter, AuthenticationLevel auth_level) {  // ESP610
         if (*parameter == '\0') {
@@ -570,9 +600,9 @@ namespace WebUI {
 
 #ifdef ENABLE_WIFI
     static Error listAPs(char* parameter, AuthenticationLevel auth_level) {  // ESP410
-        JSONencoder* j = new JSONencoder(espresponse->client() != CLIENT_WEBUI);
-        j->begin();
-        j->begin_array("AP_LIST");
+        JSONencoder j(espresponse->client() != CLIENT_WEBUI);
+        j.begin();
+        j.begin_array("AP_LIST");
         // An initial async scanNetworks was issued at startup, so there
         // is a good chance that scan information is already available.
         int n = WiFi.scanComplete();
@@ -584,12 +614,12 @@ namespace WebUI {
                 break;
             default:
                 for (int i = 0; i < n; ++i) {
-                    j->begin_object();
-                    j->member("SSID", WiFi.SSID(i));
-                    j->member("SIGNAL", wifi_config.getSignal(WiFi.RSSI(i)));
-                    j->member("IS_PROTECTED", WiFi.encryptionType(i) != WIFI_AUTH_OPEN);
+                    j.begin_object();
+                    j.member("SSID", WiFi.SSID(i));
+                    j.member("SIGNAL", wifi_config.getSignal(WiFi.RSSI(i)));
+                    j.member("IS_PROTECTED", WiFi.encryptionType(i) != WIFI_AUTH_OPEN);
                     //            j->member("IS_PROTECTED", WiFi.encryptionType(i) == WIFI_AUTH_OPEN ? "0" : "1");
-                    j->end_object();
+                    j.end_object();
                 }
                 WiFi.scanDelete();
                 // Restart the scan in async mode so new data will be available
@@ -600,9 +630,8 @@ namespace WebUI {
                 }
                 break;
         }
-        j->end_array();
-        webPrint(j->end());
-        delete j;
+        j.end_array();
+        webPrint(j.end());
         if (espresponse->client() != CLIENT_WEBUI) {
             espresponse->println("");
         }
@@ -627,27 +656,29 @@ namespace WebUI {
     }
 
     static Error listSettings(char* parameter, AuthenticationLevel auth_level) {  // ESP400
-        JSONencoder* j = new JSONencoder(espresponse->client() != CLIENT_WEBUI);
-        j->begin();
-        j->begin_array("EEPROM");
+        JSONencoder j(espresponse->client() != CLIENT_WEBUI);
+        j.begin();
+        j.begin_array("EEPROM");
         for (Setting* js = Setting::List; js; js = js->next()) {
             if (js->getType() == WEBSET) {
-                js->addWebui(j);
+                js->addWebui(&j);
             }
         }
-        j->end_array();
-        webPrint(j->end());
-        delete j;
+        j.end_array();
+        webPrint(j.end());
         return Error::Ok;
     }
 
 #ifdef ENABLE_SD_CARD
-    static Error runSDFile(char* parameter, AuthenticationLevel auth_level) {  // ESP220
-        parameter = trim(parameter);
+    static Error openSDFile(char* parameter) {
         if (*parameter == '\0') {
             webPrintln("Missing file name!");
             return Error::InvalidValue;
         }
+        String path = trim(parameter);
+        if (path[0] != '/') {
+            path = "/" + path;
+        }
         int8_t state = get_sd_state(true);
         if (state != SDCARD_IDLE) {
             if (state == SDCARD_NOT_PRESENT) {
@@ -658,14 +689,39 @@ namespace WebUI {
                 return Error::SdFailedBusy;
             }
         }
+        if (!openFile(SD, path.c_str())) {
+            report_status_message(Error::SdFailedRead, (espresponse) ? espresponse->client() : CLIENT_ALL);
+            webPrintln("");
+            return Error::SdFailedOpenFile;
+        }
+        return Error::Ok;
+    }
+    static Error showSDFile(char* parameter, AuthenticationLevel auth_level) {  // ESP221
+        if (sys.state != State::Idle && sys.state != State::Alarm) {
+            return Error::IdleError;
+        }
+        Error err;
+        if ((err = openSDFile(parameter)) != Error::Ok) {
+            return err;
+        }
+        SD_client = (espresponse) ? espresponse->client() : CLIENT_ALL;
+        char fileLine[255];
+        while (readFileLine(fileLine, 255)) {
+            webPrintln(fileLine);
+        }
+        webPrintln("");
+        closeFile();
+        return Error::Ok;
+    }
+
+    static Error runSDFile(char* parameter, AuthenticationLevel auth_level) {  // ESP220
+        Error err;
         if (sys.state != State::Idle) {
             webPrintln("Busy");
             return Error::IdleError;
         }
-        if (!openFile(SD, parameter)) {
-            report_status_message(Error::SdFailedRead, (espresponse) ? espresponse->client() : CLIENT_ALL);
-            webPrintln("");
-            return Error::Ok;
+        if ((err = openSDFile(parameter)) != Error::Ok) {
+            return err;
         }
         char fileLine[255];
         if (!readFileLine(fileLine, 255)) {
@@ -675,9 +731,10 @@ namespace WebUI {
             return Error::Ok;
         }
         SD_client = (espresponse) ? espresponse->client() : CLIENT_ALL;
-        report_status_message(gc_execute_line(fileLine, (espresponse) ? espresponse->client() : CLIENT_ALL),
-                              (espresponse) ? espresponse->client() : CLIENT_ALL);  // execute the first line
-        report_realtime_status((espresponse) ? espresponse->client() : CLIENT_ALL);
+        SD_auth_level = auth_level;
+        // execute the first line now; Protocol.cpp handles later ones when SD_ready_next
+        report_status_message(execute_line(fileLine, SD_client, SD_auth_level), SD_client);
+        report_realtime_status(SD_client);
         webPrintln("");
         return Error::Ok;
     }
@@ -737,6 +794,7 @@ namespace WebUI {
         ssd += " Total:" + ESPResponseStream::formatBytes(SD.totalBytes());
         ssd += "]";
         webPrintln(ssd);
+        SD.end();
         return Error::Ok;
     }
 #endif
@@ -773,15 +831,15 @@ namespace WebUI {
     }
 
     static Error listLocalFilesJSON(char* parameter, AuthenticationLevel auth_level) {  // No ESP command
-        JSONencoder* j = new JSONencoder(espresponse->client() != CLIENT_WEBUI);
-        j->begin();
-        j->begin_array("files");
-        listDirJSON(SPIFFS, "/", 4, j);
-        j->end_array();
-        j->member("total", SPIFFS.totalBytes());
-        j->member("used", SPIFFS.usedBytes());
-        j->member("occupation", String(100 * SPIFFS.usedBytes() / SPIFFS.totalBytes()));
-        webPrint(j->end());
+        JSONencoder j(espresponse->client() != CLIENT_WEBUI);
+        j.begin();
+        j.begin_array("files");
+        listDirJSON(SPIFFS, "/", 4, &j);
+        j.end_array();
+        j.member("total", SPIFFS.totalBytes());
+        j.member("used", SPIFFS.usedBytes());
+        j.member("occupation", String(100 * SPIFFS.usedBytes() / SPIFFS.totalBytes()));
+        webPrint(j.end());
         if (espresponse->client() != CLIENT_WEBUI) {
             webPrintln("");
         }
@@ -961,7 +1019,8 @@ namespace WebUI {
         new WebCommand(NULL, WEBCMD, WG, "ESP800", "Firmware/Info", showFwInfo);
         new WebCommand(NULL, WEBCMD, WU, "ESP720", "LocalFS/Size", SPIFFSSize);
         new WebCommand("FORMAT", WEBCMD, WA, "ESP710", "LocalFS/Format", formatSpiffs);
-        new WebCommand("path", WEBCMD, WU, "ESP700", "LocalFS/Run", runFile);
+        new WebCommand("path", WEBCMD, WU, "ESP701", "LocalFS/Show", showLocalFile);
+        new WebCommand("path", WEBCMD, WU, "ESP700", "LocalFS/Run", runLocalFile);
         new WebCommand("path", WEBCMD, WU, NULL, "LocalFS/List", listLocalFiles);
         new WebCommand("path", WEBCMD, WU, NULL, "LocalFS/ListJSON", listLocalFilesJSON);
 #endif
@@ -985,6 +1044,7 @@ namespace WebUI {
         new WebCommand(NULL, WEBCMD, WU, "ESP400", "WebUI/List", listSettings);
 #endif
 #ifdef ENABLE_SD_CARD
+        new WebCommand("path", WEBCMD, WU, "ESP221", "SD/Show", showSDFile);
         new WebCommand("path", WEBCMD, WU, "ESP220", "SD/Run", runSDFile);
         new WebCommand("file_or_directory_path", WEBCMD, WU, "ESP215", "SD/Delete", deleteSDObject);
         new WebCommand(NULL, WEBCMD, WU, "ESP210", "SD/List", listSDFiles);
diff --git a/debug.ini b/debug.ini
index 0d148f7f..db39152f 100644
--- a/debug.ini
+++ b/debug.ini
@@ -6,14 +6,18 @@
 upload_port = COM3
 monitor_port = COM3
 ; macOS
-;upload_port = /dev/cu.usbserial-*
-;monitor_port = /dev/cu.usbserial-*
+;upload_port = /dev/cu.SLAB_USBtoUART
+;monitor_port = /dev/cu.SLAB_USBtoUART
+; macOS ESP-Prog
+;upload_port = /dev/cu.usbserial-14200
+;monitor_port = /dev/cu.usbserial-14201
+;upload_protocol = esp-prog
 ; Linux
 ;upload_port = /dev/ttyUSB*
 ;monitor_port = /dev/ttyUSB*
 build_flags =
     ${common.build_flags}
-	-DMACHINE_FILENAME=test_drive.h 
+	-DMACHINE_FILENAME=test_drive.h
 
 [env:debug]
 ; You can switch between debugging tools using debug_tool option
diff --git a/platformio.ini b/platformio.ini
index c7f14c90..cdd2e8a0 100644
--- a/platformio.ini
+++ b/platformio.ini
@@ -28,6 +28,8 @@ build_flags =
 	-DCORE_DEBUG_LEVEL=0
 	-Wno-unused-variable
 	-Wno-unused-function
+        ;-DDEBUG_REPORT_HEAP_SIZE
+        ;-DDEBUG_REPORT_STACK_FREE
 
 [env]
 lib_deps =
@@ -49,7 +51,7 @@ board_build.flash_mode = qio
 build_flags = ${common.build_flags}
 src_filter =
     +<*.h> +<*.s> +<*.S> +<*.cpp> +<*.c> +<*.ino> +<src/>
-    -<.git/> -<data/> -<test/> -<tests/> -<Custom/>
+    -<.git/> -<data/> -<test/> -<tests/>
 
 [env:release]