Added experimental support for M70-M73, save and restore of modal state.

Added experimental support of LinuxCNC style subroutines.
Available for gcode run from local filesystem such as on a SD card or in littlefs.
Improved handling of G92 when G92 offset is changed while motion is ongoing. Ref. issue #241.
Fix for issue #521, crash when running G65 macro on ESP32.
"Hardened" stream switching code, likely fix for discussion #456.

README.md

@@ -13,7 +13,7 @@ It has been written to complement grblHAL and has features such as proper keyboa
 
 ---
 
-Latest build date is 20240513, see the [changelog](changelog.md) for details.  
+Latest build date is 20240526, see the [changelog](changelog.md) for details.  
 
 __NOTE:__ Build 20240222 has moved the probe input to the ioPorts pool of inputs and will be allocated from it when configured.
 The change is major and _potentially dangerous_, it may damage your probe, so please _verify correct operation_ after installing this, or later, builds.
@@ -78,8 +78,9 @@ This is a port/rewrite of [grbl 1.1f](https://github.com/gnea/grbl) and should b
   - Tool Change: M6* (Two modes possible: manual** - supports jogging, ATC), M61
   - Switches: M48, M49, M50, M51, M53
   - Input/output control***: M62, M63, M64, M65, M66, M67, M68
+  - Modal state handling*: M70, M71, M72, M73
   - Return from macro*****: M99
-  - Valid Non-Command Words: A*, B*, C*, D, E*, F, H*, I, J, K, L, N, P, Q*, R, S, T, U*, V*, W*, X, Y, Z
+  - Valid Non-Command Words: A*, B*, C*, D, E*, F, H*, I, J, K, L, N, O*, P, Q*, R, S, T, U*, V*, W*, X, Y, Z
 
   * driver/configuration dependent. W axis only available when ABC axes are remapped to UVW or when lathe UVW mode is enabled.
   ** requires compatible GCode sender due to protocol extensions, new state and RT command.
@@ -93,4 +94,4 @@ G/M-codes not supported by [legacy Grbl](https://github.com/gnea/grbl/wiki) are
 Some [plugins](https://github.com/grblHAL/plugins) implements additional M-codes.
 
 ---
-20240402
+20240523

changelog.md

@@ -1,5 +1,35 @@
 ## grblHAL changelog
 
+<a name="20240526"/>Build 20240526
+
+Core:
+
+* Added experimental support for M70-M73, save and restore of modal state.
+
+* Added experimental support of LinuxCNC style subroutines.
+Available for gcode run from local filesystem such as on a SD card or in littlefs.
+
+* Improved handling of G92 when G92 offset is changed while motion is ongoing.
+Position and WCO offset in the realtime report will now be the actual realtime values and not
+the values based on the parser state which may be quite a bit ahead of the machine. Ref. [issue #241](https://github.com/grblHAL/core/discussions/241#discussioncomment-9463390).
+
+* Fix for [issue #521](https://github.com/grblHAL/core/issues/521), crash when running G65 macro on ESP32.
+
+* "Hardened" stream switching code, likely fix for [discussion #456](https://github.com/grblHAL/core/discussions/456#discussioncomment-9533613).
+
+Drivers:
+
+* STM32F7xx: added initial support for WizNet W5500 and W5100S ethernet modules. Stack starts up but fails later, likely due to SPI issue?.
+To be completed later.
+
+Plugins:
+
+* Keypad, OpenPNP and Encoder: updated for core signature change related to improved G92 handling.
+
+* Networking: fixed incorrect signature of WizNet ethernet init function.
+
+---
+
 <a name="20240513"/>Build 20240513
 
 Core:

gcode.c

@@ -105,7 +105,7 @@ typedef union {
 } ijk_words_t;
 
 // Declare gc extern struct
-parser_state_t gc_state, *saved_state = NULL;
+parser_state_t gc_state;
 
 #define FAIL(status) return(status);
 
@@ -186,9 +186,16 @@ axes_signals_t gc_get_g51_state (void)
     return scaled;
 }
 
-float gc_get_offset (uint_fast8_t idx)
+float gc_get_offset (uint_fast8_t idx, bool real_time)
 {
-    return gc_state.modal.coord_system.xyz[idx] + gc_state.g92_coord_offset[idx] + gc_state.tool_length_offset[idx];
+    offset_id_t offset_id;
+
+    if(real_time &&
+        !(settings.status_report.machine_position && settings.status_report.sync_on_wco_change) &&
+          (offset_id = st_get_offset_id()) >= 0)
+        return gc_state.modal.coord_system.xyz[idx] + gc_state.offset_queue[offset_id].values[idx] + gc_state.tool_length_offset[idx];
+    else
+        return gc_state.modal.coord_system.xyz[idx] + gc_state.g92_coord_offset[idx] + gc_state.tool_length_offset[idx];
 }
 
 inline static float gc_get_block_offset (parser_block_t *gc_block, uint_fast8_t idx)
@@ -299,8 +306,12 @@ void gc_init (void)
     if (!settings_read_coord_data(gc_state.modal.coord_system.id, &gc_state.modal.coord_system.xyz))
         grbl.report.status_message(Status_SettingReadFail);
 
-    if (sys.cold_start && !settings.flags.g92_is_volatile && !settings_read_coord_data(CoordinateSystem_G92, &gc_state.g92_coord_offset))
-        grbl.report.status_message(Status_SettingReadFail);
+    if(sys.cold_start && !settings.flags.g92_is_volatile) {
+        if(!settings_read_coord_data(CoordinateSystem_G92, &gc_state.g92_coord_offset))
+            grbl.report.status_message(Status_SettingReadFail);
+        else
+            memcpy(&gc_state.offset_queue[gc_state.offset_id], &gc_state.g92_coord_offset, sizeof(coord_data_t));
+    }
 
     if(grbl.on_wco_changed && (!sys.cold_start ||
                                 !is0_position_vector(gc_state.modal.coord_system.xyz) ||
@@ -310,6 +321,9 @@ void gc_init (void)
 #if NGC_EXPRESSIONS_ENABLE
     ngc_flowctrl_init();
 #endif
+#if NGC_PARAMETERS_ENABLE
+    ngc_modal_state_invalidate();
+#endif
 
 //    if(settings.flags.lathe_mode)
 //        gc_state.modal.plane_select = PlaneSelect_ZX;
@@ -349,6 +363,13 @@ spindle_ptrs_t *gc_spindle_get (void)
     return gc_state.spindle.hal;
 }
 
+static void add_offset (void)
+{
+    gc_state.offset_id = (gc_state.offset_id + 1) & (MAX_OFFSET_ENTRIES - 1);
+    memcpy(&gc_state.offset_queue[gc_state.offset_id], &gc_state.g92_coord_offset, sizeof(coord_data_t));
+    system_flag_wco_change();
+}
+
 static tool_data_t *tool_get_pending (tool_id_t tool_id)
 {
     static tool_data_t tool_data = {0};
@@ -454,8 +475,15 @@ static status_code_t read_parameter (char *line, uint_fast8_t *char_counter, flo
             (*char_counter)++;
             char *pos = line = line + *char_counter;
 
-            while(*line && *line != '>')
-                line++;
+            while(*line && *line != '>') {
+                if(*line == ' ') {
+                    char *s1 = line, *s2 = line + 1;
+                    while(*s2)
+                        *s1++ = *s2++;
+                    *(--s2) = '\0';
+                } else
+                    line++;
+            }
 
             *char_counter += line - pos + 1;
 
@@ -467,7 +495,7 @@ static status_code_t read_parameter (char *line, uint_fast8_t *char_counter, flo
             } else
                 status = Status_BadNumberFormat;
 
-        } else if (read_float(line, char_counter, value)) {
+        } else if(read_float(line, char_counter, value)) {
             if(!ngc_param_get((ngc_param_id_t)*value, value))
                 status = Status_BadNumberFormat;
         } else
@@ -481,8 +509,39 @@ static status_code_t read_parameter (char *line, uint_fast8_t *char_counter, flo
     return status;
 }
 
+
 #endif // NGC_EXPRESSIONS_ENABLE
 
+#if NGC_PARAMETERS_ENABLE
+
+bool gc_modal_state_restore (gc_modal_t *copy)
+{
+    bool ok = false;
+
+    if((ok = !!copy && !ABORTED)) {
+
+        copy->auto_restore = false;
+        copy->motion = gc_state.modal.motion;
+
+        if(copy->coolant.value != gc_state.modal.coolant.value) {
+            hal.coolant.set_state(copy->coolant);
+            delay_sec(settings.safety_door.coolant_on_delay, DelayMode_SysSuspend);
+        }
+
+        if(copy->spindle.state.value != gc_state.modal.spindle.state.value || copy->rpm != gc_state.modal.rpm)
+            spindle_restore(gc_state.spindle.hal, copy->spindle.state, copy->rpm);
+
+        memcpy(&gc_state.modal, copy, sizeof(gc_modal_t));
+
+        gc_state.spindle.rpm = gc_state.modal.rpm;
+        gc_state.feed_rate = gc_state.modal.feed_rate;
+    }
+
+    return ok;
+}
+
+#endif // NGC_PARAMETERS_ENABLE
+
 // Remove whitespace, control characters, comments and if block delete is active block delete lines
 // else the block delete character. Remaining characters are converted to upper case.
 // If the driver handles message comments then the first is extracted and returned in a dynamically
@@ -561,7 +620,7 @@ char *gc_normalize_block (char *block, char **message)
                                     if(c == '#') {
                                         char_counter--;
                                         if(read_parameter(comment, &char_counter, &value) == Status_OK)
-                                            len += strlen(ftoa(value, 6));
+                                            len += strlen(trim_float(ftoa(value, 6)));
                                         else
                                             len += 3; // "N/A"
                                     } else
@@ -578,7 +637,7 @@ char *gc_normalize_block (char *block, char **message)
                                         if(c == '#') {
                                             char_counter--;
                                             if(read_parameter(comment, &char_counter, &value) == Status_OK)
-                                                strcat(s3, ftoa(value, 6));
+                                                strcat(s3, trim_float(ftoa(value, 6)));
                                             else
                                                 strcat(s3, "N/A");
                                             s3 = strchr(s3, '\0');
@@ -1263,30 +1322,13 @@ status_code_t gc_execute_block (char *block)
                         word_bit.modal_group.M10 = On;
                         port_command = (io_mcode_t)int_value;
                         break;
-/*
-                    case 70:
-                        if(!saved_state)
-                            saved_state = malloc(sizeof(parser_state_t));
-                        if(!saved_state)
-                            FAIL(Status_GcodeUnsupportedCommand); // [Unsupported M command]
-                        memcpy(saved_state, &gc_state, sizeof(parser_state_t));
-                        return Status_OK;
 
-                    case 71: // Invalidate saved state
-                        if(saved_state) {
-                            free(saved_state);
-                            saved_state = NULL;
-                        }
-                        return Status_OK; // Should fail if no state is saved...
-
-                    case 72:
-                        if(saved_state) {
-                            // TODO: restore state, need to split out execution part of parser to separate functions first?
-                            free(saved_state);
-                            saved_state = NULL;
-                        }
-                        return Status_OK;
-*/
+#if NGC_PARAMETERS_ENABLE
+                    case 70: case 71: case 72: case 73:
+                        //word_bit.modal_group.G0 = On; ??
+                        gc_block.state_action = (modal_state_action_t)int_value;
+                        break;
+#endif
 
                     case 99:
                         word_bit.modal_group.M4 = On;
@@ -2348,6 +2390,10 @@ status_code_t gc_execute_block (char *block)
                         FAIL(Status_GcodeValueWordMissing); // [P word missing]
                     if(gc_block.values.p > 65535.0f)
                         FAIL(Status_GcodeValueOutOfRange); // [P word out of range]
+#if NGC_PARAMETERS_ENABLE
+                    if(!ngc_call_push(&gc_state + ngc_call_level()))
+                        FAIL(Status_FlowControlStackOverflow); // [Call level too deep]
+#endif
 #if NGC_EXPRESSIONS_ENABLE
                     // TODO: add context for local storage?
                     {
@@ -2934,6 +2980,7 @@ status_code_t gc_execute_block (char *block)
     // Initialize planner data struct for motion blocks.
     plan_line_data_t plan_data;
     memset(&plan_data, 0, sizeof(plan_line_data_t)); // Zero plan_data struct
+    plan_data.offset_id = gc_state.offset_id;
     plan_data.condition.target_validated = plan_data.condition.target_valid = sys.soft_limits.mask == 0;
 
     // Intercept jog commands and complete error checking for valid jog commands and execute.
@@ -3011,7 +3058,7 @@ status_code_t gc_execute_block (char *block)
         if(gc_block.modal.motion != MotionMode_None && gc_block.modal.motion != MotionMode_Seek) {
             gc_state.spindle.css = &gc_state.spindle.hal->param->css;
             gc_state.spindle.css->axis = plane.axis_1;
-            gc_state.spindle.css->tool_offset = gc_get_offset(gc_state.spindle.css->axis);
+            gc_state.spindle.css->tool_offset = gc_get_offset(gc_state.spindle.css->axis, false);
             float pos = gc_state.position[gc_state.spindle.css->axis] - gc_state.spindle.css->tool_offset;
             gc_block.values.s = pos <= 0.0f ? gc_state.spindle.css->max_rpm : min(gc_state.spindle.css->max_rpm, gc_state.spindle.css->surface_speed / (pos * (float)(2.0f * M_PI)));
 //??            gc_parser_flags.spindle_force_sync = On;
@@ -3198,6 +3245,33 @@ status_code_t gc_execute_block (char *block)
     plan_data.condition.is_rpm_rate_adjusted = gc_state.is_rpm_rate_adjusted;
     plan_data.condition.is_laser_ppi_mode = gc_state.is_rpm_rate_adjusted && gc_state.is_laser_ppi_mode;
 
+#if NGC_PARAMETERS_ENABLE
+
+    // [7a. Modal state actions ]:
+    switch(gc_block.state_action) {
+
+        case ModalState_Save:
+        case ModalState_SaveAutoRestore:
+            gc_state.modal.rpm = gc_state.spindle.rpm;
+            gc_state.modal.feed_rate = gc_state.feed_rate;
+            if(!ngc_modal_state_save(&gc_state.modal, gc_block.state_action == ModalState_SaveAutoRestore))
+                FAIL(Status_FlowControlOutOfMemory); // [Out of memory] TODO: allocate memory during validation? Static allocation?
+            break;
+
+        case ModalState_Invalidate:
+            ngc_modal_state_invalidate();
+            break;
+
+        case ModalState_Restore:
+            ngc_modal_state_restore();
+            break;
+
+        default:
+            break;
+    }
+
+#endif // NGC_PARAMETERS_ENABLE
+
     // [8. Coolant control ]:
     if (gc_parser_flags.set_coolant && gc_state.modal.coolant.value != gc_block.modal.coolant.value) {
     // NOTE: Coolant M-codes are modal. Only one command per line is allowed. But, multiple states
@@ -3370,7 +3444,7 @@ status_code_t gc_execute_block (char *block)
             memcpy(gc_state.g92_coord_offset, gc_block.values.xyz, sizeof(gc_state.g92_coord_offset));
             if(!settings.flags.g92_is_volatile)
                 settings_write_coord_data(CoordinateSystem_G92, &gc_state.g92_coord_offset); // Save G92 offsets to non-volatile storage
-            system_flag_wco_change();
+            add_offset();
             break;
 
         case NonModal_ResetCoordinateOffset: // G92.1
@@ -3378,19 +3452,19 @@ status_code_t gc_execute_block (char *block)
             clear_vector(gc_state.g92_coord_offset); // Disable G92 offsets by zeroing offset vector.
             if(!settings.flags.g92_is_volatile)
                 settings_write_coord_data(CoordinateSystem_G92, &gc_state.g92_coord_offset); // Save G92 offsets to non-volatile storage
-            system_flag_wco_change();
+            add_offset();
             break;
 
         case NonModal_ClearCoordinateOffset: // G92.2
             gc_state.g92_coord_offset_applied = false;
             clear_vector(gc_state.g92_coord_offset); // Disable G92 offsets by zeroing offset vector.
-            system_flag_wco_change();
+            add_offset();
             break;
 
         case NonModal_RestoreCoordinateOffset: // G92.3
             gc_state.g92_coord_offset_applied = true; // TODO: check for all zero?
             settings_read_coord_data(CoordinateSystem_G92, &gc_state.g92_coord_offset); // Restore G92 offsets from non-volatile storage
-            system_flag_wco_change();
+            add_offset();
             break;
 
         default:
@@ -3647,6 +3721,10 @@ status_code_t gc_execute_block (char *block)
                 output_commands = next;
             }
 
+#if NGC_PARAMETERS_ENABLE
+            ngc_modal_state_invalidate();
+#endif
+
             grbl.report.feedback_message(Message_ProgramEnd);
         }
         gc_state.modal.program_flow = ProgramFlow_Running; // Reset program flow.

gcode.h

@@ -3,22 +3,22 @@
 
   Part of grblHAL
 
-  Copyright (c) 2017-2023 Terje Io
+  Copyright (c) 2017-2024 Terje Io
   Copyright (c) 2011-2016 Sungeun K. Jeon for Gnea Research LLC
   Copyright (c) 2009-2011 Simen Svale Skogsrud
 
-  Grbl is free software: you can redistribute it and/or modify
+  grblHAL 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,
+  grblHAL 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
+  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/>.
+  along with grblHAL. If not, see <http://www.gnu.org/licenses/>.
 */
 
 #ifndef _GCODE_H_
@@ -29,8 +29,11 @@
 #include "spindle_control.h"
 #include "errors.h"
 
+#define MAX_OFFSET_ENTRIES 4 // must be a power of 2
+
 typedef uint32_t tool_id_t;
 typedef uint16_t macro_id_t;
+typedef int8_t offset_id_t;
 
 // Define command actions for within execution-type modal groups (motion, stopping, non-modal). Used
 // internally by the parser to know which command to execute.
@@ -59,6 +62,15 @@ typedef enum {
     NonModal_RestoreCoordinateOffset = 122  //!< 122 - G92.3
 } non_modal_t;
 
+
+typedef enum {
+    ModalState_NoAction = 0,                //!< 0 - Default, must be zero
+    ModalState_Save = 70,                   //!< 70 - M70
+    ModalState_Invalidate = 71,             //!< 71 - M71
+    ModalState_Restore = 72,                //!< 72 - M72
+    ModalState_SaveAutoRestore = 73,        //!< 73 - M73
+} modal_state_action_t;
+
 /*! Modal Group G1: Motion modes
 
 Do not alter values!
@@ -411,7 +423,7 @@ typedef struct {
     coord_system_t coord_data; //!< Coordinate data
     int32_t $;                 //!< Spindle id - single-meaning word
     int32_t n;                 //!< Line number - single-meaning word
-    uint32_t o;                //!< Subroutine identifier - single-meaning word (not used by the core)
+    uint32_t o;                //!< Subroutine identifier - single-meaning word
     uint32_t h;                //!< Tool number or number of G76 thread spring passes
     tool_id_t t;               //!< Tool selection - single-meaning word
     uint8_t l;                 //!< G10 or canned cycles parameters
@@ -493,6 +505,11 @@ typedef struct {
     bool scaling_active;                 //!< {G50,G51}
     bool canned_cycle_active;
     float spline_pq[2];                  //!< {G5}
+#if NGC_PARAMETERS_ENABLE
+    bool auto_restore;
+    float feed_rate;                     //!< {F} NOTE: only set when saving modal state
+    float rpm;                           //!< {S} NOTE: only set when saving modal state
+#endif
 } gc_modal_t;
 
 //! Data for canned cycles.
@@ -570,6 +587,8 @@ typedef struct {
     bool tool_change;
     bool skip_blocks;                   //!< true if skipping conditional blocks
     status_code_t last_error;           //!< last return value from parser
+    offset_id_t offset_id;              //!< id(x) of last G92 coordinate offset (into circular buffer)
+    coord_data_t offset_queue[MAX_OFFSET_ENTRIES];
     //!< The following variables are not cleared upon warm restart when COMPATIBILITY_LEVEL <= 1
     bool g92_coord_offset_applied;      //!< true when G92 offset applied
     float g92_coord_offset[N_AXIS];     //!< Retains the G92 coordinate offset (work coordinates) relative to
@@ -602,6 +621,9 @@ typedef struct {
     output_command_t output_command;    //!< Details about M62-M68 output command to execute if present in block.
     uint32_t arc_turns;                 //
     spindle_ptrs_t *spindle;            //!< Spindle to control, NULL for all
+#if NGC_PARAMETERS_ENABLE
+    modal_state_action_t state_action;  //!< M70-M73 modal state action
+#endif
 } parser_block_t;
 
 // Initialize the parser
@@ -629,7 +651,7 @@ axes_signals_t gc_get_g51_state (void);
 float *gc_get_scaling (void);
 
 // Get current axis offset.
-float gc_get_offset (uint_fast8_t idx);
+float gc_get_offset (uint_fast8_t idx, bool real_time);
 
 spindle_ptrs_t *gc_spindle_get (void);
 
@@ -639,4 +661,8 @@ void gc_coolant (coolant_state_t state);
 void gc_set_tool_offset (tool_offset_mode_t mode, uint_fast8_t idx, int32_t offset);
 plane_t *gc_get_plane_data (plane_t *plane, plane_select_t select);
 
+#if NGC_PARAMETERS_ENABLE
+bool gc_modal_state_restore (gc_modal_t *copy);
 #endif
+
+#endif // _GCODE_H_

grbl.h

@@ -42,7 +42,7 @@
 #else
 #define GRBL_VERSION "1.1f"
 #endif
-#define GRBL_BUILD 20240513
+#define GRBL_BUILD 20240526
 
 #define GRBL_URL "https://github.com/grblHAL"
 

kinematics/polar.c

@@ -135,7 +135,7 @@ static float *polar_segment_line (float *target, float *position, plan_line_data
     if(init) {
 
         jog_cancel = false;
-        r_offset = gc_get_offset(RADIUS_AXIS) * 2.0f; //??
+        r_offset = gc_get_offset(RADIUS_AXIS, false) * 2.0f; //??
 
         memcpy(final_target.values, target, sizeof(final_target));
 
@@ -229,7 +229,7 @@ static void report_options (bool newopt)
     on_report_options(newopt);
 
     if(!newopt)
-        hal.stream.write("[KINEMATICS:Polar v0.02]" ASCII_EOL);
+        hal.stream.write("[KINEMATICS:Polar v0.03]" ASCII_EOL);
 }
 
 static bool polar_homing_cycle (axes_signals_t cycle, axes_signals_t auto_square)

ngc_flowctrl.c

@@ -32,7 +32,7 @@
 #include "ngc_params.h"
 
 #ifndef NGC_STACK_DEPTH
-#define NGC_STACK_DEPTH 10
+#define NGC_STACK_DEPTH 20
 #endif
 
 typedef enum {
@@ -48,14 +48,25 @@ typedef enum {
     NGCFlowCtrl_EndWhile,
     NGCFlowCtrl_Repeat,
     NGCFlowCtrl_EndRepeat,
+    NGCFlowCtrl_Sub,
+    NGCFlowCtrl_EndSub,
+    NGCFlowCtrl_Call,
     NGCFlowCtrl_Return,
     NGCFlowCtrl_RaiseAlarm,
     NGCFlowCtrl_RaiseError
 } ngc_cmd_t;
 
+typedef struct ngc_sub {
+    uint32_t o_label;
+    vfs_file_t *file;
+    size_t file_pos;
+    struct ngc_sub *next;
+} ngc_sub_t;
+
 typedef struct {
     uint32_t o_label;
     ngc_cmd_t operation;
+    ngc_sub_t *sub;
     vfs_file_t *file;
     size_t file_pos;
     char *expr;
@@ -66,6 +77,8 @@ typedef struct {
 } ngc_stack_entry_t;
 
 static volatile int_fast8_t stack_idx = -1;
+static bool skip_sub = false;
+static ngc_sub_t *subs = NULL, *exec_sub = NULL;
 static ngc_stack_entry_t stack[NGC_STACK_DEPTH] = {0};
 
 static status_code_t read_command (char *line, uint_fast8_t *pos, ngc_cmd_t *operation)
@@ -97,6 +110,9 @@ static status_code_t read_command (char *line, uint_fast8_t *pos, ngc_cmd_t *ope
             if (!strncmp(line + *pos, "ONTINUE", 7)) {
                 *operation = NGCFlowCtrl_Continue;
                 *pos += 7;
+            } else if (!strncmp(line + *pos, "ALL", 3)) {
+                *operation = NGCFlowCtrl_Call;
+                *pos += 3;
             } else
                 status = Status_FlowControlSyntaxError; // Unknown statement name starting with C
             break;
@@ -125,6 +141,9 @@ static status_code_t read_command (char *line, uint_fast8_t *pos, ngc_cmd_t *ope
             } else if (!strncmp(line + *pos, "NDREPEAT", 8)) {
                 *operation = NGCFlowCtrl_EndRepeat;
                 *pos += 8;
+            } else if (!strncmp(line + *pos, "NDSUB", 5)) {
+                *operation = NGCFlowCtrl_EndSub;
+                *pos += 5;
             } else if (!strncmp(line + *pos, "RROR", 4)) {
                 *operation = NGCFlowCtrl_RaiseError;
                 *pos += 4;
@@ -151,6 +170,14 @@ static status_code_t read_command (char *line, uint_fast8_t *pos, ngc_cmd_t *ope
                 *operation = Status_FlowControlSyntaxError; // Unknown statement name starting with R
             break;
 
+        case 'S':
+            if (!strncmp(line + *pos, "UB", 2)) {
+                *operation = NGCFlowCtrl_Sub;
+                *pos += 2;
+            } else
+                status = Status_FlowControlSyntaxError; // Unknown statement name starting with S
+            break;
+
         case 'W':
             if (!strncmp(line + *pos, "HILE", 4)) {
                 *operation = NGCFlowCtrl_While;
@@ -166,12 +193,34 @@ static status_code_t read_command (char *line, uint_fast8_t *pos, ngc_cmd_t *ope
     return status;
 }
 
+static void clear_subs (vfs_file_t *file)
+{
+    ngc_sub_t *current = subs, *prev = NULL, *next;
+
+    subs = NULL;
+
+    while(current) {
+        next = current->next;
+        if(file == NULL || file == current->file) {
+            free(current);
+            if(prev)
+                prev->next = next;
+        } else {
+            if(subs == NULL)
+                subs = current;
+            prev = current;
+        }
+        current = next;
+    }
+}
+
 static status_code_t stack_push (uint32_t o_label, ngc_cmd_t operation)
 {
-    if(stack_idx < (NGC_STACK_DEPTH - 1)) {
+    if(stack_idx < (NGC_STACK_DEPTH - 1) && (operation != NGCFlowCtrl_Call || ngc_call_push(&stack[stack_idx + 1]))) {
         stack[++stack_idx].o_label = o_label;
         stack[stack_idx].file = hal.stream.file;
         stack[stack_idx].operation = operation;
+        stack[stack_idx].sub = exec_sub;
         return Status_OK;
     }
 
@@ -185,6 +234,8 @@ static bool stack_pull (void)
     if((ok = stack_idx >= 0)) {
         if(stack[stack_idx].expr)
             free(stack[stack_idx].expr);
+        if(stack[stack_idx].operation == NGCFlowCtrl_Call)
+            ngc_call_pop();
         memset(&stack[stack_idx], 0, sizeof(ngc_stack_entry_t));
         stack_idx--;
     }
@@ -192,16 +243,31 @@ static bool stack_pull (void)
     return ok;
 }
 
+static void stack_unwind_sub (uint32_t o_label)
+{
+    while(stack_idx >= 0 && stack[stack_idx].o_label != o_label)
+        stack_pull();
+
+    if(stack_idx >= 0) {
+        vfs_seek(stack[stack_idx].file, stack[stack_idx].file_pos);
+        stack_pull();
+    }
+
+    exec_sub = stack_idx >= 0 ? stack[stack_idx].sub : NULL;
+}
+
 // Public functions
 
 void ngc_flowctrl_unwind_stack (vfs_file_t *file)
 {
+    clear_subs(file);
     while(stack_idx >= 0 && stack[stack_idx].file == file)
         stack_pull();
 }
 
 void ngc_flowctrl_init (void)
 {
+    clear_subs(NULL);
     while(stack_idx >= 0)
         stack_pull();
 }
@@ -217,8 +283,8 @@ status_code_t ngc_flowctrl (uint32_t o_label, char *line, uint_fast8_t *pos, boo
     if((status = read_command(line, pos, &operation)) != Status_OK)
         return status;
 
-    skipping = stack_idx >= 0 && stack[stack_idx].skip;
-    last_op = stack_idx >= 0 ? stack[stack_idx].operation : NGCFlowCtrl_NoOp;
+    skipping = skip_sub || (stack_idx >= 0 && stack[stack_idx].skip);
+    last_op = stack_idx >= 0 ? stack[stack_idx].operation : (skip_sub ? NGCFlowCtrl_Sub : NGCFlowCtrl_NoOp);
 
     switch(operation) {
 
@@ -421,16 +487,104 @@ status_code_t ngc_flowctrl (uint32_t o_label, char *line, uint_fast8_t *pos, boo
                 status = (status_code_t)value;
             break;
 
+        case NGCFlowCtrl_Sub:
+            if(hal.stream.file) {
+                ngc_sub_t *sub;
+                if((skip_sub = (sub = malloc(sizeof(ngc_sub_t))) != NULL)) {
+                    sub->o_label = o_label;
+                    sub->file = hal.stream.file;
+                    sub->file_pos = vfs_tell(hal.stream.file);
+                    sub->next = NULL;
+                    if(subs == NULL)
+                        subs = sub;
+                    else {
+                        ngc_sub_t *last = subs;
+                        while(last->next)
+                            last = last->next;
+                        last->next = sub;
+                    }
+                } // else out of memory
+            } else
+                status = Status_FlowControlNotExecutingMacro;
+            break;
+
+        case NGCFlowCtrl_EndSub:
+            if(hal.stream.file) {
+                if(!skip_sub)
+                    stack_unwind_sub(o_label);
+                skip_sub = false;
+            } else
+                status = Status_FlowControlNotExecutingMacro;
+            break;
+
+        case NGCFlowCtrl_Call:
+            if(hal.stream.file) {
+                if(!skipping) {
+
+                    ngc_sub_t *sub = subs;
+                    do {
+                        if(sub->o_label == o_label && sub->file == hal.stream.file)
+                            break;
+                    } while((sub = sub->next));
+
+                    if(sub == NULL)
+                        status = Status_FlowControlSyntaxError;
+                    else {
+
+                        float params[29];
+                        ngc_param_id_t param_id = 1;
+
+                        while(line[*pos] && status == Status_OK && param_id <= 30) {
+                            status = ngc_eval_expression(line, pos, &params[param_id - 1]);
+                            param_id++;
+                        }
+
+                        if(status == Status_OK && param_id < 30) do {
+                            ngc_param_get(param_id, &params[param_id - 1]);
+                        } while(++param_id <= 30);
+
+                        if(status == Status_OK && (status = stack_push(o_label, operation)) == Status_OK) {
+
+                            stack[stack_idx].sub = exec_sub = sub;
+                            stack[stack_idx].file = hal.stream.file;
+                            stack[stack_idx].file_pos = vfs_tell(hal.stream.file);
+                            stack[stack_idx].repeats = 1;
+
+                            for(param_id = 1; param_id <= 30; param_id++) {
+                                if(params[param_id - 1] != 0.0f) {
+                                    if(!ngc_param_set(param_id, params[param_id - 1]))
+                                        status = Status_FlowControlOutOfMemory;
+                                }
+                            }
+
+                            if(status == Status_OK)
+                                vfs_seek(sub->file, sub->file_pos);
+                        }
+                    }
+                }
+            } else
+                status = Status_FlowControlNotExecutingMacro;
+            break;
+
         case NGCFlowCtrl_Return:
             if(hal.stream.file) {
-                if(!skipping && grbl.on_macro_return) {
-                    ngc_flowctrl_unwind_stack(stack[stack_idx].file);
+                if(!skipping) {
+
+                    bool g65_return = false;
+
+                    if(exec_sub)
+                        stack_unwind_sub(o_label);
+                    else if((g65_return = !!grbl.on_macro_return))
+                        ngc_flowctrl_unwind_stack(stack[stack_idx].file);
+
                     if(ngc_eval_expression(line, pos, &value) == Status_OK) {
                         ngc_named_param_set("_value", value);
                         ngc_named_param_set("_value_returned", 1.0f);
                     } else
                         ngc_named_param_set("_value_returned", 0.0f);
-                    grbl.on_macro_return();
+
+                    if(g65_return)
+                        grbl.on_macro_return();
                 }
             } else
                 status = Status_FlowControlNotExecutingMacro;
@@ -446,7 +600,7 @@ status_code_t ngc_flowctrl (uint32_t o_label, char *line, uint_fast8_t *pos, boo
         if(settings.flags.ngc_debug_out)
             report_message(line, Message_Plain);
     } else
-        *skip = stack_idx >= 0 && stack[stack_idx].skip;
+        *skip = skip_sub || (stack_idx >= 0 && stack[stack_idx].skip);
 
     return status;
 }

ngc_params.c

@@ -38,7 +38,12 @@
 #include "settings.h"
 #include "ngc_params.h"
 
-#define MAX_PARAM_LENGTH 20
+#ifndef NGC_MAX_CALL_LEVEL
+#define NGC_MAX_CALL_LEVEL 10
+#endif
+#ifndef NGC_MAX_PARAM_LENGTH
+#define NGC_MAX_PARAM_LENGTH 20
+#endif
 
 typedef float (*ngc_param_get_ptr)(ngc_param_id_t id);
 typedef float (*ngc_named_param_get_ptr)(void);
@@ -50,6 +55,7 @@ typedef struct {
 } ngc_ro_param_t;
 
 typedef struct ngc_rw_param {
+    void *context;
     ngc_param_id_t id;
     float value;
     struct ngc_rw_param *next;
@@ -62,20 +68,31 @@ typedef struct {
 } ngc_named_ro_param_t;
 
 typedef struct ngc_named_rw_param {
-    char name[MAX_PARAM_LENGTH + 1];
+    void *context;
+    char name[NGC_MAX_PARAM_LENGTH + 1];
     float value;
     struct ngc_named_rw_param *next;
 } ngc_named_rw_param_t;
 
-ngc_rw_param_t *rw_params = NULL;
-ngc_named_rw_param_t *rw_global_params = NULL;
+typedef struct {
+    uint32_t level;
+    void *context;
+    gc_modal_t *modal_state;
+} ngc_param_context_t;
+
+static int32_t call_level = -1;
+static void *call_context;
+static gc_modal_t *modal_state;
+static ngc_param_context_t call_levels[NGC_MAX_CALL_LEVEL];
+static ngc_rw_param_t *rw_params = NULL;
+static ngc_named_rw_param_t *rw_global_params = NULL;
 
 static float _relative_pos (uint_fast8_t axis)
 {
     float value;
 
     if(axis < N_AXIS) {
-        value = sys.position[axis] / settings.axis[axis].steps_per_mm - gc_get_offset(axis);
+        value = sys.position[axis] / settings.axis[axis].steps_per_mm - gc_get_offset(axis, false);
         if(settings.flags.report_inches)
             value *= 25.4f;
     } else
@@ -249,9 +266,10 @@ bool ngc_param_get (ngc_param_id_t id, float *value)
     *value = 0.0f;
 
     if(found) {
+        void *context = id > (ngc_param_id_t)30 ? NULL : call_context;
         ngc_rw_param_t *rw_param = rw_params;
         while(rw_param) {
-            if(rw_param->id == id) {
+            if(rw_param->context == context && rw_param->id == id) {
                 *value = rw_param->value;
                 rw_param = NULL;
             } else
@@ -282,10 +300,11 @@ bool ngc_param_set (ngc_param_id_t id, float value)
 
     if(ok) {
 
+        void *context = id > (ngc_param_id_t)30 ? NULL : call_context;
         ngc_rw_param_t *rw_param = rw_params, *rw_param_last = rw_params;
 
         while(rw_param) {
-            if(rw_param->id == id) {
+            if(rw_param->context == context && rw_param->id == id) {
                 break;
             } else {
                 rw_param_last = rw_param;
@@ -295,6 +314,7 @@ bool ngc_param_set (ngc_param_id_t id, float value)
 
         if(rw_param == NULL && value != 0.0f && (rw_param = malloc(sizeof(ngc_rw_param_t)))) {
             rw_param->id = id;
+            rw_param->context = context;
             rw_param->next = NULL;
             if(rw_params == NULL)
                 rw_params = rw_param;
@@ -357,6 +377,7 @@ PROGMEM static const ngc_named_ro_param_t ngc_named_ro_param[] = {
     { .name = "_current_pocket",      .id = NGCParam_current_pocket },
     { .name = "_selected_tool",       .id = NGCParam_selected_tool },
     { .name = "_selected_pocket",     .id = NGCParam_selected_pocket },
+    { .name = "_call_level",          .id = NGCParam_call_level },
 };
 
 
@@ -539,6 +560,10 @@ float ngc_named_param_get_by_id (ncg_name_param_id_t id)
             value = 0.0f;
             break;
 
+        case NGCParam_call_level:
+            value = (float)ngc_call_level();
+            break;
+
         default:
             value = NAN;
     }
@@ -546,15 +571,32 @@ float ngc_named_param_get_by_id (ncg_name_param_id_t id)
     return value;
 }
 
+// Lowercase name, remove control characters and spaces
+// Assumes names stored in flash are lowercase...
+static char *ngc_name_tolower (char *name)
+{
+	char c, *s1 = name, *s2 = name;
+	bool convert = false;
+
+    while((c = *s1++) && c > ' ')
+        convert = (c & 0x20) == 0 || c <= ' ';
+
+    if(convert) {
+    	s1 = name;
+		while((c = *s1++) && c > ' ')
+			*s2++ = LCAPS(c);
+	    *s2 = '\0';
+    }
+
+	return name;
+}
+
 bool ngc_named_param_get (char *name, float *value)
 {
-    char c, *s = name;
     bool found = false;
     uint_fast8_t idx = sizeof(ngc_named_ro_param) / sizeof(ngc_named_ro_param_t);
 
-    // Lowercase name
-    while((c = *s))
-        *s++ = LCAPS(c);
+    ngc_name_tolower(name);
 
     *value = 0.0f;
 
@@ -565,9 +607,10 @@ bool ngc_named_param_get (char *name, float *value)
     } while(idx && !found);
 
     if(!found) {
+        void *context = *name == '_' ? NULL : call_context;
         ngc_named_rw_param_t *rw_param = rw_global_params;
         while(rw_param && !found) {
-            if((found = !strcmp(rw_param->name, name)))
+            if((found = rw_param->context == context && !strcmp(rw_param->name, name)))
                 *value = rw_param->value;
             else
                 rw_param = rw_param->next;
@@ -579,15 +622,10 @@ bool ngc_named_param_get (char *name, float *value)
 
 bool ngc_named_param_exists (char *name)
 {
-    char c, *s1 = name, *s2 = name;
     bool ok = false;
     uint_fast8_t idx = sizeof(ngc_named_ro_param) / sizeof(ngc_named_ro_param_t);
 
-    // Lowercase name, remove control characters and spaces
-    while((c = *s1++) && c > ' ')
-        *s2++ = LCAPS(c);
-
-    *s2 = '\0';
+    ngc_name_tolower(name);
 
     // Check if name is supplied, return false if not.
     if((*name == '_' ? *(name + 1) : *name) == '\0')
@@ -595,36 +633,31 @@ bool ngc_named_param_exists (char *name)
 
     // Check if it is a (read only) predefined parameter.
     if(*name == '_') do {
-        idx--;
-        ok = !strcmp(name, ngc_named_ro_param[idx].name);
+        ok = !strcmp(name, ngc_named_ro_param[--idx].name);
     } while(idx && !ok);
 
     // If not predefined attempt to find it.
-    if(!ok && rw_global_params && strlen(name) < MAX_PARAM_LENGTH) {
+    if(!ok && rw_global_params && strlen(name) < NGC_MAX_PARAM_LENGTH) {
 
+        void *context = *name == '_' ? NULL : call_context;
         ngc_named_rw_param_t *rw_param = rw_global_params;
 
         while(rw_param) {
-            if((ok = !strcmp(rw_param->name, name)))
+            if((ok = rw_param->context == context && !strcmp(rw_param->name, name)))
                 break;
             rw_param = rw_param->next;
-         }
-     }
+        }
+    }
 
     return ok;
 }
 
 bool ngc_named_param_set (char *name, float value)
 {
-    char c, *s1 = name, *s2 = name;
     bool ok = false;
     uint_fast8_t idx = sizeof(ngc_named_ro_param) / sizeof(ngc_named_ro_param_t);
 
-    // Lowercase name, remove control characters and spaces
-    while((c = *s1++) && c > ' ')
-        *s2++ = LCAPS(c);
-
-    *s2 = '\0';
+    ngc_name_tolower(name);
 
     // Check if name is supplied, return false if not.
     if((*name == '_' ? *(name + 1) : *name) == '\0')
@@ -637,12 +670,13 @@ bool ngc_named_param_set (char *name, float value)
     } while(idx && !ok);
 
     // If not predefined attempt to set it.
-    if(!ok && (ok = strlen(name) < MAX_PARAM_LENGTH)) {
+    if(!ok && (ok = strlen(name) < NGC_MAX_PARAM_LENGTH)) {
 
+        void *context = *name == '_' ? NULL : call_context;
         ngc_named_rw_param_t *rw_param = rw_global_params, *rw_param_last = rw_global_params;
 
          while(rw_param) {
-             if(!strcmp(rw_param->name, name)) {
+             if(rw_param->context == context && !strcmp(rw_param->name, name)) {
                  break;
              } else {
                  rw_param_last = rw_param;
@@ -652,6 +686,7 @@ bool ngc_named_param_set (char *name, float value)
 
          if(rw_param == NULL && (rw_param = malloc(sizeof(ngc_named_rw_param_t)))) {
              strcpy(rw_param->name, name);
+             rw_param->context = context;
              rw_param->next = NULL;
              if(rw_global_params == NULL)
                  rw_global_params = rw_param;
@@ -666,4 +701,101 @@ bool ngc_named_param_set (char *name, float value)
     return ok;
 }
 
+bool ngc_modal_state_save (gc_modal_t *state, bool auto_restore)
+{
+    gc_modal_t **saved_state = call_level == -1 ? &modal_state : &call_levels[call_level].modal_state;
+
+    if(*saved_state == NULL)
+        *saved_state = malloc(sizeof(gc_modal_t));
+
+    if(*saved_state)
+        memcpy(*saved_state, state, sizeof(gc_modal_t));
+
+    return *saved_state != NULL;
+}
+
+void ngc_modal_state_invalidate (void)
+{
+    gc_modal_t **saved_state = call_level == -1 ? &modal_state : &call_levels[call_level].modal_state;
+
+    if(*saved_state) {
+        free(*saved_state);
+        *saved_state = NULL;
+    }
+}
+
+bool ngc_modal_state_restore (void)
+{
+    return gc_modal_state_restore(call_level == -1 ? modal_state : call_levels[call_level].modal_state);
+}
+
+bool ngc_call_push (void *context)
+{
+    bool ok;
+
+    if((ok = call_level < (NGC_MAX_CALL_LEVEL - 1)))
+        call_levels[++call_level].context = call_context = context;
+
+    return ok;
+}
+
+bool ngc_call_pop (void)
+{
+    if(call_level >= 0) {
+
+        if(call_context) {
+
+        ngc_rw_param_t *rw_param = rw_params, *rw_param_last = rw_params;
+
+        while(rw_param) {
+            if(rw_param->context == call_context) {
+                ngc_rw_param_t *rw_param_free = rw_param;
+                rw_param = rw_param->next;
+                if(rw_param_free == rw_params)
+                    rw_params = rw_param_last = rw_param;
+                else
+                    rw_param_last->next = rw_param;
+                free(rw_param_free);
+            } else {
+                rw_param_last = rw_param;
+                rw_param = rw_param->next;
+            }
+        }
+
+        ngc_named_rw_param_t *rw_named_param = rw_global_params, *rw_named_param_last = rw_global_params;
+
+        while(rw_named_param) {
+            if(rw_named_param->context == call_context) {
+                ngc_named_rw_param_t *rw_named_param_free = rw_named_param;
+                rw_named_param = rw_named_param->next;
+                if(rw_named_param_free == rw_global_params)
+                    rw_global_params = rw_named_param_last = rw_named_param;
+                else
+                    rw_named_param_last->next = rw_named_param;
+                free(rw_named_param_free);
+            } else {
+                rw_named_param_last = rw_named_param;
+                rw_named_param = rw_named_param->next;
+            }
+        }
+        }
+
+        if(call_levels[call_level].modal_state) {
+            if(call_levels[call_level].modal_state->auto_restore)
+                gc_modal_state_restore(call_levels[call_level].modal_state);
+            free(call_levels[call_level].modal_state);
+            call_levels[call_level].modal_state = NULL;
+        }
+
+        call_context = --call_level >= 0 ? call_levels[call_level].context : NULL;
+    }
+
+    return call_level >= 0;
+}
+
+uint_fast8_t ngc_call_level (void)
+{
+    return (uint_fast8_t)(call_level + 1);
+}
+
 #endif // NGC_PARAMETERS_ENABLE

ngc_params.h

@@ -1,22 +1,22 @@
 /*
-  ngc_params.c - get/set NGC parameter value by id or name
+  ngc_params.h - get/set NGC parameter value by id or name
 
   Part of grblHAL
 
-  Copyright (c) 2021 Terje Io
+  Copyright (c) 2021-2024 Terje Io
 
-  Grbl is free software: you can redistribute it and/or modify
+  grblHAL 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,
+  grblHAL 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
+  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/>.
+  along with grblHAL. If not, see <http://www.gnu.org/licenses/>.
 */
 
 /*
@@ -28,6 +28,8 @@
 #ifndef _NGC_PARAMS_H_
 #define _NGC_PARAMS_H_
 
+#include "gcode.h"
+
 typedef uint16_t ngc_param_id_t;
 
 typedef struct {
@@ -81,6 +83,7 @@ typedef enum {
     NGCParam_current_pocket,
     NGCParam_selected_tool,
     NGCParam_selected_pocket,
+    NGCParam_call_level,
     NGCParam_Last
 } ncg_name_param_id_t;
 
@@ -92,5 +95,11 @@ bool ngc_named_param_get (char *name, float *value);
 float ngc_named_param_get_by_id (ncg_name_param_id_t id);
 bool ngc_named_param_set (char *name, float value);
 bool ngc_named_param_exists (char *name);
+bool ngc_call_push (void *context);
+bool ngc_call_pop (void);
+uint_fast8_t ngc_call_level (void);
+bool ngc_modal_state_save (gc_modal_t *state, bool auto_restore);
+bool ngc_modal_state_restore (void);
+void ngc_modal_state_invalidate (void);
 
-#endif
+#endif // _NGC_PARAMS_H_

nuts_bolts.c

@@ -166,6 +166,20 @@ char *ftoa (float n, uint8_t decimal_places)
     return bptr;
 }
 
+// Trim trailing zeros and possibly decimal point
+char *trim_float (char *s)
+{
+    if(strchr(s, '.')) {
+        char *s2 = strchr(s, '\0') - 1;
+        while(*s2 == '0')
+            *s2-- = '\0';
+        if(*s2 == '.')
+            *s2 = '\0';
+    }
+
+    return s;
+}
+
 // Extracts an unsigned integer value from a string.
 status_code_t read_uint (char *line, uint_fast8_t *char_counter, uint32_t *uint_ptr)
 {

nuts_bolts.h

@@ -211,6 +211,9 @@ char *uitoa (uint32_t n);
 // Converts a float variable to string with the specified number of decimal places.
 char *ftoa (float n, uint8_t decimal_places);
 
+// Trim trailing zeros and possibly decimal point
+char *trim_float (char *s);
+
 // Returns true if float value is a whole number (integer)
 bool isintf (float value);
 

planner.c

@@ -397,6 +397,7 @@ bool plan_buffer_line (float *target, plan_line_data_t *pl_data)
     block->condition = pl_data->condition;
     block->overrides = pl_data->overrides;
     block->line_number = pl_data->line_number;
+    block->offset_id = pl_data->offset_id;
     block->output_commands = pl_data->output_commands;
     block->message = pl_data->message;
 
@@ -689,6 +690,7 @@ void plan_feed_override (override_t feed_override, override_t rapid_override)
 void plan_data_init (plan_line_data_t *plan_data)
 {
     memset(plan_data, 0, sizeof(plan_line_data_t));
+    plan_data->offset_id = gc_state.offset_id;
     plan_data->spindle.hal = gc_state.spindle.hal ? gc_state.spindle.hal : spindle_get(0);
     plan_data->condition.target_validated = plan_data->condition.target_valid = sys.soft_limits.mask == 0;
 #ifdef KINEMATICS_API

planner.h

@@ -52,6 +52,7 @@ typedef struct plan_block {
     uint32_t step_event_count;      // The maximum step axis count and number of steps required to complete this block.
     axes_signals_t direction_bits;  // The direction bit set for this block (refers to *_DIRECTION_PIN in config.h)
 
+    offset_id_t offset_id;
     // Block condition data to ensure correct execution depending on states and overrides.
     gc_override_flags_t overrides;  // Block bitfield variable for overrides
     planner_cond_t condition;       // Block bitfield variable defining block run conditions. Copied from pl_line_data.
@@ -95,6 +96,7 @@ typedef struct {
     spindle_t spindle;              // Desired spindle parameters, such as RPM, through line motion.
     planner_cond_t condition;       // Bitfield variable to indicate planner conditions. See defines above.
     gc_override_flags_t overrides;  // Block bitfield variable for overrides
+    offset_id_t offset_id;
     int32_t line_number;            // Desired line number to report when executing.
 //    void *parameters;               // TODO: pointer to extra parameters, for canned cycles and threading?
     char *message;                  // Message to be displayed when block is executed.

platform.h

@@ -21,8 +21,28 @@
 
 #pragma once
 
-#if defined(STM32F103xB) || defined(STM32F103xE) || defined(STM32F401xC) || defined(STM32F401xE) || defined(STM32F407xx) || defined(STM32F411xE) || \
-    defined(STM32F412Vx) || defined(STM32F446xx) || defined(STM32F756xx) || defined(STM32F765xx) || defined(STM32H743xx) || defined(STM32H723xx)
+#if defined(STM32F103xB) || defined(STM32F103xE)
+#define STM32_F1_PLATFORM
+#endif
+
+#if defined(STM32F303xC)
+#define STM32_F3_PLATFORM
+#endif
+
+#if defined(STM32F401xC) || defined(STM32F401xE) || defined(STM32F407xx) || defined(STM32F411xE) || \
+    defined(STM32F412Vx) || defined(STM32F446xx)
+#define STM32_F4_PLATFORM
+#endif
+
+#if defined(STM32F756xx) || defined(STM32F765xx)
+#define STM32_F7_PLATFORM
+#endif
+
+#if defined(STM32H743xx) || defined(STM32H723xx)
+#define STM32_H7_PLATFORM
+#endif
+
+#if defined(STM32_F1_PLATFORM) || defined(STM32_F3_PLATFORM) || defined(STM32_F4_PLATFORM) || defined(STM32_F7_PLATFORM) || defined(STM32_H7_PLATFORM)
 #define STM32_PLATFORM
 #endif
 

protocol.c

@@ -375,9 +375,13 @@ bool protocol_main_loop (void)
 bool protocol_buffer_synchronize (void)
 {
     bool ok = true;
+
     // If system is queued, ensure cycle resumes if the auto start flag is present.
     protocol_auto_cycle_start();
+
+    sys.flags.synchronizing = On;
     while ((ok = protocol_execute_realtime()) && (plan_get_current_block() || state_get() == STATE_CYCLE));
+    sys.flags.synchronizing = Off;
 
     return ok;
 }
@@ -939,12 +943,12 @@ ISR_CODE bool ISR_FUNC(protocol_enqueue_realtime_command)(char c)
             break;
 
         case CMD_MPG_MODE_TOGGLE:           // Switch off MPG mode
-            if(hal.stream.type == StreamType_MPG)
+            if((drop = hal.stream.type == StreamType_MPG))
                 protocol_enqueue_foreground_task(stream_mpg_set_mode, NULL);
             break;
 
         case CMD_AUTO_REPORTING_TOGGLE:
-            if(settings.report_interval)
+            if((drop = settings.report_interval != 0))
                 sys.flags.auto_reporting = !sys.flags.auto_reporting;
             break;
 

report.c

@@ -1171,11 +1171,11 @@ void report_realtime_status (void)
 
     uint_fast8_t idx;
     float wco[N_AXIS];
-    if (!settings.status_report.machine_position || report.wco) {
-        for (idx = 0; idx < N_AXIS; idx++) {
+    if(!settings.status_report.machine_position || report.wco) {
+        for(idx = 0; idx < N_AXIS; idx++) {
             // Apply work coordinate offsets and tool length offset to current position.
-            wco[idx] = gc_get_offset(idx);
-            if (!settings.status_report.machine_position)
+            wco[idx] = gc_get_offset(idx, true);
+            if(!settings.status_report.machine_position)
                 print_position[idx] -= wco[idx];
         }
     }
@@ -1281,13 +1281,13 @@ void report_realtime_status (void)
     if(settings.status_report.work_coord_offset) {
 
         if(wco_counter > 0 && !report.wco) {
-            if(wco_counter > (REPORT_WCO_REFRESH_IDLE_COUNT - 1) && state_get() == STATE_IDLE)
+            if(wco_counter > (REPORT_WCO_REFRESH_IDLE_COUNT - 1) && state == STATE_IDLE)
                 wco_counter = REPORT_WCO_REFRESH_IDLE_COUNT - 1;
             wco_counter--;
         } else
-            wco_counter = state_get() & (STATE_HOMING|STATE_CYCLE|STATE_HOLD|STATE_JOG|STATE_SAFETY_DOOR)
-                            ? (REPORT_WCO_REFRESH_BUSY_COUNT - 1) // Reset counter for slow refresh
-                            : (REPORT_WCO_REFRESH_IDLE_COUNT - 1);
+            wco_counter = state & (STATE_HOMING|STATE_CYCLE|STATE_HOLD|STATE_JOG|STATE_SAFETY_DOOR)
+                           ? (REPORT_WCO_REFRESH_BUSY_COUNT - 1) // Reset counter for slow refresh
+                           : (REPORT_WCO_REFRESH_IDLE_COUNT - 1);
     } else
         report.wco = Off;
 
@@ -1298,9 +1298,9 @@ void report_realtime_status (void)
         else if((report.overrides = !report.wco)) {
             report.spindle = report.spindle || spindle_0_state.on;
             report.coolant = report.coolant || hal.coolant.get_state().value != 0;
-            override_counter = state_get() & (STATE_HOMING|STATE_CYCLE|STATE_HOLD|STATE_JOG|STATE_SAFETY_DOOR)
-                                 ? (REPORT_OVERRIDE_REFRESH_BUSY_COUNT - 1) // Reset counter for slow refresh
-                                 : (REPORT_OVERRIDE_REFRESH_IDLE_COUNT - 1);
+            override_counter = state & (STATE_HOMING|STATE_CYCLE|STATE_HOLD|STATE_JOG|STATE_SAFETY_DOOR)
+                                ? (REPORT_OVERRIDE_REFRESH_BUSY_COUNT - 1) // Reset counter for slow refresh
+                                : (REPORT_OVERRIDE_REFRESH_IDLE_COUNT - 1);
         }
     } else
         report.overrides = Off;
@@ -1308,8 +1308,14 @@ void report_realtime_status (void)
     if(report.value || gc_state.tool_change) {
 
         if(report.wco) {
-            hal.stream.write_all("|WCO:");
-            hal.stream.write_all(get_axis_values(wco));
+            // If protocol_buffer_synchronize() is running
+            // delay outputting WCO until sync is completed
+            // unless requested from stepper_driver_interrupt_handler.
+            if(report.force_wco || !sys.flags.synchronizing) {
+                hal.stream.write_all("|WCO:");
+                hal.stream.write_all(get_axis_values(wco));
+            } else
+                wco_counter = 0;
         }
 
         if(report.gwco) {

stepper.c

@@ -305,6 +305,10 @@ ISR_CODE void ISR_FUNC(stepper_driver_interrupt_handler)(void)
 
                 if((st.dir_change = st.exec_block == NULL || st.dir_outbits.value != st.exec_segment->exec_block->direction_bits.value))
                     st.dir_outbits = st.exec_segment->exec_block->direction_bits;
+
+                if(st.exec_block != NULL && st.exec_block->offset_id != st.exec_segment->exec_block->offset_id)
+                    sys.report.wco = sys.report.force_wco = On; // Do not generate grbl.on_rt_reports_added event!
+
                 st.exec_block = st.exec_segment->exec_block;
                 st.step_event_count = st.exec_block->step_event_count;
                 st.new_block = true;
@@ -704,9 +708,10 @@ void st_prep_buffer (void)
                 st_prep_block->spindle = pl_block->spindle.hal;
                 st_prep_block->output_commands = pl_block->output_commands;
                 st_prep_block->overrides = pl_block->overrides;
+                st_prep_block->offset_id = pl_block->offset_id;
                 st_prep_block->backlash_motion = pl_block->condition.backlash_motion;
                 st_prep_block->message = pl_block->message;
-                pl_block->message= NULL;
+                pl_block->message = NULL;
 
                 // Initialize segment buffer data for generating the segments.
                 prep.steps_per_mm = st_prep_block->steps_per_mm;
@@ -1098,3 +1103,14 @@ float st_get_realtime_rate (void)
 #endif
             : 0.0f;
 }
+
+offset_id_t st_get_offset_id (void)
+{
+    plan_block_t *pl_block;
+
+    return st.exec_block
+            ? st.exec_block->offset_id
+            : (sys.holding_state == Hold_Complete && (pl_block = plan_get_current_block())
+                ? pl_block->offset_id
+                : -1);
+}

stepper.h

@@ -51,6 +51,7 @@ typedef struct st_block {
     output_command_t *output_commands; //!< Output commands (linked list) to be performed when block is executed
     bool backlash_motion;
     bool dynamic_rpm;                  //!< Tracks motions that require dynamic RPM adjustment
+    offset_id_t offset_id;
     spindle_ptrs_t *spindle;           //!< Pointer to current spindle for motions that require dynamic RPM adjustment
 } st_block_t;
 
@@ -137,4 +138,6 @@ float st_get_realtime_rate (void);
 
 void stepper_driver_interrupt_handler (void);
 
+offset_id_t st_get_offset_id (void);
+
 #endif

stream.c

@@ -47,7 +47,7 @@ typedef struct stream_connection {
     const io_stream_t *stream;
     stream_is_connected_ptr is_up;
     stream_connection_flags_t flags;
-    struct stream_connection *next;
+    struct stream_connection *next, *prev;
 } stream_connection_t;
 
 static const io_stream_properties_t null_stream = {
@@ -232,6 +232,7 @@ static stream_connection_t *add_connection (const io_stream_t *stream)
                 return NULL;
             }
         }
+        connection->prev = last;
         last->next = connection;
     }
 
@@ -246,42 +247,42 @@ static bool stream_select (const io_stream_t *stream, bool add)
 {
     static const io_stream_t *active_stream = NULL;
 
-    bool send_init_message = false;
+    bool send_init_message = false, mpg_enable = false;
 
     if(stream == base.stream) {
         base.is_up = add ? (stream->is_connected ? stream->is_connected : stream_connected) : is_not_connected;
         return true;
     }
 
-    if(add) {
+    if(!add) { // disconnect
 
-        if(add_connection(stream) == NULL)
-            return false;
+        if(stream == base.stream || stream == mpg.stream)
+        	return false;
 
-    } else { // disconnect
+        bool disconnected = false;
+        stream_connection_t *connection = connections->next;
 
-        const io_stream_t *org_stream;
-        stream_connection_t *prev, *last = connections;
-
-        while(last->next) {
-            prev = last;
-            last = last->next;
-            if(prev->stream != mpg.stream)
-                org_stream = prev->stream;
-            if(last->stream == stream) {
-                prev->next = last->next;
-                free(last);
-                if(prev->next)
-                    return false;
-                else {
-                    if(mpg.flags.mpg_control || stream->type == StreamType_MPG)
-                        protocol_enqueue_foreground_task(stream_mpg_set_mode, (void *)1);
-                    stream = org_stream;
-                    break;
+        while(connection) {
+        	if(stream == connection->stream) {
+        		if((connection->prev->next = connection->next))
+        			connection->next->prev = connection->prev;
+                if((stream = connection->prev->stream) == mpg.stream) {
+                	mpg_enable = mpg.flags.mpg_control;
+                	if((stream = connection->prev->prev->stream) == NULL)
+                		stream = base.stream;
                 }
-            }
+                free(connection);
+        		connection = NULL;
+        		disconnected = true;
+        	} else
+        		connection = connection->next;
         }
-    }
+
+        if(!disconnected)
+        	return false;
+
+	} else if(add_connection(stream) == NULL)
+        return false;
 
     bool webui_connected = hal.stream.state.webui_connected;
 
@@ -323,7 +324,8 @@ static bool stream_select (const io_stream_t *stream, bool add)
     if(hal.stream.type == StreamType_MPG) {
         stream_mpg_enable(false);
         mpg.flags.mpg_control = On;
-    }
+    } else if(mpg_enable)
+		protocol_enqueue_foreground_task(stream_mpg_set_mode, (void *)1);
 
     memcpy(&hal.stream, stream, sizeof(io_stream_t));
 
@@ -455,7 +457,7 @@ void stream_mpg_set_mode (void *data)
 ISR_CODE bool ISR_FUNC(stream_mpg_check_enable)(char c)
 {
     if(c == CMD_MPG_MODE_TOGGLE)
-        protocol_enqueue_foreground_task(stream_mpg_set_mode, (void *)1);
+    	task_add_immediate(stream_mpg_set_mode, (void *)1);
     else {
         protocol_enqueue_realtime_command(c);
         if((c == CMD_CYCLE_START || c == CMD_CYCLE_START_LEGACY) && settings.status_report.pin_state)
@@ -492,6 +494,7 @@ bool stream_mpg_register (const io_stream_t *stream, bool rx_only, stream_write_
         memcpy(&mpg, connection, sizeof(stream_connection_t));
 
         mpg.flags.is_mpg_tx = On;
+        mpg.flags.mpg_control = Off;
 
         if(mpg_write_char)
             mpg.stream->set_enqueue_rt_handler(mpg_write_char);

system.c

@@ -1143,7 +1143,7 @@ the WCO report element to the next status report.
 */
 void system_flag_wco_change (void)
 {
-    if(!settings.status_report.sync_on_wco_change)
+    if(settings.status_report.sync_on_wco_change)
         protocol_buffer_synchronize();
 
     if(grbl.on_wco_changed)

system.h

@@ -187,6 +187,7 @@ typedef enum {
     Report_TLOReference = (1 << 15),
     Report_Fan = (1 << 16),
     Report_SpindleId = (1 << 17),
+    Report_ForceWCO = (1 << 29),
     Report_CycleStart = (1 << 30),
     Report_All = 0x8003FFFF
 } report_tracking_t;
@@ -212,7 +213,8 @@ typedef union {
                  tlo_reference :1, //!< Tool length offset reference changed.
                  fan           :1, //!< Fan on/off changed.
                  spindle_id    :1, //!< Spindle changed
-                 unassigned   :12, //
+                 unassigned   :11, //
+                 force_wco     :1, //!< Add work coordinates (due to WCO changed during motion).
                  cycle_start   :1, //!< Cycle start signal triggered. __NOTE:__ do __NOT__ add to Report_All enum above!
                  all           :1; //!< Set when CMD_STATUS_REPORT_ALL is requested, may be used by user code.
     };
@@ -249,7 +251,8 @@ typedef union {
                  single_block            :1, //!< Set to true to disable M1 (optional stop), via realtime command.
                  keep_input              :1, //!< Set to true to not flush stream input buffer on executing STOP.
                  auto_reporting          :1, //!< Set to true when auto real time reporting is enabled.
-                 unused                  :6;
+                 synchronizing           :1, //!< Set to true when protocol_buffer_synchronize() is running.
+                 unused                  :5;
     };
 } system_flags_t;
 

tool_change.c

@@ -143,7 +143,7 @@ static bool restore (void)
         spindle_restore(plan_data.spindle.hal, gc_state.modal.spindle.state, gc_state.spindle.rpm);
 
         if(!settings.flags.no_restore_position_after_M6) {
-            previous.values[plane.axis_linear] += gc_get_offset(plane.axis_linear);
+            previous.values[plane.axis_linear] += gc_get_offset(plane.axis_linear, false);
             mc_line(previous.values, &plan_data);
         }
     }
@@ -360,7 +360,7 @@ static status_code_t tool_change (parser_state_t *parser_state)
 
     // Establish axis assignments.
 
-    previous.values[plane.axis_linear] -= gc_get_offset(plane.axis_linear);
+    previous.values[plane.axis_linear] -= gc_get_offset(plane.axis_linear, false);
 
     plan_line_data_t plan_data;