diff --git a/README.md b/README.md
index 72d339f..3389657 100644
--- a/README.md
+++ b/README.md
@@ -1,6 +1,6 @@
 ## grblHAL ##
 
-Latest build date is 20260212, see the [changelog](changelog.md) for details.  
+Latest build date is 20260215, see the [changelog](changelog.md) for details.  
 
 > [!NOTE]
 >  A settings reset will be performed on an update of builds prior to 20241208. Backup and restore of settings is recommended.  
@@ -89,4 +89,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.
 
 ---
-20260212
+20260215
diff --git a/changelog.md b/changelog.md
index 768a882..9ad752e 100644
--- a/changelog.md
+++ b/changelog.md
@@ -1,5 +1,28 @@
 ## grblHAL changelog
 
+<a name="20260215">Build 20260215
+
+Core:
+
+* Marked a large number of non-critical functions with `FLASHMEM` to save RAM for the iMXRT1062 driver.
+
+* Improved alarm handling, if a critical alarm is active when a non-critical alarm is raised the non-critical alarm will be delayed until after reset for the critical alarm.
+
+* Improved Modbus exception handling and added high level API call for creating and sending Modbus messages.
+Added `G65P7` inbuilt macro, using the new API call, for interacting with Modbus devices from gcode. See the [Wiki](https://github.com/grblHAL/core/wiki/Expressions-and-flow-control#inbuilt-g65-macros) for details.
+
+Drivers:
+
+* iMXRT1062: marked some non-critical functions with `FLASHMEM`.
+
+Plugins:
+
+* SD card: marked some non-critical functions with `FLASHMEM` to save RAM for the iMXRT1062 driver.
+
+* Templates, Modbus cmd: new plugin that adds the system command `$MODBUSCMD` that can be used to interact with Modbus devices. 
+
+---
+
 <a name="20260212">Build 20260212
 
 Core:
diff --git a/config.h b/config.h
index d31d1a1..71dba07 100644
--- a/config.h
+++ b/config.h
@@ -549,7 +549,7 @@ Set to \ref On or 1 to enable experimental support for expressions.
 Some LinuxCNC extensions are supported, conditionals and subroutines are not.
 */
 #if !defined NGC_EXPRESSIONS_ENABLE || defined __DOXYGEN__
-#define NGC_EXPRESSIONS_ENABLE On
+#define NGC_EXPRESSIONS_ENABLE Off
 #endif
 
 /*! \def NGC_PARAMETERS_ENABLE
diff --git a/fs_device.c b/fs_device.c
index ab1bb47..2ae45b1 100644
--- a/fs_device.c
+++ b/fs_device.c
@@ -202,7 +202,7 @@ static int fs_stat (const char *filename, vfs_stat_t *st)
 
 void fs_device_mount (void)
 {
-    static const vfs_t fs = {
+    PROGMEM static const vfs_t fs = {
         .fopen = fs_open,
         .fclose = fs_close,
         .fread = fs_read,
diff --git a/gcode.c b/gcode.c
index 6601d58..3f15487 100644
--- a/gcode.c
+++ b/gcode.c
@@ -173,7 +173,7 @@ parser_state_t *gc_get_state (void)
     return &gc_state;
 }
 
-char *gc_coord_system_to_str (coord_system_id_t id)
+FLASHMEM char *gc_coord_system_to_str (coord_system_id_t id)
 {
     static char buf[6];
 
@@ -188,7 +188,7 @@ char *gc_coord_system_to_str (coord_system_id_t id)
     return buf;
 }
 
-static void override_disable (spindle_t *spindle, int32_t spindle_id, gc_override_flags_t flags, overrides_t *overrides)
+FLASHMEM static void override_disable (spindle_t *spindle, int32_t spindle_id, gc_override_flags_t flags, overrides_t *overrides)
 {
     gc_override_flags_t org_flags = gc_state.modal.override_ctrl;
 
@@ -214,7 +214,7 @@ static void override_disable (spindle_t *spindle, int32_t spindle_id, gc_overrid
     mc_override_ctrl_update(org_flags);
 }
 
-static void override_restore (spindle_t *spindle, int32_t spindle_id, gc_override_flags_t flags, overrides_t *overrides)
+FLASHMEM static void override_restore (spindle_t *spindle, int32_t spindle_id, gc_override_flags_t flags, overrides_t *overrides)
 {
     if(flags.spindle_rpm) {
         if(!(gc_state.modal.override_ctrl.spindle_rpm_disable = overrides->control.spindle_rpm_disable)) {
@@ -235,7 +235,7 @@ static void override_restore (spindle_t *spindle, int32_t spindle_id, gc_overrid
     mc_override_ctrl_update(gc_state.modal.override_ctrl);
 }
 
-static void set_scaling (float factor)
+FLASHMEM static void set_scaling (float factor)
 {
     uint_fast8_t idx = N_AXIS;
     axes_signals_t state = gc_get_g51_state();
@@ -290,7 +290,7 @@ inline static float gc_get_block_offset (parser_block_t *gc_block, uint_fast8_t
     return gc_block->modal.g5x_offset.data.coord.values[idx] + gc_state.g92_offset.coord.values[idx] + gc_state.modal.tool_length_offset[idx];
 }
 
-void gc_set_tool_offset (tool_offset_mode_t mode, uint_fast8_t idx, int32_t offset)
+FLASHMEM void gc_set_tool_offset (tool_offset_mode_t mode, uint_fast8_t idx, int32_t offset)
 {
     bool tlo_changed = false;
 
@@ -354,7 +354,7 @@ plane_t *gc_get_plane_data (plane_t *plane, plane_select_t select)
     return plane;
 }
 
-axes_signals_t gc_claim_axis_words (parser_block_t *gc_block, axes_signals_t validate)
+FLASHMEM axes_signals_t gc_claim_axis_words (parser_block_t *gc_block, axes_signals_t validate)
 {
     static const parameter_words_t wordmap[] = {
         { .x = On },
@@ -416,7 +416,7 @@ float gc_get_accel_factor (uint8_t profile)
 
 #if NGC_PARAMETERS_ENABLE
 
-static parameter_words_t macro_arguments_push (gc_values_t *values, parameter_words_t words)
+FLASHMEM static parameter_words_t macro_arguments_push (gc_values_t *values, parameter_words_t words)
 {
     // NOTE: this array has to match the parameter_words_t order!
     PROGMEM static const uint8_t offset[] = {
@@ -490,7 +490,7 @@ static parameter_words_t macro_arguments_push (gc_values_t *values, parameter_wo
     return g65_words;
 }
 
-static modal_restore_actions_t *get_state_restore_commands (gc_modal_t *modal, gc_modal_snapshot_t *snapshot)
+FLASHMEM static modal_restore_actions_t *get_state_restore_commands (gc_modal_t *modal, gc_modal_snapshot_t *snapshot)
 {
     static modal_restore_actions_t actions = {0};
 
@@ -573,7 +573,7 @@ static modal_restore_actions_t *get_state_restore_commands (gc_modal_t *modal, g
     return &actions;
 }
 
-bool gc_modal_state_restore (gc_modal_snapshot_t *snapshot)
+FLASHMEM bool gc_modal_state_restore (gc_modal_snapshot_t *snapshot)
 {
     bool ok = false;
 
@@ -613,7 +613,7 @@ bool gc_modal_state_restore (gc_modal_snapshot_t *snapshot)
 
 #endif // NGC_PARAMETERS_ENABLE
 
-static m98_macro_t *macro_find (macro_id_t id)
+FLASHMEM static m98_macro_t *macro_find (macro_id_t id)
 {
     m98_macro_t *sub;
 
@@ -625,7 +625,7 @@ static m98_macro_t *macro_find (macro_id_t id)
     return sub;
 }
 
-size_t gc_macro_get_pos (macro_id_t id, vfs_file_t *file)
+FLASHMEM size_t gc_macro_get_pos (macro_id_t id, vfs_file_t *file)
 {
     m98_macro_t *sub = macro_find(id);
 
@@ -646,7 +646,7 @@ bool gc_macros_validate (void)
 }
 */
 
-static void macros_clear (void)
+FLASHMEM static void macros_clear (void)
 {
     m98_macro_t *next;
 
@@ -656,7 +656,7 @@ static void macros_clear (void)
     } while((m98_macros = next));
 }
 
-static status_code_t macro_add (macro_id_t id, vfs_file_t *file)
+FLASHMEM static status_code_t macro_add (macro_id_t id, vfs_file_t *file)
 {
     m98_macro_t *sub = macro_find(id);
 
@@ -675,7 +675,7 @@ static status_code_t macro_add (macro_id_t id, vfs_file_t *file)
     return sub ? Status_OK : Status_FlowControlOutOfMemory;
 }
 
-static status_code_t macro_call (macro_id_t macro, parameter_words_t args, uint8_t repeats)
+FLASHMEM static status_code_t macro_call (macro_id_t macro, parameter_words_t args, uint8_t repeats)
 {
 #if NGC_PARAMETERS_ENABLE
     ngc_named_param_set("_value", 0.0f);
@@ -716,7 +716,7 @@ static status_code_t gc_at_exit (status_code_t status)
     return status;
 }
 
-void gc_init (bool stop)
+FLASHMEM void gc_init (bool stop)
 {
 #if COMPATIBILITY_LEVEL > 1
         memset(&gc_state, 0, sizeof(parser_state_t));
@@ -813,14 +813,14 @@ inline static bool is_single_spindle_block (parser_block_t *gc_block, modal_grou
 // Set dynamic laser power mode to PPI (Pulses Per Inch)
 // Returns true if driver uses hardware implementation.
 // Driver support for pulsing the laser on signal is required for this to work.
-bool gc_laser_ppi_enable (uint_fast16_t ppi, uint_fast16_t pulse_length)
+FLASHMEM bool gc_laser_ppi_enable (uint_fast16_t ppi, uint_fast16_t pulse_length)
 {
     gc_state.is_laser_ppi_mode = ppi > 0 && pulse_length > 0;
 
     return grbl.on_laser_ppi_enable && grbl.on_laser_ppi_enable(ppi, pulse_length);
 }
 
-spindle_t *gc_spindle_get (spindle_num_t spindle)
+FLASHMEM spindle_t *gc_spindle_get (spindle_num_t spindle)
 {
 #if N_SYS_SPINDLE > 1
     return spindle < 0 ? gc_state.spindle : &gc_state.modal.spindle[spindle];
@@ -829,7 +829,7 @@ spindle_t *gc_spindle_get (spindle_num_t spindle)
 #endif
 }
 
-void gc_spindle_off (void)
+FLASHMEM void gc_spindle_off (void)
 {
     uint_fast8_t idx;
     for(idx = 0; idx < N_SYS_SPINDLE; idx++) {
@@ -840,7 +840,7 @@ void gc_spindle_off (void)
     report_add_realtime(Report_Spindle);
 }
 
-void gc_coolant (coolant_state_t state)
+FLASHMEM void gc_coolant (coolant_state_t state)
 {
     gc_state.modal.coolant = state;
     hal.coolant.set_state(gc_state.modal.coolant);
@@ -854,7 +854,7 @@ static void add_offset (const coord_data_t *offset)
     system_flag_wco_change();
 }
 
-static tool_data_t *tool_get_pending (tool_id_t tool_id, char **message)
+FLASHMEM static tool_data_t *tool_get_pending (tool_id_t tool_id, char **message)
 {
     static tool_data_t tool_data = {0};
 
@@ -885,7 +885,7 @@ static inline void tool_set (tool_data_t *tool)
 }
 
 // Add output command to linked list
-static bool add_output_command (output_command_t *command)
+FLASHMEM static bool add_output_command (output_command_t *command)
 {
     output_command_t *add_cmd;
 
@@ -907,7 +907,7 @@ static bool add_output_command (output_command_t *command)
 }
 
 // Free linked list of output commands
-void gc_clear_output_commands (output_command_t *cmd)
+FLASHMEM void gc_clear_output_commands (output_command_t *cmd)
 {
     while(cmd) {
         output_command_t *next = cmd->next;
@@ -918,7 +918,7 @@ void gc_clear_output_commands (output_command_t *cmd)
 
 static gc_thread_data thread;
 
-static status_code_t init_sync_motion (plan_line_data_t *pl_data, float pitch)
+FLASHMEM static status_code_t init_sync_motion (plan_line_data_t *pl_data, float pitch)
 {
     if(pl_data->spindle.hal->get_data == NULL)
         RETURN(Status_GcodeUnsupportedCommand); // [Spindle not sync capable]
@@ -946,7 +946,7 @@ static status_code_t init_sync_motion (plan_line_data_t *pl_data, float pitch)
 }
 
 // Output and free previously allocated message
-void gc_output_message (char *message)
+FLASHMEM void gc_output_message (char *message)
 {
     if(message) {
 
diff --git a/grbl.h b/grbl.h
index c005578..b63d508 100644
--- a/grbl.h
+++ b/grbl.h
@@ -42,7 +42,7 @@
 #else
 #define GRBL_VERSION "1.1f"
 #endif
-#define GRBL_BUILD 20260212
+#define GRBL_BUILD 20260215
 
 #define GRBL_URL "https://github.com/grblHAL"
 
diff --git a/grbllib.c b/grbllib.c
index 55cf06d..dbb063f 100644
--- a/grbllib.c
+++ b/grbllib.c
@@ -144,7 +144,7 @@ static bool dummy_irq_claim (irq_type_t irq, uint_fast8_t id, irq_callback_ptr c
     return false;
 }
 
-static void report_driver_error (void *data)
+FLASHMEM static void report_driver_error (void *data)
 {
     char msg[40];
 
@@ -196,12 +196,12 @@ ISR_CODE static home_signals_t ISR_FUNC(get_homing_status2)(void)
     return home;
 }
 
-static void output_welcome_message (void *data)
+FLASHMEM static void output_welcome_message (void *data)
 {
     grbl.report.init_message(hal.stream.write);
 }
 
-static void onLinestateChanged (serial_linestate_t state)
+FLASHMEM static void onLinestateChanged (serial_linestate_t state)
 {
     if(state.dtr) {
         task_delete(output_welcome_message, NULL);
@@ -212,7 +212,7 @@ static void onLinestateChanged (serial_linestate_t state)
         on_linestate_changed(state);
 }
 
-static void stepperEnable (axes_signals_t enable, bool hold)
+FLASHMEM static void stepperEnable (axes_signals_t enable, bool hold)
 {
     if(stepper_enable)
         stepper_enable(enable, hold);
@@ -220,7 +220,7 @@ static void stepperEnable (axes_signals_t enable, bool hold)
     sys.steppers_enabled = /*!hold &&*/ enable.bits == AXES_BITMASK;
 }
 
-static void print_pos_msg (void *data)
+FLASHMEM static void print_pos_msg (void *data)
 {
     hal.stream.write("grblHAL: power on self-test (POS) failed!" ASCII_EOL);
 
@@ -228,13 +228,13 @@ static void print_pos_msg (void *data)
     } while((on_booted = task_run(on_booted)));
 }
 
-static void onPosFailure (serial_linestate_t state)
+FLASHMEM static void onPosFailure (serial_linestate_t state)
 {
     if(state.dtr) // delay a bit to let the USB stack come up
         task_add_delayed(print_pos_msg, NULL, 50);
 }
 
-static bool onProbeToolsetter (tool_data_t *tool, coord_data_t *position, bool at_g59_3, bool on)
+FLASHMEM static bool onProbeToolsetter (tool_data_t *tool, coord_data_t *position, bool at_g59_3, bool on)
 {
     bool ok = false;
 
@@ -244,7 +244,7 @@ static bool onProbeToolsetter (tool_data_t *tool, coord_data_t *position, bool a
     return ok;
 }
 
-static void tool_changed (tool_data_t *tool)
+FLASHMEM static void tool_changed (tool_data_t *tool)
 {
     if(settings.flags.tool_persistent && tool->tool_id != settings.tool_id) {
         settings.tool_id = tool->tool_id;
@@ -252,7 +252,7 @@ static void tool_changed (tool_data_t *tool)
     }
 }
 
-static void settings_changed (settings_t *settings, settings_changed_flags_t changed)
+FLASHMEM static void settings_changed (settings_t *settings, settings_changed_flags_t changed)
 {
     hal_settings_changed(settings, changed);
 
@@ -260,14 +260,14 @@ static void settings_changed (settings_t *settings, settings_changed_flags_t cha
         grbl.on_settings_changed(settings, changed);
 }
 
-static atc_status_t atc_get_state (void)
+FLASHMEM static atc_status_t atc_get_state (void)
 {
     return hal.driver_cap.atc ? ATC_Online : ATC_None;
 }
 
 // main entry point
 
-int grbl_enter (void)
+FLASHMEM int grbl_enter (void)
 {
     assert(NVS_ADDR_PARAMETERS + N_CoordinateSystems * (sizeof(coord_data_t) + NVS_CRC_BYTES) < NVS_ADDR_STARTUP_BLOCK);
     assert(NVS_ADDR_STARTUP_BLOCK + N_STARTUP_LINE * (sizeof(stored_line_t) + NVS_CRC_BYTES) < NVS_ADDR_BUILD_INFO);
@@ -685,7 +685,7 @@ ISR_CODE bool ISR_FUNC(task_add_systick)(foreground_task_ptr fn, void *data)
     return task != NULL;
 }
 
-void task_delete_systick (foreground_task_ptr fn, void *data)
+FLASHMEM void task_delete_systick (foreground_task_ptr fn, void *data)
 {
     core_task_t *task, *prev = NULL;
 
@@ -776,7 +776,7 @@ ISR_CODE bool ISR_FUNC(task_run_on_startup)(foreground_task_ptr fn, void *data)
 }
 
 // for core use only, called once from protocol.c on cold start
-void task_execute_on_startup (void)
+FLASHMEM void task_execute_on_startup (void)
 {
     if(!sys.driver_started) {
 
@@ -817,7 +817,7 @@ void task_execute_on_startup (void)
     }
 }
 
-void task_raise_alarm (void *data)
+FLASHMEM void task_raise_alarm (void *data)
 {
     system_raise_alarm((alarm_code_t)data);
 }
diff --git a/ioports.c b/ioports.c
index 3e4f60d..bdadaf3 100644
--- a/ioports.c
+++ b/ioports.c
@@ -101,8 +101,8 @@ static io_ports_private_t ports_cfg[] = {
     }
 };
 
-PROGMEM static const char *apnum = "E0\0E1\0E2\0E3\0E4\0E5\0E6\0E7\0E8\0E9\0E10\0E11\0E12\0E13\0E14\0E15";
-PROGMEM static const char *dpnum = "P0\0P1\0P2\0P3\0P4\0P5\0P6\0P7\0P8\0P9\0P10\0P11\0P12\0P13\0P14\0P15\0P16\0P17\0P18\0P19\0P20\0P21\0P22\0P23";
+PROGMEM static const char apnum[] = "E0\0E1\0E2\0E3\0E4\0E5\0E6\0E7\0E8\0E9\0E10\0E11\0E12\0E13\0E14\0E15";
+PROGMEM static const char dpnum[] = "P0\0P1\0P2\0P3\0P4\0P5\0P6\0P7\0P8\0P9\0P10\0P11\0P12\0P13\0P14\0P15\0P16\0P17\0P18\0P19\0P20\0P21\0P22\0P23";
 
 __STATIC_FORCEINLINE io_ports_private_t *get_port_data (io_port_type_t type, io_port_direction_t dir)
 {
@@ -134,7 +134,7 @@ __STATIC_FORCEINLINE uint8_t resolve_portnum (io_ports_private_t *p_data, xbar_t
     return is_aux(p_data, port->function) ? (port->function - p_data->min_fn) : map_reverse(p_data, port->id);
 }
 
-static uint8_t ioports_count (io_port_type_t type, io_port_direction_t dir, io_ports_private_t *p_data)
+FLASHMEM static uint8_t ioports_count (io_port_type_t type, io_port_direction_t dir, io_ports_private_t *p_data)
 {
     xbar_t *port;
     uint8_t n_ports = 0, n_remapped = 0;
@@ -156,7 +156,7 @@ static uint8_t ioports_count (io_port_type_t type, io_port_direction_t dir, io_p
 \param dir as an \a #io_port_direction_t enum value.
 \returns number of ports available excluding remapped ports but including claimed ports if the API implementation supports that.
 */
-uint8_t ioports_available (io_port_type_t type, io_port_direction_t dir)
+FLASHMEM uint8_t ioports_available (io_port_type_t type, io_port_direction_t dir)
 {
     io_ports_private_t *p_data = get_port_data(type, dir);
 
@@ -171,7 +171,7 @@ uint8_t ioports_available (io_port_type_t type, io_port_direction_t dir)
 \param dir as an \a #io_port_direction_t enum value.
 \returns number of ports available.
 */
-uint8_t ioports_unclaimed (io_port_type_t type, io_port_direction_t dir)
+FLASHMEM uint8_t ioports_unclaimed (io_port_type_t type, io_port_direction_t dir)
 {
     io_ports_private_t *p_data = get_port_data(type, dir);
 
@@ -197,7 +197,7 @@ struct ff_data {
     const char *description;
 };
 
-static bool match_port (xbar_t *properties, uint8_t port, void *data)
+FLASHMEM static bool match_port (xbar_t *properties, uint8_t port, void *data)
 {
     bool ok;
     struct ff_data *ff_data = (struct ff_data *)data;
@@ -208,7 +208,7 @@ static bool match_port (xbar_t *properties, uint8_t port, void *data)
     return ok;
 }
 
-static bool match_description (xbar_t *properties, uint8_t port, void *data)
+FLASHMEM static bool match_description (xbar_t *properties, uint8_t port, void *data)
 {
     bool ok;
     struct ff_data *ff_data = (struct ff_data *)data;
@@ -226,8 +226,7 @@ static bool match_description (xbar_t *properties, uint8_t port, void *data)
 or a port number to be used as the upper limit for the search, or \a NULL if searching for the first free port.
 \returns the port number if successful, 0xFF (255) if not.
 */
-
-uint8_t ioport_find_free (io_port_type_t type, io_port_direction_t dir, pin_cap_t filter, const char *description)
+FLASHMEM uint8_t ioport_find_free (io_port_type_t type, io_port_direction_t dir, pin_cap_t filter, const char *description)
 {
     struct ff_data ff_data = { .port = IOPORT_UNASSIGNED, .max_port = IOPORT_UNASSIGNED + 1 };
 
@@ -252,7 +251,7 @@ uint8_t ioport_find_free (io_port_type_t type, io_port_direction_t dir, pin_cap_
 \param port the port aux number.
 \returns pointer to \a xbar_t struct if successful, \a NULL if not.
 */
-static xbar_t *get_info (io_port_type_t type, io_port_direction_t dir, uint8_t port, bool claim)
+FLASHMEM static xbar_t *get_info (io_port_type_t type, io_port_direction_t dir, uint8_t port, bool claim)
 {
     bool ok = false;
     xbar_t *portinfo = NULL;
@@ -272,7 +271,7 @@ static xbar_t *get_info (io_port_type_t type, io_port_direction_t dir, uint8_t p
 \param port the claimed port aux number.
 \returns pointer to \a xbar_t struct if successful, \a NULL if not.
 */
-xbar_t *ioport_get_info (io_port_type_t type, io_port_direction_t dir, uint8_t port)
+FLASHMEM xbar_t *ioport_get_info (io_port_type_t type, io_port_direction_t dir, uint8_t port)
 {
     return hal.port.get_pin_info(type, dir, port);
 }
@@ -308,7 +307,7 @@ static inline void dec_hcount (io_port_type_t type, io_port_direction_t dir)
 \param description pointer to a \a char constant for the pin description.
 \returns pointer to a \a #xbar_t structure with details about the claimed port if successful, \a NULL if not.
 */
-xbar_t *ioport_claim (io_port_type_t type, io_port_direction_t dir, uint8_t *port, const char *description)
+FLASHMEM xbar_t *ioport_claim (io_port_type_t type, io_port_direction_t dir, uint8_t *port, const char *description)
 {
     xbar_t *portinfo = NULL;
 
@@ -340,7 +339,7 @@ xbar_t *ioport_claim (io_port_type_t type, io_port_direction_t dir, uint8_t *por
 \param port a \a uint8_t holding the ports aux number.
 \returns \a TRUE if available, \a FALSE if not.
 */
-bool ioport_claimable (io_port_type_t type, io_port_direction_t dir, uint8_t port)
+FLASHMEM bool ioport_claimable (io_port_type_t type, io_port_direction_t dir, uint8_t port)
 {
     xbar_t *portinfo = port == IOPORT_UNASSIGNED ? NULL : hal.port.get_pin_info(type, dir, map_reverse(get_port_data(type, dir), port));
 
@@ -348,7 +347,7 @@ bool ioport_claimable (io_port_type_t type, io_port_direction_t dir, uint8_t por
 }
 
 // Deprecated
-void ioport_assign_function (aux_ctrl_t *aux_ctrl, pin_function_t *function)
+FLASHMEM void ioport_assign_function (aux_ctrl_t *aux_ctrl, pin_function_t *function)
 {
     xbar_t *input;
 
@@ -365,7 +364,7 @@ void ioport_assign_function (aux_ctrl_t *aux_ctrl, pin_function_t *function)
 }
 
 // Deprecated
-void ioport_assign_out_function (aux_ctrl_out_t *aux_ctrl, pin_function_t *function)
+FLASHMEM void ioport_assign_out_function (aux_ctrl_out_t *aux_ctrl, pin_function_t *function)
 {
     xbar_t *output;
 
@@ -384,7 +383,7 @@ void ioport_assign_out_function (aux_ctrl_out_t *aux_ctrl, pin_function_t *funct
 \param function a \a #pin_function_t enum value.
 \param caps pointer to \a #driver_caps_t capability flags.
 */
-bool ioport_set_function (xbar_t *pin, pin_function_t function, driver_caps_t caps)
+FLASHMEM bool ioport_set_function (xbar_t *pin, pin_function_t function, driver_caps_t caps)
 {
     bool ok = false;
     io_ports_list_t *io_port = ports;
@@ -432,7 +431,7 @@ bool ioport_set_function (xbar_t *pin, pin_function_t function, driver_caps_t ca
 /*! \brief Get basic ioports capabilities.
 \returns a \a #io_port_cando_t union.
 */
-io_port_cando_t ioports_can_do (void)
+FLASHMEM io_port_cando_t ioports_can_do (void)
 {
     io_port_cando_t can_do = {};
 
@@ -447,7 +446,7 @@ io_port_cando_t ioports_can_do (void)
 }
 
 // Deprecated
-bool ioport_can_claim_explicit (void)
+FLASHMEM bool ioport_can_claim_explicit (void)
 {
     return ioports_can_do().claim_explicit;
 }
@@ -498,7 +497,7 @@ uint8_t _get_next (io_port_cfg_t *p, uint8_t port, const char *description, pin_
     return px;
 }
 
-static xbar_t *_claim (io_port_cfg_t *p, uint8_t *port, const char *description, pin_cap_t caps)
+FLASHMEM static xbar_t *_claim (io_port_cfg_t *p, uint8_t *port, const char *description, pin_cap_t caps)
 {
     xbar_t *portinfo = *port <= p->port_max ? hal.port.get_pin_info(p->handle->type >> 1, p->handle->type & 1, map_reverse(&ports_cfg[p->handle->type], *port)) : NULL;
 
@@ -516,7 +515,7 @@ static xbar_t *_claim (io_port_cfg_t *p, uint8_t *port, const char *description,
 \param dir as an \a #io_port_direction_t enum value.
 \returns the pointer to the \a io_port_cfg_t struct passed in the pp argument.
 */
-io_port_cfg_t *ioports_cfg (io_port_cfg_t *pp, io_port_type_t type, io_port_direction_t dir)
+FLASHMEM io_port_cfg_t *ioports_cfg (io_port_cfg_t *pp, io_port_type_t type, io_port_direction_t dir)
 {
     static io_port_cfg_t cfg[4] = {0};
 
@@ -549,7 +548,7 @@ io_port_cfg_t *ioports_cfg (io_port_cfg_t *pp, io_port_type_t type, io_port_dire
 If the function returns \a true the enumeration will end.
 \param data a pointer to context data passed to the callback function.
 */
-bool ioports_enumerate (io_port_type_t type, io_port_direction_t dir, pin_cap_t filter, ioports_enumerate_callback_ptr callback, void *data)
+FLASHMEM bool ioports_enumerate (io_port_type_t type, io_port_direction_t dir, pin_cap_t filter, ioports_enumerate_callback_ptr callback, void *data)
 {
     bool ok = false;
     io_ports_private_t *p_data = get_port_data(type, dir);
@@ -588,7 +587,7 @@ bool ioports_enumerate (io_port_type_t type, io_port_direction_t dir, pin_cap_t
 \param port the port aux number.
 \param description pointer to a \a char constant for the pin description.
 */
-bool ioport_set_description (io_port_type_t type, io_port_direction_t dir, uint8_t port, const char *description)
+FLASHMEM bool ioport_set_description (io_port_type_t type, io_port_direction_t dir, uint8_t port, const char *description)
 {
     if(hal.port.set_pin_description)
         hal.port.set_pin_description(type, dir, port, description);
@@ -614,7 +613,7 @@ int32_t ioport_wait_on_input (io_port_type_t type, uint8_t port, wait_mode_t wai
     return hal.port.wait_on_input ? hal.port.wait_on_input(type, port, wait_mode, timeout) : -1;
 }
 
-bool ioport_analog_out_config (uint8_t port, pwm_config_t *config)
+FLASHMEM bool ioport_analog_out_config (uint8_t port, pwm_config_t *config)
 {
     xbar_t *pin;
     bool ok = (pin = hal.port.get_pin_info(Port_Analog, Port_Output, port)) && pin->config;
@@ -622,7 +621,7 @@ bool ioport_analog_out_config (uint8_t port, pwm_config_t *config)
     return ok && pin->config(pin, config, false);
 }
 
-bool ioport_digital_in_config (uint8_t port, gpio_in_config_t *config)
+FLASHMEM bool ioport_digital_in_config (uint8_t port, gpio_in_config_t *config)
 {
     xbar_t *pin;
     bool ok = (pin = hal.port.get_pin_info(Port_Digital, Port_Input, port)) && pin->config;
@@ -630,12 +629,12 @@ bool ioport_digital_in_config (uint8_t port, gpio_in_config_t *config)
     return ok && pin->config(pin, config, false);
 }
 
-bool ioport_enable_irq (uint8_t port, pin_irq_mode_t irq_mode, ioport_interrupt_callback_ptr handler)
+FLASHMEM bool ioport_enable_irq (uint8_t port, pin_irq_mode_t irq_mode, ioport_interrupt_callback_ptr handler)
 {
     return hal.port.register_interrupt_handler && hal.port.register_interrupt_handler(port, irq_mode, handler);
 }
 
-bool ioport_digital_out_config (uint8_t port, gpio_out_config_t *config)
+FLASHMEM bool ioport_digital_out_config (uint8_t port, gpio_out_config_t *config)
 {
     xbar_t *pin;
     bool ok = (pin = hal.port.get_pin_info(Port_Digital, Port_Output, port)) && pin->config && !(pin->mode.pwm || pin->mode.servo_pwm);
@@ -643,7 +642,7 @@ bool ioport_digital_out_config (uint8_t port, gpio_out_config_t *config)
     return ok && pin->config(pin, config, false);
 }
 
-bool ioport_digital_pwm_config (uint8_t port, pwm_config_t *config)
+FLASHMEM bool ioport_digital_pwm_config (uint8_t port, pwm_config_t *config)
 {
     xbar_t *pin;
     bool ok = (pin = hal.port.get_pin_info(Port_Digital, Port_Output, port)) && pin->config && pin->mode.claimed && pin->cap.pwm;
@@ -663,7 +662,7 @@ __STATIC_FORCEINLINE const char *pnum_to_string (uint8_t port, const char *pnum)
     return pnum ? (pnum + (port * 3) + (port > 9 ? port - 10 : 0)) : NULL;
 }
 
-static xbar_t *io_get_pin_info (io_port_type_t type, io_port_direction_t dir, uint8_t port)
+FLASHMEM static xbar_t *io_get_pin_info (io_port_type_t type, io_port_direction_t dir, uint8_t port)
 {
     xbar_t *pin = NULL;
     io_ports_list_t *io_port = ports;
@@ -684,7 +683,7 @@ static xbar_t *io_get_pin_info (io_port_type_t type, io_port_direction_t dir, ui
     return pin;
 }
 
-static void io_set_pin_description (io_port_type_t type, io_port_direction_t dir, uint8_t port, const char *s)
+FLASHMEM static void io_set_pin_description (io_port_type_t type, io_port_direction_t dir, uint8_t port, const char *s)
 {
     io_ports_list_t *io_port = ports;
     io_ports_private_t *cfg = get_port_data(type, dir);
@@ -699,7 +698,7 @@ static void io_set_pin_description (io_port_type_t type, io_port_direction_t dir
     } while((io_port = io_port->next));
 }
 
-static bool io_claim (io_port_type_t type, io_port_direction_t dir, uint8_t *port, const char *description)
+FLASHMEM static bool io_claim (io_port_type_t type, io_port_direction_t dir, uint8_t *port, const char *description)
 {
     bool ok = false;
     io_ports_list_t *io_port = ports;
@@ -794,7 +793,7 @@ static int32_t io_wait_on_input (io_port_type_t type, uint8_t port, wait_mode_t
     return value;
 }
 
-static bool io_register_interrupt_handler (uint8_t port, pin_irq_mode_t irq_mode, ioport_interrupt_callback_ptr interrupt_callback)
+FLASHMEM static bool io_register_interrupt_handler (uint8_t port, pin_irq_mode_t irq_mode, ioport_interrupt_callback_ptr interrupt_callback)
 {
     uint8_t user_port = port;
     io_ports_list_t *io_port = ports;
@@ -812,7 +811,7 @@ static bool io_register_interrupt_handler (uint8_t port, pin_irq_mode_t irq_mode
 
 /**/
 
-static io_ports_list_t *insert_ports (void)
+FLASHMEM static io_ports_list_t *insert_ports (void)
 {
     io_ports_list_t *io_ports;
 
@@ -833,7 +832,7 @@ static io_ports_list_t *insert_ports (void)
     return io_ports;
 }
 
-static bool claim_hal (void)
+FLASHMEM static bool claim_hal (void)
 {
     io_port_t empty = {};
 
@@ -867,14 +866,14 @@ ISR_CODE uint8_t ISR_FUNC(ioports_map_reverse)(io_ports_detail_t *type, uint8_t
     return port;
 }
 
-static const char *get_pnum (io_ports_data_t *ports, uint8_t port)
+FLASHMEM static const char *get_pnum (io_ports_data_t *ports, uint8_t port)
 {
     return ports->pnum ? (ports->pnum + (port * 3) + (port > 9 ? port - 10 : 0)) : NULL;
 }
 
 #endif
 
-static uint8_t add_ports (io_ports_detail_t *ports, uint8_t *map, io_port_type_t type, io_port_direction_t dir, uint8_t n_ports)
+FLASHMEM static uint8_t add_ports (io_ports_detail_t *ports, uint8_t *map, io_port_type_t type, io_port_direction_t dir, uint8_t n_ports)
 {
     io_ports_private_t *p_data = get_port_data(type, dir);
 
@@ -903,7 +902,7 @@ static uint8_t add_ports (io_ports_detail_t *ports, uint8_t *map, io_port_type_t
     return n_ports;
 }
 
-static bool _ioports_add (io_ports_data_t *ports, io_port_type_t type, uint8_t n_in, uint8_t n_out, set_pin_description_ptr set_description)
+FLASHMEM static bool _ioports_add (io_ports_data_t *ports, io_port_type_t type, uint8_t n_in, uint8_t n_out, set_pin_description_ptr set_description)
 {
     uint_fast8_t n_ports;
     io_ports_private_t *cfg_in = get_port_data(type, Port_Input),
@@ -990,7 +989,7 @@ static bool _ioports_add (io_ports_data_t *ports, io_port_type_t type, uint8_t n
 \param port_to the remapped port number. The original port number will be swapped with \a port_from.
 \returns \a true if successful, \a false if original port is already claimed.
 */
-bool ioport_remap (io_port_type_t type, io_port_direction_t dir, uint8_t port_from, uint8_t port_to)
+FLASHMEM bool ioport_remap (io_port_type_t type, io_port_direction_t dir, uint8_t port_from, uint8_t port_to)
 {
     uint8_t org_port;
     bool ok;
@@ -1010,19 +1009,19 @@ bool ioport_remap (io_port_type_t type, io_port_direction_t dir, uint8_t port_fr
     return ok;
 }
 
-bool ioports_add (io_ports_data_t *ports, io_port_type_t type, uint8_t n_in, uint8_t n_out)
+FLASHMEM bool ioports_add (io_ports_data_t *ports, io_port_type_t type, uint8_t n_in, uint8_t n_out)
 {
     return hal.port.get_pin_info != io_get_pin_info && _ioports_add(ports, type, n_in, n_out, hal.port.set_pin_description);
 }
 
 static ll_set_pin_description_ptr set_descr_veneer;
 
-static void set_description (io_port_type_t type, io_port_direction_t dir, uint8_t port, const char *s)
+FLASHMEM static void set_description (io_port_type_t type, io_port_direction_t dir, uint8_t port, const char *s)
 {
     set_descr_veneer(dir, port, s);
 }
 
-bool ioports_add_analog (io_analog_t *analog)
+FLASHMEM bool ioports_add_analog (io_analog_t *analog)
 {
     if(analog->ports->in.n_ports + analog->ports->out.n_ports == 0)
         return false;
@@ -1051,7 +1050,7 @@ bool ioports_add_analog (io_analog_t *analog)
     return ok;
 }
 
-bool ioports_add_digital (io_digital_t *digital)
+FLASHMEM bool ioports_add_digital (io_digital_t *digital)
 {
     if(digital->ports->in.n_ports + digital->ports->out.n_ports == 0)
         return false;
@@ -1100,7 +1099,7 @@ __STATIC_FORCEINLINE uint_fast16_t invert_pwm (ioports_pwm_t *pwm_data, uint_fas
 \param clock_hz timer clock frequency used for PWM generation.
 \returns \a true if successful, \a false if no PWM range possible - driver should then revert to simple on/off control.
 */
-bool ioports_precompute_pwm_values (pwm_config_t *config, ioports_pwm_t *pwm_data, uint32_t clock_hz)
+FLASHMEM bool ioports_precompute_pwm_values (pwm_config_t *config, ioports_pwm_t *pwm_data, uint32_t clock_hz)
 {
     pwm_data->f_clock = clock_hz;
 
@@ -1149,7 +1148,7 @@ uint_fast16_t ioports_compute_pwm_value (ioports_pwm_t *pwm_data, float value)
     return pwm_value;
 }
 
-void ioport_save_input_settings (xbar_t *xbar, gpio_in_config_t *config)
+FLASHMEM void ioport_save_input_settings (xbar_t *xbar, gpio_in_config_t *config)
 {
     io_ports_list_t *io_port = ports;
     io_ports_private_t *cfg = get_port_data(!xbar->mode.analog, xbar->mode.output);
@@ -1191,7 +1190,7 @@ void ioport_save_input_settings (xbar_t *xbar, gpio_in_config_t *config)
     settings_write_global();
 }
 
-void ioport_save_output_settings (xbar_t *xbar, gpio_out_config_t *config)
+FLASHMEM void ioport_save_output_settings (xbar_t *xbar, gpio_out_config_t *config)
 {
     io_ports_list_t *io_port = ports;
     io_ports_private_t *cfg = get_port_data(!xbar->mode.analog, xbar->mode.output);
@@ -1220,7 +1219,7 @@ void ioport_save_output_settings (xbar_t *xbar, gpio_out_config_t *config)
     settings_write_global();
 }
 
-static bool is_setting_available (const setting_detail_t *setting, uint_fast16_t offset)
+FLASHMEM static bool is_setting_available (const setting_detail_t *setting, uint_fast16_t offset)
 {
     bool available = false;
 
@@ -1243,7 +1242,7 @@ static bool is_setting_available (const setting_detail_t *setting, uint_fast16_t
     return available;
 }
 
-static status_code_t aux_set_value (setting_id_t id, uint_fast16_t value)
+FLASHMEM static status_code_t aux_set_value (setting_id_t id, uint_fast16_t value)
 {
     xbar_t *xbar;
     uint8_t port = 0;
@@ -1363,7 +1362,7 @@ static status_code_t aux_set_value (setting_id_t id, uint_fast16_t value)
     return Status_OK;
 }
 
-static uint32_t aux_get_value (setting_id_t id)
+FLASHMEM static uint32_t aux_get_value (setting_id_t id)
 {
     uint32_t value = 0;
 
@@ -1392,11 +1391,11 @@ static uint32_t aux_get_value (setting_id_t id)
     return value;
 }
 
-static const setting_group_detail_t ioport_groups[] = {
+PROGMEM static const setting_group_detail_t ioport_groups[] = {
     { Group_Root, Group_AuxPorts, "Aux ports"}
 };
 
-static const setting_detail_t ioport_settings[] = {
+PROGMEM static const setting_detail_t ioport_settings[] = {
     { Settings_IoPort_InvertIn, Group_AuxPorts, "Invert I/O Port inputs", NULL, Format_Bitfield, ports_cfg[Port_DigitalIn].port_names, NULL, NULL, Setting_NonCoreFn, aux_set_value, aux_get_value, is_setting_available },
 #ifdef AUX_SETTINGS_PULLUP
     { Settings_IoPort_Pullup_Disable, Group_AuxPorts, "I/O Port inputs pullup disable", NULL, Format_Bitfield, digital.in.port_names, NULL, NULL, Setting_NonCoreFn, aux_set_value, aux_get_value, is_setting_available },
@@ -1405,14 +1404,14 @@ static const setting_detail_t ioport_settings[] = {
 //    { Settings_IoPort_OD_Enable, Group_AuxPorts, "I/O Port outputs as open drain", NULL, Format_Bitfield, digital.out.port_names, NULL, NULL, Setting_NonCoreFn, aux_set_value, aux_get_value, is_setting_available }
 };
 
-static const setting_descr_t ioport_settings_descr[] = {
+PROGMEM static const setting_descr_t ioport_settings_descr[] = {
     { Settings_IoPort_InvertIn, "Invert IOPort inputs." },
 //    { Settings_IoPort_Pullup_Disable, "Disable IOPort input pullups." },
     { Settings_IoPort_InvertOut, "Invert IOPort output." },
 //    { Settings_IoPort_OD_Enable, "Set IOPort outputs as open drain (OD)." }
 };
 
-static bool config_probe_pins (pin_function_t function, gpio_in_config_t *config)
+FLASHMEM static bool config_probe_pins (pin_function_t function, gpio_in_config_t *config)
 {
     bool ok = true;
 
@@ -1444,7 +1443,7 @@ static bool config_probe_pins (pin_function_t function, gpio_in_config_t *config
     return ok;
 }
 
-void ioport_setting_changed (setting_id_t id)
+FLASHMEM void ioport_setting_changed (setting_id_t id)
 {
     if(on_setting_changed)
         on_setting_changed(id);
@@ -1511,7 +1510,7 @@ void ioport_setting_changed (setting_id_t id)
     }
 }
 
-static void ioports_configure (settings_t *settings)
+FLASHMEM static void ioports_configure (settings_t *settings)
 {
     uint8_t port;
     xbar_t *xbar;
@@ -1569,7 +1568,7 @@ static void ioports_configure (settings_t *settings)
         on_settings_loaded();
 }
 
-static void onSettingsChanged (settings_t *settings, settings_changed_flags_t changed)
+FLASHMEM static void onSettingsChanged (settings_t *settings, settings_changed_flags_t changed)
 {
     if(on_settings_changed)
         on_settings_changed(settings, changed);
@@ -1578,7 +1577,7 @@ static void onSettingsChanged (settings_t *settings, settings_changed_flags_t ch
         ioports_configure(settings);
 }
 
-void ioports_add_settings (driver_settings_load_ptr settings_loaded, setting_changed_ptr setting_changed)
+FLASHMEM void ioports_add_settings (driver_settings_load_ptr settings_loaded, setting_changed_ptr setting_changed)
 {
     static bool ok = false;
     static setting_details_t setting_details = {
diff --git a/machine_limits.c b/machine_limits.c
index 448d269..cd9e6d0 100644
--- a/machine_limits.c
+++ b/machine_limits.c
@@ -113,7 +113,7 @@ ISR_CODE void ISR_FUNC(limit_interrupt_handler)(limit_signals_t state) // DEFAUL
 
 // Establish work envelope for homed axes, used by soft limits and jog limits handling.
 // When hard limits are enabled pulloff distance is subtracted to avoid triggering limit switches.
-void limits_set_work_envelope (void)
+FLASHMEM void limits_set_work_envelope (void)
 {
     uint_fast8_t idx = N_AXIS;
 
@@ -143,7 +143,7 @@ void limits_set_work_envelope (void)
 
 // Set machine positions for homed limit switches. Don't update non-homed axes.
 // NOTE: settings.max_travel[] is stored as a negative value.
-void limits_set_machine_positions (axes_signals_t cycle, bool add_pulloff)
+FLASHMEM void limits_set_machine_positions (axes_signals_t cycle, bool add_pulloff)
 {
     uint_fast8_t idx = N_AXIS;
 
@@ -167,7 +167,7 @@ void limits_set_machine_positions (axes_signals_t cycle, bool add_pulloff)
 #endif
 
 // Set, get homing pulloff
-coord_data_t *limits_homing_pulloff (coord_data_t *distance)
+FLASHMEM coord_data_t *limits_homing_pulloff (coord_data_t *distance)
 {
     if(distance)
         memcpy(&homing_pulloff, distance, sizeof(coord_data_t));
@@ -177,7 +177,7 @@ coord_data_t *limits_homing_pulloff (coord_data_t *distance)
 
 // Pulls off axes from asserted homing switches before homing starts.
 // For now only for auto squared axes.
-static bool limits_pull_off (axes_signals_t axis, coord_data_t *distance, float scaling)
+FLASHMEM static bool limits_pull_off (axes_signals_t axis, coord_data_t *distance, float scaling)
 {
     uint_fast8_t n_axis = 0, idx = N_AXIS;
     coord_data_t target = {0};
@@ -266,7 +266,7 @@ static bool limits_pull_off (axes_signals_t axis, coord_data_t *distance, float
 // mask, which prevents the stepper algorithm from executing step pulses. Homing motions typically
 // circumvent the processes for executing motions in normal operation.
 // NOTE: Only the abort realtime command can interrupt this process.
-static bool homing_cycle (axes_signals_t cycle, axes_signals_t auto_square)
+FLASHMEM static bool homing_cycle (axes_signals_t cycle, axes_signals_t auto_square)
 {
     if (ABORTED) // Block if system reset has been issued.
         return false;
@@ -532,7 +532,7 @@ static bool homing_cycle (axes_signals_t cycle, axes_signals_t auto_square)
 
 // Perform homing cycle(s) according to configuration.
 // NOTE: only one auto squared axis can be homed at a time.
-status_code_t limits_go_home (axes_signals_t cycle)
+FLASHMEM status_code_t limits_go_home (axes_signals_t cycle)
 {
     axes_signals_t auto_square = {0}, auto_squared = {0};
 
@@ -604,7 +604,7 @@ status_code_t limits_go_home (axes_signals_t cycle)
 // Performs a soft limit check. Called from mc_line() only. Assumes the machine has been homed,
 // the workspace volume is in all negative space, and the system is in normal operation.
 // NOTE: Also used by jogging to block travel outside soft-limit volume.
-void limits_soft_check (float *target, planner_cond_t condition)
+FLASHMEM void limits_soft_check (float *target, planner_cond_t condition)
 {
 #ifdef KINEMATICS_API
     if(condition.target_validated ? !condition.target_valid : !grbl.check_travel_limits(target, sys.soft_limits, false, &sys.work_envelope)) {
@@ -630,7 +630,7 @@ void limits_soft_check (float *target, planner_cond_t condition)
 }
 
 // Set axes to be homed from settings.
-void limits_set_homing_axes (void)
+FLASHMEM void limits_set_homing_axes (void)
 {
     uint_fast8_t idx = N_AXIS;
 
@@ -644,7 +644,7 @@ void limits_set_homing_axes (void)
 }
 
 // Check if homing is required.
-bool limits_homing_required (void)
+FLASHMEM bool limits_homing_required (void)
 {
     return settings.homing.flags.enabled && settings.homing.flags.init_lock &&
             (sys.cold_start || !settings.homing.flags.override_locks) &&
@@ -652,7 +652,7 @@ bool limits_homing_required (void)
 }
 
 // Get homing rate from the first axis in the cycle.
-static float get_homing_rate (axes_signals_t cycle, homing_mode_t mode)
+FLASHMEM static float get_homing_rate (axes_signals_t cycle, homing_mode_t mode)
 {
     uint_fast8_t idx = 0;
 
@@ -845,7 +845,7 @@ static void apply_travel_limits (float *target, float *position, work_envelope_t
     } while(idx);
 }
 
-void limits_init (void)
+FLASHMEM void limits_init (void)
 {
     hal.homing.get_feedrate = get_homing_rate;
     grbl.check_travel_limits = check_travel_limits;
diff --git a/modbus.c b/modbus.c
index 310dfcc..b1e9e3b 100644
--- a/modbus.c
+++ b/modbus.c
@@ -32,7 +32,7 @@
 static uint_fast16_t n_api = 0, tcp_api = N_MODBUS_API, rtu_api = N_MODBUS_API;
 static modbus_api_t modbus[N_MODBUS_API] = {0};
 
-modbus_cap_t modbus_isup (void)
+FLASHMEM modbus_cap_t modbus_isup (void)
 {
     uint_fast16_t idx = n_api;
     modbus_cap_t cap = {};
@@ -85,12 +85,12 @@ bool modbus_isbusy (void)
     return busy;
 }
 
-bool modbus_enabled (void)
+FLASHMEM bool modbus_enabled (void)
 {
     return n_api > 0;
 }
 
-void modbus_flush_queue (void)
+FLASHMEM void modbus_flush_queue (void)
 {
     uint_fast16_t idx = n_api;
 
@@ -99,7 +99,7 @@ void modbus_flush_queue (void)
     } while(idx);
 }
 
-void modbus_set_silence (const modbus_silence_timeout_t *timeout)
+FLASHMEM void modbus_set_silence (const modbus_silence_timeout_t *timeout)
 {
     if(rtu_api != N_MODBUS_API)
         modbus[rtu_api].set_silence(timeout);
@@ -116,23 +116,23 @@ bool modbus_send (modbus_message_t *msg, const modbus_callbacks_t *callbacks, bo
 }
 
 // Dummy exception handler
-void modbus_null_exception_handler (uint8_t code, void *context)
+FLASHMEM void modbus_null_exception_handler (uint8_t code, void *context)
 {
 //    NOOP
 }
 
-uint16_t modbus_read_u16 (uint8_t *p)
+FLASHMEM uint16_t modbus_read_u16 (uint8_t *p)
 {
     return (*p << 8) | *(p + 1);
 }
 
-void modbus_write_u16 (uint8_t *p, uint16_t value)
+FLASHMEM void modbus_write_u16 (uint8_t *p, uint16_t value)
 {
     *p = (uint8_t)(value >> 8);
     *(p + 1) = (uint8_t)(value & 0x00FF);
 }
 
-bool modbus_register_api (const modbus_api_t *api)
+FLASHMEM bool modbus_register_api (const modbus_api_t *api)
 {
     bool ok;
 
@@ -147,3 +147,161 @@ bool modbus_register_api (const modbus_api_t *api)
 
     return ok;
 }
+
+// Experimental high level API
+
+static void rx_packet (modbus_message_t *msg);
+static void rx_exception (uint8_t code, void *context);
+static void rx_timeout (uint8_t code, void *context);
+
+PROGMEM static const modbus_function_properties_t cmds[] = {
+    { 0, false, false },
+    { ModBus_ReadCoils, false, false },
+    { ModBus_ReadDiscreteInputs, false, false },
+    { ModBus_ReadHoldingRegisters, false, false },
+    { ModBus_ReadInputRegisters, false, false },
+    { ModBus_WriteCoil, true, true },
+    { ModBus_WriteRegister, true, true },
+    { ModBus_ReadExceptionStatus, false, true },
+    { 0, true, false }, // ModBus_Diagnostics
+    { 0, false, false },
+    { 0, false, false },
+    { 0, false, false },
+    { 0, false, false },
+    { 0, false, false },
+    { 0, false, false },
+    { ModBus_WriteCoils, true, false },
+    { ModBus_WriteRegisters, true, false }
+};
+PROGMEM static const uint8_t max_function = (sizeof(cmds) / sizeof(modbus_function_properties_t)) - 1;
+PROGMEM static const modbus_callbacks_t callbacks = {
+    .retries = 5,
+    .retry_delay = 100,
+    .on_rx_packet = rx_packet,
+    .on_rx_exception = rx_exception,
+    .on_rx_timeout = rx_timeout
+};
+
+static uint8_t tx_id;
+static modbus_callback_ptr xcallback;
+static modbus_response_t response;
+
+FLASHMEM static void rx_exception (uint8_t code, void *context)
+{
+    response.exception = code;
+
+    if(xcallback)
+        xcallback(&response);
+}
+
+FLASHMEM static void rx_timeout (uint8_t code, void *context)
+{
+    response.exception = ModBus_Timeout;
+
+    if(xcallback)
+        xcallback(&response);
+}
+
+FLASHMEM static void rx_packet (modbus_message_t *msg)
+{
+    if(!(msg->adu[0] & 0x80)) {
+
+        uint_fast8_t idx;
+
+        response.function = msg->adu[1];
+
+        if(response.function <= max_function && cmds[response.function].function != 0)
+          switch(response.function) {
+
+            case ModBus_ReadExceptionStatus:
+                response.num_values = 1;
+                response.values[0] = msg->adu[2];
+                break;
+
+            case ModBus_Diagnostics:
+                response.num_values = 0; // TBA
+                break;
+
+            default:;
+                response.num_values = cmds[response.function].single_register || cmds[response.function].is_write ? 2 : msg->adu[2] / 2;
+                response.num_values = min(response.num_values, 3);
+                uint_fast8_t pos = cmds[response.function].single_register || cmds[response.function].is_write ? 2 : 3;
+                for(idx = 0; idx < response.num_values; idx++)
+                    response.values[idx] = modbus_read_u16(&msg->adu[pos + (idx << 1)]);
+                break;
+        } else
+            response.exception = 255;
+
+        if(xcallback)
+            xcallback(&response);
+    }
+}
+
+FLASHMEM const modbus_function_properties_t *modbus_get_function_properties (modbus_function_t function)
+{
+    const modbus_function_properties_t *details = NULL;
+
+    if(function <= max_function && cmds[function].function)
+        details = &cmds[function];
+
+    return details;
+}
+
+FLASHMEM status_code_t modbus_message (uint8_t server, modbus_function_t function, uint16_t address, uint16_t *values, uint8_t registers, modbus_callback_ptr callback)
+{
+    if(function > max_function || !cmds[function].function)
+        return Status_InvalidStatement;
+
+    uint_fast8_t idx;
+    status_code_t status;
+    modbus_message_t cmd = {
+        .context = (void *)((uint32_t)tx_id++),
+        .crc_check = true,
+        .adu[0] = (uint8_t)server,
+        .adu[1] = (uint8_t)function,
+        .adu[2] = (uint8_t)(address >> 8),
+        .adu[3] = (uint8_t)(address & 0xFF)
+    };
+
+    xcallback = callback;
+
+    memset(&response, 0, sizeof(modbus_response_t));
+
+    if(function == ModBus_ReadExceptionStatus) {
+        cmd.tx_length = 4;
+        cmd.rx_length = 5;
+    } else {
+
+        cmd.tx_length = 6 + 2 * registers;
+        cmd.rx_length = cmd.tx_length - 1;
+
+        if(cmds[function].is_write) {
+            if(cmds[function].single_register) {
+                cmd.tx_length = cmd.rx_length = 8;
+                for(idx = 0; idx < registers; idx++) {
+                    cmd.adu[(idx << 1) + 4] = (uint8_t)(values[idx] >> 8);
+                    cmd.adu[(idx << 1) + 5] = (uint8_t)(values[idx] & 0xFF);
+                }
+            } else {
+                cmd.tx_length += 3; cmd.rx_length = 8;
+                cmd.adu[4] = (uint8_t)(registers >> 8);
+                cmd.adu[5] = (uint8_t)(registers & 0xFF);
+                cmd.adu[6] = (uint8_t)(registers << 1);
+                for(idx = 0; idx < registers; idx++) {
+                    cmd.adu[(idx << 1) + 7] = (uint8_t)(values[idx] >> 8);
+                    cmd.adu[(idx << 1) + 8] = (uint8_t)(values[idx] & 0xFF);
+                }
+            }
+        } else { // read
+            cmd.adu[4] = (uint8_t)(registers >> 8);
+            cmd.adu[5] = (uint8_t)(registers & 0xFF);
+            cmd.rx_length = 5 + (registers << 1);
+        }
+    }
+
+    status = modbus_send(&cmd, &callbacks, true) ? Status_OK : Status_AccessDenied;
+
+    return status;
+}
+
+/**/
diff --git a/modbus.h b/modbus.h
index ea6f921..1d0859f 100644
--- a/modbus.h
+++ b/modbus.h
@@ -24,6 +24,11 @@
 #ifndef _MODBUS_H_
 #define _MODBUS_H_
 
+#include <stdint.h>
+#include <stdbool.h>
+
+#include "errors.h"
+
 #ifndef MODBUS_MAX_ADU_SIZE
 #define MODBUS_MAX_ADU_SIZE 12
 #endif
@@ -31,11 +36,9 @@
 #define MODBUS_QUEUE_LENGTH 8
 #endif
 
-#include <stdint.h>
-#include <stdbool.h>
-
 #define MODBUS_SET_MSB16(v) ((v) >> 8)
 #define MODBUS_SET_LSB16(v) ((v) & 0xFF)
+#define MODBUS_MAX_REGISTERS ((MODBUS_MAX_ADU_SIZE - 6) / 2)
 
 typedef enum {
     Modbus_InterfaceRTU = 0,
@@ -56,6 +59,24 @@ typedef enum {
     ModBus_WriteRegisters = 16
 } modbus_function_t;
 
+typedef enum {
+    ModBus_NoException = 0, // For internal use, not defined in standard
+    ModBus_IllegalFunction = 1,
+    ModBus_IllegalDataAddress = 2,
+    ModBus_IllegalDataValue = 3,
+    ModBus_ServerDeviceFailure = 4,
+    ModBus_Acknowledge = 5,
+    ModBus_ServerDeviceBusy = 6,
+    ModBus_MemoryParityError = 7,
+    ModBus_GatewayPathUnavailable = 8, // 0x0A
+    ModBus_GatewayTargetUnresponsive = 9, // 0x0B
+// Internal exception codes, not defined in standard
+    ModBus_UnknownException = 252,
+    ModBus_IllegalSize = 253,
+    ModBus_CRCError = 254,
+    ModBus_Timeout = 255
+} __attribute__ ((__packed__)) modbus_exception_t;
+
 typedef struct {
     void *context;
     bool crc_check;
@@ -94,6 +115,23 @@ typedef union {
     };
 } modbus_cap_t;
 
+struct modbus_response;
+
+typedef void (*modbus_callback_ptr)(struct modbus_response *response);
+
+typedef struct modbus_response {
+    uint8_t function;
+    modbus_exception_t exception;
+    uint8_t num_values;
+    uint16_t values[MODBUS_MAX_REGISTERS];
+} modbus_response_t;
+
+typedef struct {
+    modbus_function_t function;
+    bool is_write;
+    bool single_register;
+} modbus_function_properties_t;
+
 typedef bool (*modbus_is_up_ptr)(void);
 typedef void (*modbus_flush_queue_ptr)(void);
 typedef void (*modbus_set_silence_ptr)(const modbus_silence_timeout_t *timeout);
@@ -120,4 +158,9 @@ uint16_t modbus_read_u16 (uint8_t *p);
 void modbus_write_u16 (uint8_t *p, uint16_t value);
 bool modbus_register_api (const modbus_api_t *api);
 
+// Experimental high level API
+
+const modbus_function_properties_t *modbus_get_function_properties (modbus_function_t function);
+status_code_t modbus_message (uint8_t server, modbus_function_t function, uint16_t address, uint16_t *values, uint8_t registers, modbus_callback_ptr callback);
+
 #endif
diff --git a/modbus_rtu.c b/modbus_rtu.c
index d898459..288e5f3 100644
--- a/modbus_rtu.c
+++ b/modbus_rtu.c
@@ -43,7 +43,7 @@ typedef enum {
     ModBus_Silent,
     ModBus_TX,
     ModBus_AwaitReply,
-    ModBus_Timeout,
+    ModBus_TimeoutException,
     ModBus_GotReply,
     ModBus_Exception,
     ModBus_Retry
@@ -68,8 +68,8 @@ typedef struct queue_entry {
     struct queue_entry *next;
 } queue_entry_t;
 
-static const uint32_t baud[] = { 2400, 4800, 9600, 19200, 38400, 115200 };
-static const modbus_silence_timeout_t dflt_timeout =
+PROGMEM static const uint32_t baud[] = { 2400, 4800, 9600, 19200, 38400, 115200 };
+PROGMEM static const modbus_silence_timeout_t dflt_timeout =
 {
     .b2400   = 16,
     .b4800   = 8,
@@ -82,11 +82,11 @@ static const modbus_silence_timeout_t dflt_timeout =
 static modbus_stream_t stream;
 static int8_t stream_instance = -1;
 static uint32_t rx_timeout = 0, silence_until = 0, silence_timeout;
-static int16_t exception_code = 0;
+static modbus_exception_t exception_code = ModBus_NoException;
 static modbus_silence_timeout_t silence;
 static queue_entry_t queue[MODBUS_QUEUE_LENGTH];
 static rtu_settings_t modbus;
-static volatile bool spin_lock = false, is_up = false;
+static volatile bool spin_lock = false, is_blocking = false, is_up = false;
 static volatile queue_entry_t *tail, *head, *packet = NULL;
 static volatile modbus_state_t state = ModBus_Idle;
 static uint8_t dir_port = IOPORT_UNASSIGNED;
@@ -103,15 +103,6 @@ static driver_reset_ptr driver_reset;
 static on_report_options_ptr on_report_options;
 static nvs_address_t nvs_address;
 
-/*
-static bool valid_crc (const char *buf, uint_fast16_t len)
-{
-    uint16_t crc = modbus_crc16x(buf, len - 2);
-
-    return buf[len - 1] == (crc >> 8) && buf[len - 2] == (crc & 0xFF);
-}
-*/
-
 static void retry_exception (uint8_t code, void *context)
 {
     if(packet && packet->callbacks.retries) {
@@ -165,7 +156,7 @@ static void modbus_poll (void *data)
     switch(state) {
 
         case ModBus_Idle:
-            if(tail != head && !packet) {
+            if(tail != head && !packet && !is_blocking) {
                 tx_message(tail);
                 tail = tail->next;
             }
@@ -205,11 +196,12 @@ static void modbus_poll (void *data)
                         packet->callbacks.on_rx_timeout(0, packet->msg.context);
                     packet = NULL;
                 } else if(stream.read() == packet->msg.adu[0] && (stream.read() & 0x80)) {
-                    exception_code = stream.read();
+                    int32_t code = stream.read();
+                    exception_code = code == SERIAL_NO_DATA ? ModBus_UnknownException : (modbus_exception_t)(code & 0xFF);
                     state = ModBus_Exception;
                     stats.rx_exceptions++;
                 } else
-                    state = ModBus_Timeout;
+                    state = ModBus_TimeoutException;
 
                 spin_lock = false;
                 if(state != ModBus_AwaitReply)
@@ -234,7 +226,7 @@ static void modbus_poll (void *data)
                         stats.crc_errors++;
                         if((state = packet->async ? ModBus_Silent : ModBus_Exception) == ModBus_Silent) {
                             if(packet->callbacks.on_rx_exception)
-                                packet->callbacks.on_rx_exception(0, packet->msg.context);
+                                packet->callbacks.on_rx_exception(ModBus_CRCError, packet->msg.context);
                             packet = NULL;
                         }
                         silence_until = hal.get_elapsed_ticks() + silence_timeout;
@@ -254,7 +246,7 @@ static void modbus_poll (void *data)
             }
             break;
 
-        case ModBus_Timeout:
+        case ModBus_TimeoutException:
             if(packet->async)
                 state = ModBus_Silent;
             silence_until = hal.get_elapsed_ticks() + silence_timeout;
@@ -269,12 +261,11 @@ static void modbus_poll (void *data)
 
 static bool modbus_send_rtu (modbus_message_t *msg, const modbus_callbacks_t *callbacks, bool block)
 {
-    static bool poll = false;
     static queue_entry_t sync_msg = {0};
 
     if(msg->tx_length > MODBUS_MAX_ADU_SIZE || msg->rx_length > MODBUS_MAX_ADU_SIZE) {
         if(callbacks->on_rx_exception)
-            callbacks->on_rx_exception(0, msg->context);
+            callbacks->on_rx_exception(ModBus_IllegalSize, msg->context);
         return false;
     }
 
@@ -287,39 +278,38 @@ static bool modbus_send_rtu (modbus_message_t *msg, const modbus_callbacks_t *ca
 
     if(block) {
 
-        if(poll)
+        if(is_blocking)
             return false;
 
-        poll = true;
+        is_blocking = true;
 
-        do {
+        while(state != ModBus_Idle)
             grbl.on_execute_realtime(state_get());
-        } while(state != ModBus_Idle);
 
         tx_message(add_message(&sync_msg, msg, false, callbacks));
 
-        while(poll) {
+        while(is_blocking) {
 
             grbl.on_execute_realtime(state_get());
 
             switch(state) {
 
-                case ModBus_Timeout:
+                case ModBus_TimeoutException:
                     if(packet->callbacks.on_rx_timeout)
-                        packet->callbacks.on_rx_timeout(0, packet->msg.context);
-                    poll = packet->callbacks.retries > 0;
+                        packet->callbacks.on_rx_timeout(ModBus_Timeout, packet->msg.context);
+                    is_blocking = packet->callbacks.retries > 0;
                     break;
 
                 case ModBus_Exception:
                     if(packet->callbacks.on_rx_exception)
-                        packet->callbacks.on_rx_exception(exception_code == -1 ? 0 : (uint8_t)(exception_code & 0xFF), packet->msg.context);
-                    poll = packet->callbacks.retries > 0;
+                        packet->callbacks.on_rx_exception((uint8_t)exception_code, packet->msg.context);
+                    is_blocking = packet->callbacks.retries > 0;
                     break;
 
                 case ModBus_GotReply:
                     if(packet->callbacks.on_rx_packet)
                         packet->callbacks.on_rx_packet(&((queue_entry_t *)packet)->msg);
-                    poll = block = false;
+                    is_blocking = block = false;
                     break;
 
                 case ModBus_Retry:
@@ -339,8 +329,8 @@ static bool modbus_send_rtu (modbus_message_t *msg, const modbus_callbacks_t *ca
             }
         }
 
-        poll = false;
         packet = NULL;
+        is_blocking = false;
         state = silence_until > 0 ? ModBus_Silent : ModBus_Idle;
 
     } else if(packet != &sync_msg) {
@@ -353,7 +343,7 @@ static bool modbus_send_rtu (modbus_message_t *msg, const modbus_callbacks_t *ca
     return !block;
 }
 
-static void modbus_reset (void)
+FLASHMEM static void modbus_reset (void)
 {
     while(spin_lock);
 
@@ -380,7 +370,7 @@ static void modbus_reset (void)
     driver_reset();
 }
 
-static uint32_t get_baudrate (uint32_t rate)
+FLASHMEM static uint32_t get_baudrate (uint32_t rate)
 {
     uint32_t idx = sizeof(baud) / sizeof(uint32_t);
 
@@ -392,11 +382,11 @@ static uint32_t get_baudrate (uint32_t rate)
     return DEFAULT_MODBUS_STREAM_BAUD;
 }
 
-static const setting_group_detail_t modbus_groups [] = {
+PROGMEM static const setting_group_detail_t modbus_groups [] = {
     { Group_Root, Group_ModBus, "ModBus"}
 };
 
-static status_code_t modbus_set_baud (setting_id_t id, uint_fast16_t value)
+FLASHMEM static status_code_t modbus_set_baud (setting_id_t id, uint_fast16_t value)
 {
     settings.modbus_baud = (uint8_t)value;
     modbus.baud_rate = baud[settings.modbus_baud];
@@ -406,12 +396,12 @@ static status_code_t modbus_set_baud (setting_id_t id, uint_fast16_t value)
     return Status_OK;
 }
 
-static uint32_t modbus_get_baud (setting_id_t setting)
+FLASHMEM static uint32_t modbus_get_baud (setting_id_t setting)
 {
     return get_baudrate(modbus.baud_rate);
 }
 
-static status_code_t modbus_set_format (setting_id_t id, uint_fast16_t value)
+FLASHMEM static status_code_t modbus_set_format (setting_id_t id, uint_fast16_t value)
 {
     if(stream.set_format) {
         settings.modbus_stream_format.parity = (serial_parity_t)value;
@@ -422,28 +412,28 @@ static status_code_t modbus_set_format (setting_id_t id, uint_fast16_t value)
     return stream.set_format ? Status_OK : Status_SettingDisabled;
 }
 
-static uint32_t modbus_get_format (setting_id_t setting)
+FLASHMEM static uint32_t modbus_get_format (setting_id_t setting)
 {
     return (uint32_t)settings.modbus_stream_format.parity;
 }
 
-static bool can_set_format (const setting_detail_t *setting, uint_fast16_t offset)
+FLASHMEM static bool can_set_format (const setting_detail_t *setting, uint_fast16_t offset)
 {
     return stream.set_format != NULL;
 }
 
-static const setting_detail_t modbus_settings[] = {
+PROGMEM static const setting_detail_t modbus_settings[] = {
     { Settings_ModBus_BaudRate, Group_ModBus, "ModBus baud rate", NULL, Format_RadioButtons, "2400,4800,9600,19200,38400,115200", NULL, NULL, Setting_NonCoreFn, modbus_set_baud, modbus_get_baud, NULL },
     { Settings_ModBus_RXTimeout, Group_ModBus, "ModBus RX timeout", "milliseconds", Format_Integer, "####0", "50", "250", Setting_NonCore, &modbus.rx_timeout, NULL, NULL },
     { Setting_ModBus_StreamFormat, Group_ModBus, "ModBus serial format", NULL, Format_RadioButtons, "8-bit no parity, 8-bit even parity, 8-bit odd parity", NULL, NULL, Setting_NonCoreFn, modbus_set_format, modbus_get_format, can_set_format }
 };
 
-static void modbus_settings_save (void)
+FLASHMEM static void modbus_settings_save (void)
 {
     hal.nvs.memcpy_to_nvs(nvs_address, (uint8_t *)&modbus, sizeof(rtu_settings_t), true);
 }
 
-static void modbus_settings_restore (void)
+FLASHMEM static void modbus_settings_restore (void)
 {
     modbus.rx_timeout = 50;
     modbus.baud_rate = baud[DEFAULT_MODBUS_STREAM_BAUD];
@@ -451,7 +441,7 @@ static void modbus_settings_restore (void)
     hal.nvs.memcpy_to_nvs(nvs_address, (uint8_t *)&modbus, sizeof(rtu_settings_t), true);
 }
 
-static void modbus_settings_load (void)
+FLASHMEM static void modbus_settings_load (void)
 {
     if(hal.nvs.memcpy_from_nvs((uint8_t *)&modbus, nvs_address, sizeof(rtu_settings_t), true) != NVS_TransferResult_OK ||
          modbus.baud_rate != baud[get_baudrate(modbus.baud_rate)])
@@ -466,12 +456,12 @@ static void modbus_settings_load (void)
         stream.set_format(settings.modbus_stream_format);
 }
 
-static void onReportOptions (bool newopt)
+FLASHMEM static void onReportOptions (bool newopt)
 {
     on_report_options(newopt);
 
     if(!newopt)
-        report_plugin("MODBUS", "0.21");
+        report_plugin("MODBUS", "0.22");
 }
 
 static bool modbus_rtu_isup (void)
@@ -484,14 +474,14 @@ static bool modbus_is_busy (void)
     return state != STATE_IDLE;
 }
 
-static void modbus_rtu_flush_queue (void)
+FLASHMEM static void modbus_rtu_flush_queue (void)
 {
     while(spin_lock);
 
     tail = head;
 }
 
-static void modbus_rtu_set_silence (const modbus_silence_timeout_t *timeout)
+FLASHMEM static void modbus_rtu_set_silence (const modbus_silence_timeout_t *timeout)
 {
     if(timeout)
         memcpy(&silence, timeout, sizeof(modbus_silence_timeout_t));
@@ -501,7 +491,7 @@ static void modbus_rtu_set_silence (const modbus_silence_timeout_t *timeout)
     silence_timeout = silence.timeout[get_baudrate(modbus.baud_rate)];
 }
 
-static bool stream_is_valid (const io_stream_t *stream)
+FLASHMEM static bool stream_is_valid (const io_stream_t *stream)
 {
     return stream &&
             !(stream->set_baud_rate == NULL ||
@@ -519,7 +509,7 @@ static void modbus_set_direction (bool tx)
     ioport_digital_out(dir_port, tx);
 }
 
-static bool claim_stream (io_stream_properties_t const *sstream, void *data)
+FLASHMEM static bool claim_stream (io_stream_properties_t const *sstream, void *data)
 {
     io_stream_t const *claimed = NULL;
 
@@ -551,7 +541,7 @@ static bool claim_stream (io_stream_properties_t const *sstream, void *data)
     return claimed != NULL;
 }
 
-static status_code_t report_stats (sys_state_t state, char *args)
+FLASHMEM static status_code_t report_stats (sys_state_t state, char *args)
 {
     char buf[110];
 
@@ -566,9 +556,9 @@ static status_code_t report_stats (sys_state_t state, char *args)
     return Status_OK;
 }
 
-void modbus_rtu_init (int8_t instance, int8_t dir_aux)
+FLASHMEM void modbus_rtu_init (int8_t instance, int8_t dir_aux)
 {
-    static const modbus_api_t api = {
+    PROGMEM static const modbus_api_t api = {
         .interface = Modbus_InterfaceRTU,
         .is_up = modbus_rtu_isup,
         .flush_queue = modbus_rtu_flush_queue,
@@ -587,7 +577,7 @@ void modbus_rtu_init (int8_t instance, int8_t dir_aux)
         .restore = modbus_settings_restore
     };
 
-    static const sys_command_t command_list[] = {
+    PROGMEM static const sys_command_t command_list[] = {
         {"MODBUSSTATS", report_stats, { .allow_blocking = On }, { .str = "output Modbus RTU statistics" } },
     };
 
@@ -622,8 +612,6 @@ void modbus_rtu_init (int8_t instance, int8_t dir_aux)
             driver_reset = hal.driver_reset;
             hal.driver_reset = modbus_reset;
 
-            hal.driver_cap.modbus_rtu = task_add_systick(modbus_poll, NULL);
-
             on_report_options = grbl.on_report_options;
             grbl.on_report_options = onReportOptions;
 
@@ -646,7 +634,7 @@ void modbus_rtu_init (int8_t instance, int8_t dir_aux)
     }
 
     if(!hal.driver_cap.modbus_rtu) {
-        task_run_on_startup(report_warning, "Modbus failed to initialize!");
+        task_run_on_startup(report_warning, "Modbus RTU failed to initialize!");
         system_raise_alarm(Alarm_SelftestFailed);
     }
 }
diff --git a/motion_control.c b/motion_control.c
index 23292e7..fd60fce 100644
--- a/motion_control.c
+++ b/motion_control.c
@@ -474,7 +474,7 @@ static inline float dist1 (const float x1, const float y1, const float x2, const
  * power available on Arduino, I think it is not wise to implement it.
  */
 
-void mc_cubic_b_spline (float *target, plan_line_data_t *pl_data, float *position, float *first, float *second)
+FLASHMEM void mc_cubic_b_spline (float *target, plan_line_data_t *pl_data, float *position, float *first, float *second)
 {
     float bez_target[N_AXIS];
 
@@ -564,7 +564,7 @@ void mc_cubic_b_spline (float *target, plan_line_data_t *pl_data, float *positio
 
 // end Bezier splines
 
-bool mc_canned_drill (motion_mode_t motion, float *target, plan_line_data_t *pl_data, float *position, plane_t plane, uint32_t repeats, gc_canned_t *canned)
+FLASHMEM bool mc_canned_drill (motion_mode_t motion, float *target, plan_line_data_t *pl_data, float *position, plane_t plane, uint32_t repeats, gc_canned_t *canned)
 {
     pl_data->condition.rapid_motion = On; // Set rapid motion condition flag.
 
@@ -674,7 +674,7 @@ inline static float calc_thread_doc (uint_fast16_t pass, float cut_depth, float
 
 // TODO: change pitch to follow any tapers
 
-void mc_thread (plan_line_data_t *pl_data, float *position, gc_thread_data *thread, bool feed_hold_disabled)
+FLASHMEM void mc_thread (plan_line_data_t *pl_data, float *position, gc_thread_data *thread, bool feed_hold_disabled)
 {
     uint_fast16_t pass = 1, passes = 0;
     float doc = thread->initial_depth, inv_degression = 1.0f / thread->depth_degression, thread_length;
@@ -795,7 +795,7 @@ void mc_thread (plan_line_data_t *pl_data, float *position, gc_thread_data *thre
 }
 
 // Sets up valid jog motion received from g-code parser, checks for soft-limits, and executes the jog.
-status_code_t mc_jog_execute (plan_line_data_t *pl_data, parser_block_t *gc_block, float *position)
+FLASHMEM status_code_t mc_jog_execute (plan_line_data_t *pl_data, parser_block_t *gc_block, float *position)
 {
     // Initialize planner data struct for jogging motions.
     // NOTE: Spindle and coolant are allowed to fully function with overrides during a jog.
@@ -827,7 +827,7 @@ status_code_t mc_jog_execute (plan_line_data_t *pl_data, parser_block_t *gc_bloc
 }
 
 // Execute dwell in seconds.
-void mc_dwell (float seconds)
+FLASHMEM void mc_dwell (float seconds)
 {
     if (state_get() != STATE_CHECK_MODE) {
         protocol_buffer_synchronize();
@@ -838,7 +838,7 @@ void mc_dwell (float seconds)
 // Perform homing cycle to locate and set machine zero. Only '$H' executes this command.
 // NOTE: There should be no motions in the buffer and grblHAL must be in an idle state before
 // executing the homing cycle. This prevents incorrect buffered plans after homing.
-status_code_t mc_homing_cycle (axes_signals_t cycle)
+FLASHMEM status_code_t mc_homing_cycle (axes_signals_t cycle)
 {
     bool home_all = cycle.mask == 0;
     status_code_t homed_status = Status_OK;
@@ -990,7 +990,7 @@ status_code_t mc_homing_cycle (axes_signals_t cycle)
 
 // Perform tool length probe cycle. Requires probe switch.
 // NOTE: Upon probe failure, the program will be stopped and placed into ALARM state.
-gc_probe_t mc_probe_cycle (float *target, plan_line_data_t *pl_data, gc_parser_flags_t parser_flags)
+FLASHMEM gc_probe_t mc_probe_cycle (float *target, plan_line_data_t *pl_data, gc_parser_flags_t parser_flags)
 {
     uint_fast8_t idx = N_AXIS;
 
@@ -1116,7 +1116,7 @@ gc_probe_t mc_probe_cycle (float *target, plan_line_data_t *pl_data, gc_parser_f
 
 // Plans and executes the single special motion case for parking. Independent of main planner buffer.
 // NOTE: Uses the always free planner ring buffer head to store motion parameters for execution.
-bool mc_parking_motion (float *parking_target, plan_line_data_t *pl_data)
+FLASHMEM bool mc_parking_motion (float *parking_target, plan_line_data_t *pl_data)
 {
     bool ok;
 
@@ -1134,7 +1134,7 @@ bool mc_parking_motion (float *parking_target, plan_line_data_t *pl_data)
     return ok;
 }
 
-void mc_override_ctrl_update (gc_override_flags_t override_state)
+FLASHMEM void mc_override_ctrl_update (gc_override_flags_t override_state)
 {
  // Finish all queued commands before altering override control state
     if(sys.override.control.value != override_state.value)
diff --git a/ngc_expr.c b/ngc_expr.c
index 7eb136b..eafcfde 100644
--- a/ngc_expr.c
+++ b/ngc_expr.c
@@ -82,7 +82,7 @@ typedef enum {
 \param rhs pointer to the right hand side operand.
 \returns #Status_OK enum value if processed without error, appropriate \ref status_code_t enum value if not.
 */
-static status_code_t execute_binary1 (float *lhs, ngc_binary_op_t operation, float *rhs)
+FLASHMEM static status_code_t execute_binary1 (float *lhs, ngc_binary_op_t operation, float *rhs)
 {
     status_code_t status = Status_OK;
 
@@ -130,7 +130,7 @@ Any non-zero input value is taken as meaning true, and only 0.0 means false.
 \param rhs pointer to the right hand side operand.
 \returns #Status_OK enum value if processed without error, appropriate \ref status_code_t enum value if not.
 */
-static status_code_t execute_binary2 (float *lhs, ngc_binary_op_t operation, float *rhs)
+FLASHMEM static status_code_t execute_binary2 (float *lhs, ngc_binary_op_t operation, float *rhs)
 {
     switch(operation) {
 
@@ -202,7 +202,7 @@ This just calls either execute_binary1 or execute_binary2.
 \param rhs pointer to the right hand side operand.
 \returns #Status_OK enum value if processed without error, appropriate \ref status_code_t enum value if not.
 */
-static status_code_t execute_binary (float *lhs, ngc_binary_op_t operation, float *rhs)
+FLASHMEM static status_code_t execute_binary (float *lhs, ngc_binary_op_t operation, float *rhs)
 {
     if (operation <= NGCBinaryOp_Binary2)
         return execute_binary1(lhs, operation, rhs);
@@ -218,7 +218,7 @@ All angle measures in the input or output are in degrees.
 \param operation \ref ngc_binary_op_t enum value.
 \returns #Status_OK enum value if processed without error, appropriate \ref status_code_t enum value if not.
 */
-static status_code_t execute_unary (float *operand, ngc_unary_op_t operation)
+FLASHMEM static status_code_t execute_unary (float *operand, ngc_unary_op_t operation)
 {
     status_code_t status = Status_OK;
 
@@ -301,7 +301,7 @@ static status_code_t execute_unary (float *operand, ngc_unary_op_t operation)
 \param operator \ref ngc_binary_op_t enum value.
 \returns precedence level.
 */
-static uint_fast8_t precedence (ngc_binary_op_t operator)
+FLASHMEM static uint_fast8_t precedence (ngc_binary_op_t operator)
 {
     switch(operator)
     {
@@ -350,7 +350,7 @@ value of operation is set to the symbolic value for that operation.
 \param operation pointer to \ref ngc_binary_op_t enum value.
 \returns #Status_OK enum value if processed without error, appropriate \ref status_code_t enum value if not.
 */
-static status_code_t read_operation (char *line, uint_fast8_t *pos, ngc_binary_op_t *operation)
+FLASHMEM static status_code_t read_operation (char *line, uint_fast8_t *pos, ngc_binary_op_t *operation)
 {
     char c = line[*pos];
     status_code_t status = Status_OK;
@@ -475,7 +475,7 @@ value of operation is set to the symbolic value for that operation.
 \param operation pointer to \ref ngc_unary_op_t enum value.
 \returns #Status_OK enum value if processed without error, appropriate \ref status_code_t enum value if not.
 */
-static status_code_t read_operation_unary (char *line, uint_fast8_t *pos, ngc_unary_op_t *operation)
+FLASHMEM static status_code_t read_operation_unary (char *line, uint_fast8_t *pos, ngc_unary_op_t *operation)
 {
     char c = line[*pos];
     status_code_t status = Status_OK;
@@ -589,7 +589,7 @@ starting at the index given by the pos offset.
 \param buffer pointer to a character buffer for the name.
 \returns #Status_OK enum value if processed without error, appropriate \ref status_code_t enum value if not.
 */
-status_code_t ngc_read_name (char *line, uint_fast8_t *pos, char *buffer)
+FLASHMEM status_code_t ngc_read_name (char *line, uint_fast8_t *pos, char *buffer)
 {
     char *s;
     uint_fast8_t len = 0;
@@ -634,7 +634,7 @@ sequentially, the value of #2 would be 10 after the line was executed.
 \param value pointer to float where result is to be stored.
 \returns #Status_OK enum value if processed without error, appropriate \ref status_code_t enum value if not.
 */
-status_code_t ngc_read_parameter (char *line, uint_fast8_t *pos, float *value, bool check)
+FLASHMEM status_code_t ngc_read_parameter (char *line, uint_fast8_t *pos, float *value, bool check)
 {
     int32_t param;
     status_code_t status = Status_BadNumberFormat;
@@ -672,7 +672,7 @@ of the ATAN operation applied to the two arguments.
 \param value pointer to float where result is to be stored.
 \returns #Status_OK enum value if processed without error, appropriate \ref status_code_t enum value if not.
 */
-static status_code_t read_atan (char *line, uint_fast8_t *pos, float *value)
+FLASHMEM static status_code_t read_atan (char *line, uint_fast8_t *pos, float *value)
 {
     float argument2;
 
@@ -701,7 +701,7 @@ handled specially because it is followed by two arguments.
 \param value pointer to float where result is to be stored.
 \returns #Status_OK enum value if processed without error, appropriate \ref status_code_t enum value if not.
 */
-static status_code_t read_unary (char *line, uint_fast8_t *pos, float *value)
+FLASHMEM static status_code_t read_unary (char *line, uint_fast8_t *pos, float *value)
 {
     ngc_unary_op_t operation;
     status_code_t status;
@@ -793,7 +793,7 @@ other readers, depending upon the first character.
 \param value pointer to float where result is to be stored.
 \returns #Status_OK enum value if processed without error, appropriate \ref status_code_t enum value if not.
 */
-status_code_t ngc_read_real_value (char *line, uint_fast8_t *pos, float *value)
+FLASHMEM status_code_t ngc_read_real_value (char *line, uint_fast8_t *pos, float *value)
 {
     char c = line[*pos], c1;
 
@@ -839,7 +839,7 @@ an error will be reported (since a sign is not a digit).
 \param value pointer to integer where result is to be stored.
 \returns #Status_OK enum value if processed without error, appropriate \ref status_code_t enum value if not.
 */
-status_code_t ngc_read_integer_unsigned (char *line, uint_fast8_t *pos, uint32_t *value)
+FLASHMEM status_code_t ngc_read_integer_unsigned (char *line, uint_fast8_t *pos, uint32_t *value)
 {
     return line[*pos] == '+' ? Status_GcodeCommandValueNotInteger : read_uint(line, pos, value);
 }
@@ -858,7 +858,7 @@ close to an integer, then returning the integer it is close to.
 \param value pointer to integer where result is to be stored.
 \returns #Status_OK enum value if processed without error, appropriate \ref status_code_t enum value if not.
 */
-status_code_t ngc_read_integer_value (char *line, uint_fast8_t *pos, int32_t *value)
+FLASHMEM status_code_t ngc_read_integer_value (char *line, uint_fast8_t *pos, int32_t *value)
 {
   float fvalue;
   status_code_t status;
@@ -881,7 +881,7 @@ status_code_t ngc_read_integer_value (char *line, uint_fast8_t *pos, int32_t *va
 \param value pointer to float where result is to be stored.
 \returns #Status_OK enum value if evaluated without error, appropriate \ref status_code_t enum value if not.
 */
-status_code_t ngc_eval_expression (char *line, uint_fast8_t *pos, float *value)
+FLASHMEM status_code_t ngc_eval_expression (char *line, uint_fast8_t *pos, float *value)
 {
     float values[MAX_STACK];
     ngc_binary_op_t operators[MAX_STACK];
@@ -932,7 +932,7 @@ status_code_t ngc_eval_expression (char *line, uint_fast8_t *pos, float *value)
 
 /**/
 
-static int8_t get_format (char c, int8_t pos, uint8_t *decimals)
+FLASHMEM static int8_t get_format (char c, int8_t pos, uint8_t *decimals)
 {
     static uint8_t d;
 
@@ -991,7 +991,7 @@ _NOTE:_ The returned string must be freed by the caller.
 \param line pointer to the original string.
 \returns pointer to the resulting string on success, NULL on failure.
 */
-char *ngc_substitute_parameters (char *line)
+FLASHMEM char *ngc_substitute_parameters (char *line)
 {
     if(line == NULL)
         return NULL;
@@ -1072,7 +1072,7 @@ _NOTE:_ The returned string must be freed by the caller.
 \param comment pointer to the comment string.
 \returns pointer to the resulting string on success, NULL on failure.
 */
-char *ngc_process_comment (char *comment)
+FLASHMEM char *ngc_process_comment (char *comment)
 {
     if(comment == NULL)
         return NULL;
diff --git a/ngc_flowctrl.c b/ngc_flowctrl.c
index 18df987..fc2c22c 100644
--- a/ngc_flowctrl.c
+++ b/ngc_flowctrl.c
@@ -86,7 +86,7 @@ static ngc_sub_t *subs = NULL, *exec_sub = NULL;
 static ngc_stack_entry_t stack[NGC_STACK_DEPTH] = {0};
 static on_gcode_message_ptr on_gcode_comment;
 
-static status_code_t read_command (char *line, uint_fast8_t *pos, ngc_cmd_t *operation)
+FLASHMEM static status_code_t read_command (char *line, uint_fast8_t *pos, ngc_cmd_t *operation)
 {
     char c = line[*pos];
     status_code_t status = Status_OK;
@@ -199,7 +199,7 @@ static status_code_t read_command (char *line, uint_fast8_t *pos, ngc_cmd_t *ope
 }
 
 // Returns the last called named sub with reference count
-static ngc_sub_t *get_refcount (uint32_t *refcount)
+FLASHMEM static ngc_sub_t *get_refcount (uint32_t *refcount)
 {
     ngc_sub_t *sub, *last = NULL;
     uint32_t o_label = 0;
@@ -219,7 +219,7 @@ static ngc_sub_t *get_refcount (uint32_t *refcount)
     return last;
 }
 
-static ngc_sub_t *add_sub (uint32_t o_label, vfs_file_t *file)
+FLASHMEM static ngc_sub_t *add_sub (uint32_t o_label, vfs_file_t *file)
 {
     ngc_sub_t *sub;
 
@@ -241,7 +241,7 @@ static ngc_sub_t *add_sub (uint32_t o_label, vfs_file_t *file)
     return sub;
 }
 
-static void clear_subs (vfs_file_t *file)
+FLASHMEM static void clear_subs (vfs_file_t *file)
 {
     ngc_sub_t *current = subs, *prev = NULL, *next;
 
@@ -262,7 +262,7 @@ static void clear_subs (vfs_file_t *file)
     }
 }
 
-static status_code_t stack_push (uint32_t o_label, ngc_cmd_t operation)
+FLASHMEM static status_code_t stack_push (uint32_t o_label, ngc_cmd_t operation)
 {
     if(stack_idx < (NGC_STACK_DEPTH - 1) && (operation != NGCFlowCtrl_Call || ngc_call_push(&stack[stack_idx + 1]))) {
         stack[++stack_idx].o_label = o_label;
@@ -275,7 +275,7 @@ static status_code_t stack_push (uint32_t o_label, ngc_cmd_t operation)
     return Status_FlowControlStackOverflow;
 }
 
-static bool stack_pull (void)
+FLASHMEM static bool stack_pull (void)
 {
     bool ok;
 
@@ -291,7 +291,7 @@ static bool stack_pull (void)
     return ok;
 }
 
-static void stack_unwind_sub (uint32_t o_label)
+FLASHMEM static void stack_unwind_sub (uint32_t o_label)
 {
     while(stack_idx >= 0 && stack[stack_idx].o_label != o_label)
         stack_pull();
@@ -314,14 +314,14 @@ static void stack_unwind_sub (uint32_t o_label)
 
 // Public functions
 
-void ngc_flowctrl_unwind_stack (vfs_file_t *file)
+FLASHMEM void ngc_flowctrl_unwind_stack (vfs_file_t *file)
 {
     clear_subs(file);
     while(stack_idx >= 0 && stack[stack_idx].file == file)
         stack_pull();
 }
 
-static status_code_t onGcodeComment (char *comment)
+FLASHMEM static status_code_t onGcodeComment (char *comment)
 {
     uint_fast8_t pos = 6;
     status_code_t status = Status_OK;
@@ -343,7 +343,7 @@ static status_code_t onGcodeComment (char *comment)
     return status;
 }
 
-void ngc_flowctrl_init (void)
+FLASHMEM void ngc_flowctrl_init (void)
 {
     static bool init_ok = false;
 
@@ -360,7 +360,7 @@ void ngc_flowctrl_init (void)
 
 // NOTE: onNamedSubError will be called recursively for each
 // redirected file by the grbl.report.status_message() call.
-static status_code_t onNamedSubError (status_code_t status)
+FLASHMEM static status_code_t onNamedSubError (status_code_t status)
 {
     static bool closing = false;
 
@@ -396,7 +396,7 @@ static status_code_t onNamedSubError (status_code_t status)
     return status;
 }
 
-static status_code_t onNamedSubEOF (vfs_file_t *file, status_code_t status)
+FLASHMEM static status_code_t onNamedSubEOF (vfs_file_t *file, status_code_t status)
 {
     if(stack_idx >= 0 && stack[stack_idx].file == file) {
         stream_redirect_close(stack[stack_idx].file);
@@ -406,7 +406,7 @@ static status_code_t onNamedSubEOF (vfs_file_t *file, status_code_t status)
     return status;
 }
 
-status_code_t ngc_flowctrl (uint32_t o_label, char *line, uint_fast8_t *pos, bool *skip)
+FLASHMEM status_code_t ngc_flowctrl (uint32_t o_label, char *line, uint_fast8_t *pos, bool *skip)
 {
     float value;
     bool skipping;
diff --git a/ngc_params.c b/ngc_params.c
index 6b14b2b..2a25a62 100644
--- a/ngc_params.c
+++ b/ngc_params.c
@@ -38,6 +38,7 @@
 #include "settings.h"
 #include "ngc_params.h"
 #include "state_machine.h"
+#include "modbus.h"
 
 #ifndef NGC_MAX_CALL_LEVEL
 #define NGC_MAX_CALL_LEVEL 10
@@ -95,7 +96,7 @@ static ngc_string_id_t ref_id = (uint32_t)-1;
 static ngc_string_param_t *ngc_string_params = NULL;
 static on_macro_execute_ptr on_macro_execute;
 
-static const uint8_t axis_map[] = {
+PROGMEM static const uint8_t axis_map[] = {
 #ifdef ROTATION_ENABLE
     10,
 #else
@@ -138,28 +139,28 @@ static const uint8_t axis_map[] = {
 
 #if N_AXIS > 3
 
-static float _convert_pos (float value, uint_fast8_t axis)
+FLASHMEM static float _convert_pos (float value, uint_fast8_t axis)
 {
     return settings.flags.report_inches && bit_isfalse(settings.steppers.is_rotary.mask, bit(axis)) ? value * 25.4f : value;
 }
 
 #else
 
-static inline float _convert_pos (float value, uint_fast8_t axis)
+FLASHMEM static inline float _convert_pos (float value, uint_fast8_t axis)
 {
     return settings.flags.report_inches ? value * 25.4f : value;
 }
 
 #endif
 
-static float _absolute_pos (uint_fast8_t axis)
+FLASHMEM static float _absolute_pos (uint_fast8_t axis)
 {
     axis = axis_map[axis];
 
     return _convert_pos(axis <= 9 ? sys.position[axis] / settings.axis[axis].steps_per_mm : 0.0f, axis);
 }
 
-static float _relative_pos (uint_fast8_t axis)
+FLASHMEM static float _relative_pos (uint_fast8_t axis)
 {
     axis = axis_map[axis];
 
@@ -168,7 +169,7 @@ static float _relative_pos (uint_fast8_t axis)
 
 // numbered parameters
 
-static float probe_coord (ngc_param_id_t id)
+FLASHMEM static float probe_coord (ngc_param_id_t id)
 {
     float value = 0.0f;
     uint_fast8_t axis = axis_map[(id % 10)];
@@ -181,7 +182,7 @@ static float probe_coord (ngc_param_id_t id)
     return _convert_pos(value, axis);
 }
 
-static float scaling_factors (ngc_param_id_t id)
+FLASHMEM static float scaling_factors (ngc_param_id_t id)
 {
     float *factors = gc_get_scaling();
     uint_fast8_t axis = axis_map[(id % 10)];
@@ -189,7 +190,7 @@ static float scaling_factors (ngc_param_id_t id)
     return axis <= 9 ? factors[axis] : 0.0f;
 }
 
-static float probe_result (ngc_param_id_t id)
+FLASHMEM static float probe_result (ngc_param_id_t id)
 {
     return sys.flags.probe_succeeded ? 1.0f : 0.0f;
 }
@@ -202,24 +203,24 @@ static float home_pos (ngc_param_id_t id)
     return axis <= N_AXIS ? sys.home_position[axis - 1] : 0.0f;
 }
 */
-static float m66_result (ngc_param_id_t id)
+FLASHMEM static float m66_result (ngc_param_id_t id)
 {
     return (float)sys.var5399;
 }
 
-static float tool_number (ngc_param_id_t id)
+FLASHMEM static float tool_number (ngc_param_id_t id)
 {
     return (float)gc_state.tool->tool_id;
 }
 
-static float tool_offset (ngc_param_id_t id)
+FLASHMEM static float tool_offset (ngc_param_id_t id)
 {
     uint_fast8_t axis = axis_map[(id % 10)];
 
     return axis <= 9 ? gc_state.modal.tool_length_offset[axis] : 0.0f;
 }
 
-static float g28_home (ngc_param_id_t id)
+FLASHMEM static float g28_home (ngc_param_id_t id)
 {
     float value = 0.0f;
     uint_fast8_t axis = axis_map[(id % 10)];
@@ -231,7 +232,7 @@ static float g28_home (ngc_param_id_t id)
     return value;
 }
 
-static float g30_home (ngc_param_id_t id)
+FLASHMEM static float g30_home (ngc_param_id_t id)
 {
     float value = 0.0f;
     uint_fast8_t axis = axis_map[(id % 10)];
@@ -243,12 +244,12 @@ static float g30_home (ngc_param_id_t id)
     return value;
 }
 
-static float coord_system (ngc_param_id_t id)
+FLASHMEM static float coord_system (ngc_param_id_t id)
 {
     return (float)gc_state.modal.g5x_offset.id + 1;
 }
 
-static float coord_system_offset (ngc_param_id_t id)
+FLASHMEM static float coord_system_offset (ngc_param_id_t id)
 {
     float value = 0.0f;
     uint_fast8_t axis = id % 10;
@@ -274,24 +275,24 @@ static float coord_system_offset (ngc_param_id_t id)
     return value;
 }
 
-static float g92_offset_applied (ngc_param_id_t id)
+FLASHMEM static float g92_offset_applied (ngc_param_id_t id)
 {
     return (float)gc_state.g92_offset_applied;
 }
 
-static float g92_offset (ngc_param_id_t id)
+FLASHMEM static float g92_offset (ngc_param_id_t id)
 {
     uint_fast8_t axis = axis_map[(id % 10)];
 
     return axis <= 9 ? gc_state.g92_offset.coord.values[axis] : 0.0f;
 }
 
-static float work_position (ngc_param_id_t id)
+FLASHMEM static float work_position (ngc_param_id_t id)
 {
     return _relative_pos(id % 10 + 1);
 }
 
-static float debug_output (ngc_param_id_t id)
+FLASHMEM static float debug_output (ngc_param_id_t id)
 {
     return (float)settings.flags.ngc_debug_out;
 }
@@ -321,7 +322,7 @@ PROGMEM static const ngc_ro_param_t ngc_ro_params[] = {
     { .id_min = 5599, .id_max = 5599, .get = debug_output }         // LinuxCNC
 };
 
-bool ngc_param_get (ngc_param_id_t id, float *value)
+FLASHMEM bool ngc_param_get (ngc_param_id_t id, float *value)
 {
     bool found = id > 0 && id < ngc_ro_params[0].id_min;
     uint_fast8_t idx = sizeof(ngc_ro_params) / sizeof(ngc_ro_param_t);
@@ -347,17 +348,17 @@ bool ngc_param_get (ngc_param_id_t id, float *value)
     return found;
 }
 
-bool ngc_param_is_rw (ngc_param_id_t id)
+FLASHMEM bool ngc_param_is_rw (ngc_param_id_t id)
 {
     return id > 0 && id < ngc_ro_params[0].id_min;
 }
 
-bool ngc_param_exists (ngc_param_id_t id)
+FLASHMEM bool ngc_param_exists (ngc_param_id_t id)
 {
     return id > 0 && id <= ngc_ro_params[(sizeof(ngc_ro_params) / sizeof(ngc_ro_param_t)) - 1].id_max;
 }
 
-bool ngc_param_set (ngc_param_id_t id, float value)
+FLASHMEM bool ngc_param_set (ngc_param_id_t id, float value)
 {
     bool ok = id > 0 && id < ngc_ro_params[0].id_min;
 
@@ -462,7 +463,7 @@ PROGMEM static const ngc_named_ro_param_t ngc_named_ro_param[] = {
 
 // Named parameters
 
-float ngc_named_param_get_by_id (ncg_name_param_id_t id)
+FLASHMEM float ngc_named_param_get_by_id (ncg_name_param_id_t id)
 {
     float value;
 
@@ -709,7 +710,7 @@ float ngc_named_param_get_by_id (ncg_name_param_id_t id)
 }
 
 // Lowercase name, remove control characters and spaces
-static char *ngc_name_tolower (char *s)
+FLASHMEM static char *ngc_name_tolower (char *s)
 {
     static char name[NGC_MAX_PARAM_LENGTH + 1];
 
@@ -727,7 +728,7 @@ static char *ngc_name_tolower (char *s)
 	return name;
 }
 
-bool ngc_named_param_get (char *name, float *value)
+FLASHMEM bool ngc_named_param_get (char *name, float *value)
 {
     bool found = false;
     uint_fast8_t idx = sizeof(ngc_named_ro_param) / sizeof(ngc_named_ro_param_t);
@@ -760,14 +761,14 @@ bool ngc_named_param_get (char *name, float *value)
     return found;
 }
 
-bool ngc_named_param_exists (char *name)
+FLASHMEM bool ngc_named_param_exists (char *name)
 {
     float value;
 
     return ngc_named_param_get(name, &value);
 }
 
-float *ngc_named_param_set (char *name, float value)
+FLASHMEM float *ngc_named_param_set (char *name, float value)
 {
     bool ok = false;
     uint_fast8_t idx = sizeof(ngc_named_ro_param) / sizeof(ngc_named_ro_param_t);
@@ -820,7 +821,7 @@ float *ngc_named_param_set (char *name, float value)
     return ok ? &rw_param->value : NULL;
 }
 
-static ngc_string_param_t *sp_get_by_name (char *name)
+FLASHMEM static ngc_string_param_t *sp_get_by_name (char *name)
 {
     ngc_string_param_t *sr = ngc_string_params;
 
@@ -832,7 +833,7 @@ static ngc_string_param_t *sp_get_by_name (char *name)
     return sr;
 }
 
-static ngc_string_param_t *sp_set (ngc_string_id_t id, char *value)
+FLASHMEM static ngc_string_param_t *sp_set (ngc_string_id_t id, char *value)
 {
     size_t len;
     ngc_string_param_t *last = NULL, *sp;
@@ -889,7 +890,7 @@ static ngc_string_param_t *sp_set (ngc_string_id_t id, char *value)
     return sp;
 }
 
-char *ngc_string_param_get (ngc_string_id_t id)
+FLASHMEM char *ngc_string_param_get (ngc_string_id_t id)
 {
     ngc_string_param_t *sp;
 
@@ -901,17 +902,17 @@ char *ngc_string_param_get (ngc_string_id_t id)
     return sp ? sp->value : NULL;
 }
 
-bool ngc_string_param_exists (ngc_string_id_t id)
+FLASHMEM bool ngc_string_param_exists (ngc_string_id_t id)
 {
     return !!ngc_string_param_get(id);
 }
 
-bool ngc_string_param_set (ngc_param_id_t id, char *value)
+FLASHMEM bool ngc_string_param_set (ngc_param_id_t id, char *value)
 {
     return id > 0 && !!sp_set((ngc_string_id_t)id, value);
 }
 
-ngc_string_id_t ngc_string_param_set_name (char *name)
+FLASHMEM ngc_string_id_t ngc_string_param_set_name (char *name)
 {
     ngc_string_param_t *sr = *name ? sp_get_by_name(name) : NULL;
 
@@ -921,7 +922,7 @@ ngc_string_id_t ngc_string_param_set_name (char *name)
     return sr ? sr->id : 0;
 }
 
-void ngc_string_param_delete (ngc_string_id_t id)
+FLASHMEM void ngc_string_param_delete (ngc_string_id_t id)
 {
     ngc_string_param_t *sr = ngc_string_params, *rm;
 
@@ -941,7 +942,7 @@ void ngc_string_param_delete (ngc_string_id_t id)
     }
 }
 
-bool ngc_modal_state_save (gc_modal_t *state, gc_override_values_t *overrides, float feed_rate, bool auto_restore)
+FLASHMEM bool ngc_modal_state_save (gc_modal_t *state, gc_override_values_t *overrides, float feed_rate, bool auto_restore)
 {
     gc_modal_snapshot_t **saved_state = call_level == -1 ? &modal_state : &call_levels[call_level].modal_state;
 
@@ -958,7 +959,7 @@ bool ngc_modal_state_save (gc_modal_t *state, gc_override_values_t *overrides, f
     return *saved_state != NULL;
 }
 
-void ngc_modal_state_invalidate (void)
+FLASHMEM void ngc_modal_state_invalidate (void)
 {
     gc_modal_snapshot_t **saved_state = call_level == -1 ? &modal_state : &call_levels[call_level].modal_state;
 
@@ -968,17 +969,17 @@ void ngc_modal_state_invalidate (void)
     }
 }
 
-gc_modal_snapshot_t *ngc_modal_state_get (void)
+FLASHMEM gc_modal_snapshot_t *ngc_modal_state_get (void)
 {
     return call_level == -1 ? modal_state : call_levels[call_level].modal_state;
 }
 
-bool ngc_modal_state_restore (void)
+FLASHMEM 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)
+FLASHMEM bool ngc_call_push (void *context)
 {
     bool ok;
 
@@ -988,7 +989,7 @@ bool ngc_call_push (void *context)
     return ok;
 }
 
-bool ngc_call_pop (void)
+FLASHMEM bool ngc_call_pop (void)
 {
     if(call_level >= 0) {
 
@@ -1042,17 +1043,17 @@ bool ngc_call_pop (void)
     return call_level >= 0;
 }
 
-uint_fast8_t ngc_call_level (void)
+FLASHMEM uint_fast8_t ngc_call_level (void)
 {
     return (uint_fast8_t)(call_level + 1);
 }
 
-uint8_t ngc_float_decimals (void)
+FLASHMEM uint8_t ngc_float_decimals (void)
 {
 	return settings.flags.report_inches ? N_DECIMAL_COORDVALUE_INCH : N_DECIMAL_COORDVALUE_MM;
 }
 
-static status_code_t macro_set_get_setting (parameter_words_t args)
+FLASHMEM static status_code_t macro_set_get_setting (parameter_words_t args)
 {
     float setting_id;
     status_code_t status = Status_OK;
@@ -1086,7 +1087,7 @@ static status_code_t macro_set_get_setting (parameter_words_t args)
     return status;
 }
 
-static status_code_t macro_ngc_parameter_rw (parameter_words_t args)
+FLASHMEM static status_code_t macro_ngc_parameter_rw (parameter_words_t args)
 {
     float idx, value;
     status_code_t status = Status_OK;
@@ -1113,7 +1114,7 @@ static status_code_t macro_ngc_parameter_rw (parameter_words_t args)
     return status;
 }
 
-static status_code_t macro_get_machine_state (parameter_words_t args)
+FLASHMEM static status_code_t macro_get_machine_state (parameter_words_t args)
 {
     ngc_named_param_set("_value", (float)ffs(state_get()));
     ngc_named_param_set("_value_returned", 1.0f);
@@ -1121,7 +1122,7 @@ static status_code_t macro_get_machine_state (parameter_words_t args)
     return Status_OK;
 }
 
-static status_code_t macro_select_probe (parameter_words_t args)
+FLASHMEM static status_code_t macro_select_probe (parameter_words_t args)
 {
     float probe_id;
     status_code_t status = Status_OK;
@@ -1136,7 +1137,7 @@ static status_code_t macro_select_probe (parameter_words_t args)
     return status;
 }
 
-static status_code_t macro_get_tool_offset (parameter_words_t args)
+FLASHMEM static status_code_t macro_get_tool_offset (parameter_words_t args)
 {
     float tool_id, axis_id;
     status_code_t status = Status_OK;
@@ -1158,7 +1159,66 @@ static status_code_t macro_get_tool_offset (parameter_words_t args)
     return status;
 }
 
-static status_code_t onMacroExecute (macro_id_t macro_id, parameter_words_t args, uint32_t repeats)
+static void modbus_response_handler (modbus_response_t *response)
+{
+    ngc_named_param_set("_value", (float)(response->exception ? response->exception : response->values[0]));
+    ngc_named_param_set("_value_returned", response->exception ? 0.0f : (float)response->num_values);
+    if(response->exception == ModBus_NoException) {
+        if(response->num_values == 2)
+            ngc_named_param_set("_value2", (float)(response->exception ? response->exception : response->values[1]));
+        if(response->num_values == 3)
+            ngc_named_param_set("_value2", (float)(response->exception ? response->exception : response->values[2]));
+    }
+}
+
+FLASHMEM static status_code_t macro_modbus_msg (parameter_words_t args)
+{
+    float tmpvar;
+    status_code_t status = Status_OK;
+
+    if(!(args.f && args.s))
+        status = Status_GcodeValueWordMissing;
+    else if(ngc_param_get(9 /* F word -> function */, &tmpvar)) {
+
+        uint16_t server, address = 0, n_values = 0, values[MODBUS_MAX_REGISTERS];
+
+        const modbus_function_properties_t *p = modbus_get_function_properties((modbus_function_t)tmpvar);
+
+        if(p->function != ModBus_ReadExceptionStatus && p->function && args.r && ngc_param_get(18 /* R word - register address */, &tmpvar)) {
+
+            address = (uint16_t)tmpvar;
+
+            if(p->is_write) {
+
+                if(args.a && ngc_param_get(1 /* A word - first register value */, &tmpvar))
+                    values[n_values++] = (uint16_t)tmpvar;
+
+                if(!p->single_register) {
+                    if(args.b && ngc_param_get(2 /* B word - second register value */, &tmpvar))
+                        values[n_values++] = (uint16_t)tmpvar;
+                    if(args.c && ngc_param_get(3 /* C word - third register value */, &tmpvar))
+                        values[n_values++] = (uint16_t)tmpvar;
+                }
+            } else if(args.x && ngc_param_get(24 /* X word - number of registers to read */, &tmpvar)) {
+                if((n_values = (uint16_t)tmpvar) > MODBUS_MAX_REGISTERS)
+                    return Status_GcodeValueOutOfRange;
+            } else
+                n_values = 1;
+        }
+
+        if((p->function == ModBus_ReadExceptionStatus || n_values) &&
+             args.s && ngc_param_get(19 /* S word - server address */, &tmpvar)) {
+            server = (uint16_t)tmpvar;
+            status = modbus_message(server, p->function, address, values, n_values, modbus_response_handler);
+        } else
+            status = p->function ? Status_GcodeValueWordMissing : Status_GcodeUnsupportedCommand;
+    } else
+        status = Status_GcodeUnsupportedCommand;
+
+    return status;
+}
+
+FLASHMEM static status_code_t onMacroExecute (macro_id_t macro_id, parameter_words_t args, uint32_t repeats)
 {
     status_code_t status = repeats > 1 && macro_id >= 1 && macro_id <= G65Macro_LastInbuilt
                    ? Status_GcodeValueOutOfRange
@@ -1190,12 +1250,17 @@ static status_code_t onMacroExecute (macro_id_t macro_id, parameter_words_t args
             sys.override.control.spindle_wait_disable = On;
             status = Status_OK;
             break;
+
+        case G65Macro_ModbusMessage:
+            status = modbus_isup().ok ? macro_modbus_msg(args) : Status_GcodeUnsupportedCommand;
+            break;
+
     }
 
     return status == Status_Unhandled && on_macro_execute ? on_macro_execute(macro_id, args, repeats) : status;
 }
 
-void ngc_params_init (void)
+FLASHMEM void ngc_params_init (void)
 {
     static bool init_ok = false;
 
diff --git a/ngc_params.h b/ngc_params.h
index 2c5954c..eaa16d1 100644
--- a/ngc_params.h
+++ b/ngc_params.h
@@ -3,7 +3,7 @@
 
   Part of grblHAL
 
-  Copyright (c) 2021-2025 Terje Io
+  Copyright (c) 2021-2026 Terje Io
 
   grblHAL is free software: you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
@@ -118,7 +118,8 @@ typedef enum {
     G65Macro_GetMachineState     = 4,
     G65Macro_SelectProbe         = 5,
     G65Macro_SpindleDelayDisable = 6,
-    G65Macro_LastInbuilt         = G65Macro_SpindleDelayDisable
+    G65Macro_ModbusMessage       = 7,
+    G65Macro_LastInbuilt         = G65Macro_ModbusMessage
 } g65_inbuilt_t;
 
 void ngc_params_init (void);
diff --git a/nuts_bolts.c b/nuts_bolts.c
index 3b8581e..1613a23 100644
--- a/nuts_bolts.c
+++ b/nuts_bolts.c
@@ -40,7 +40,7 @@
 
 static char buf[STRLEN_COORDVALUE + 1];
 
-static const float froundvalues[MAX_PRECISION + 1] =
+PROGMEM static const float froundvalues[MAX_PRECISION + 1] =
 {
     0.5f,                // 0
     0.05f,               // 1
@@ -321,7 +321,7 @@ bool isintf (float value)
 }
 
 // Non-blocking delay function used for general operation and suspend features.
-bool delay_sec (float seconds, delaymode_t mode)
+FLASHMEM bool delay_sec (float seconds, delaymode_t mode)
 {
     bool ok = true;
 
@@ -361,7 +361,7 @@ float convert_delta_vector_to_unit_vector (float *vector)
     return magnitude;
 }
 
-void rotate (coord_data_t *pt, plane_t plane, float angle /*rad*/)
+FLASHMEM void rotate (coord_data_t *pt, plane_t plane, float angle /*rad*/)
 {
     float cos = cosf(angle), sin = sinf(angle),
           t0 = pt->values[plane.axis_0] * cos - pt->values[plane.axis_1] * sin,
@@ -372,7 +372,7 @@ void rotate (coord_data_t *pt, plane_t plane, float angle /*rad*/)
 }
 
 // parse ISO8601 datetime: YYYY-MM-DDTHH:MM:SSZxxx
-struct tm *get_datetime (const char *s)
+FLASHMEM struct tm *get_datetime (const char *s)
 {
     static struct tm dt;
     PROGMEM static const uint8_t mdays[] = {31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31};
diff --git a/nvs_buffer.c b/nvs_buffer.c
index accc60c..ff191cd 100644
--- a/nvs_buffer.c
+++ b/nvs_buffer.c
@@ -64,7 +64,7 @@ typedef struct {
 #define TOOL_ADDR(n) (NVS_ADDR_TOOL_TABLE + n * (sizeof(tool_data_t) + NVS_CRC_BYTES))
 #endif
 
-static const emap_t target[] = {
+PROGMEM static const emap_t target[] = {
     {NVS_ADDR_GLOBAL, NVS_GROUP_GLOBAL, 0},
 
     {PARAMETER_ADDR(0), NVS_GROUP_PARAMETERS, 0},
@@ -139,7 +139,7 @@ inline static void ram_put_byte (uint32_t offset, uint8_t new_value)
     nvsbuffer.addr[offset] = new_value;
 }
 
-static nvs_transfer_result_t memcpy_to_ram (uint32_t destination, uint8_t *source, uint32_t size, bool with_checksum)
+FLASHMEM static nvs_transfer_result_t memcpy_to_ram (uint32_t destination, uint8_t *source, uint32_t size, bool with_checksum)
 {
     if(hal.nvs.driver_area.address && destination > hal.nvs.driver_area.address + hal.nvs.driver_area.size)
         return physical_nvs.memcpy_to_nvs(destination, source, size, with_checksum);
@@ -206,7 +206,7 @@ static nvs_transfer_result_t memcpy_to_ram (uint32_t destination, uint8_t *sourc
     return NVS_TransferResult_OK;
 }
 
-static nvs_transfer_result_t memcpy_from_ram (uint8_t *destination, uint32_t source, uint32_t size, bool with_checksum)
+FLASHMEM static nvs_transfer_result_t memcpy_from_ram (uint8_t *destination, uint32_t source, uint32_t size, bool with_checksum)
 {
     if(hal.nvs.driver_area.address && source > hal.nvs.driver_area.address + hal.nvs.driver_area.size)
         return physical_nvs.memcpy_from_nvs(destination, source, size, with_checksum);
@@ -224,7 +224,7 @@ static nvs_transfer_result_t memcpy_from_ram (uint8_t *destination, uint32_t sou
 }
 
 // Try to allocate RAM from heap for buffer/emulation.
-bool nvs_buffer_alloc (void)
+FLASHMEM bool nvs_buffer_alloc (void)
 {
 	if(nvsbuffer.addr == NULL) {
 
@@ -238,7 +238,7 @@ bool nvs_buffer_alloc (void)
     return nvsbuffer.addr != NULL;
 }
 
-void nvs_buffer_free (void)
+FLASHMEM void nvs_buffer_free (void)
 {
     if(nvsbuffer.addr) {
         nvs_buffer_sync_physical();
@@ -248,7 +248,7 @@ void nvs_buffer_free (void)
 //
 // Switch over to RAM based copy.
 // Changes to RAM based copy will be written to physical storage when grblHAL is in IDLE state.
-bool nvs_buffer_init (void)
+FLASHMEM bool nvs_buffer_init (void)
 {
     hal.nvs.size = ((hal.nvs.size - 1) | 0x03) + 1; // Ensure NVS area ends on a word boundary
 
@@ -293,7 +293,7 @@ bool nvs_buffer_init (void)
 
 // Allocate NVS block for driver settings.
 // NOTE: allocation has to be done before content is copied from physical storage.
-nvs_address_t nvs_alloc (size_t size)
+FLASHMEM nvs_address_t nvs_alloc (size_t size)
 {
     static uint8_t *mem_address;
 
@@ -322,7 +322,7 @@ nvs_address_t nvs_alloc (size_t size)
 }
 
 // Write RAM changes to physical storage
-void nvs_buffer_sync_physical (void)
+FLASHMEM void nvs_buffer_sync_physical (void)
 {
     if(!settings_dirty.is_dirty)
         return;
@@ -397,7 +397,7 @@ void nvs_buffer_sync_physical (void)
     }
 }
 
-nvs_io_t *nvs_buffer_get_physical (void)
+FLASHMEM nvs_io_t *nvs_buffer_get_physical (void)
 {
     return hal.nvs.type == NVS_Emulated ? &physical_nvs : &hal.nvs;
 }
@@ -406,7 +406,7 @@ nvs_io_t *nvs_buffer_get_physical (void)
 
 #include "report.h"
 
-void nvs_memmap (void)
+FLASHMEM void nvs_memmap (void)
 {
     char buf[30];
 
diff --git a/override.c b/override.c
index 4e54e2e..3c63157 100644
--- a/override.c
+++ b/override.c
@@ -43,7 +43,7 @@ ISR_CODE void ISR_FUNC(enqueue_feed_override)(uint8_t cmd)
 }
 
 // Returns 0 if no commands enqueued
-uint8_t get_feed_override (void)
+FLASHMEM uint8_t get_feed_override (void)
 {
     uint8_t data = 0;
     uint_fast8_t bptr = feed.tail;
@@ -67,7 +67,7 @@ ISR_CODE void ISR_FUNC(enqueue_spindle_override)(uint8_t cmd)
 }
 
 // Returns 0 if no commands enqueued
-uint8_t get_spindle_override (void)
+FLASHMEM uint8_t get_spindle_override (void)
 {
     uint8_t data = 0;
     uint_fast8_t bptr = spindle.tail;
@@ -91,7 +91,7 @@ ISR_CODE void ISR_FUNC(enqueue_coolant_override)(uint8_t cmd)
 }
 
 // Returns 0 if no commands enqueued
-uint8_t get_coolant_override (void)
+FLASHMEM uint8_t get_coolant_override (void)
 {
     uint8_t data = 0;
     uint_fast8_t bptr = coolant.tail;
@@ -104,7 +104,7 @@ uint8_t get_coolant_override (void)
     return data;
 }
 
-void flush_override_buffers (void)
+FLASHMEM void flush_override_buffers (void)
 {
     feed.head = feed.tail = spindle.head = spindle.tail = coolant.head = coolant.tail = 0;
 }
diff --git a/planner.c b/planner.c
index fbced79..b9945aa 100644
--- a/planner.c
+++ b/planner.c
@@ -227,7 +227,7 @@ uint_fast16_t plan_get_buffer_size (void)
     return block_buffer.size;
 }
 
-bool plan_reset (void)
+FLASHMEM bool plan_reset (void)
 {
     if(block_buffer.blocks == NULL) {
 
@@ -741,7 +741,7 @@ void plan_sync_velocity (void *block)
 }
 
 // Set feed overrides
-void plan_feed_override (override_t feed_override, override_t rapid_override)
+FLASHMEM void plan_feed_override (override_t feed_override, override_t rapid_override)
 {
     bool feedrate_changed = false, rapidrate_changed = false;
 
@@ -774,7 +774,7 @@ void plan_feed_override (override_t feed_override, override_t rapid_override)
     }
 }
 
-void plan_data_init (plan_line_data_t *plan_data)
+FLASHMEM 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;
diff --git a/probe.c b/probe.c
index d930cd1..d96519a 100644
--- a/probe.c
+++ b/probe.c
@@ -48,7 +48,7 @@ static void probe_irq_handler (uint8_t port, bool state)
 }
 
 // Toggle probe connected status.
-static void probe_connected_toggle (void)
+FLASHMEM static void probe_connected_toggle (void)
 {
     if(!probe_state.is_probing) {
         if((probe->flags.connected = probe_state.connected = !probe_state.connected)) {
@@ -65,7 +65,7 @@ static void probe_connected_toggle (void)
 // Sets up the probe pin invert mask to
 // appropriately set the pin logic according to setting for normal-high/normal-low operation
 // and the probing cycle modes for toward-workpiece/away-from-workpiece.
-static void probe_configure (bool is_probe_away, bool probing)
+FLASHMEM static void probe_configure (bool is_probe_away, bool probing)
 {
     bool invert = !!(settings.probe.value & probe->inverted_bit.value);
 
@@ -94,7 +94,7 @@ static void probe_configure (bool is_probe_away, bool probing)
     probe_state.is_probing = probing;
 }
 
-static bool probe_select (probe_id_t probe_id)
+FLASHMEM static bool probe_select (probe_id_t probe_id)
 {
     uint_fast8_t idx = 0;
     probe_t *selected_probe = NULL;
@@ -116,7 +116,7 @@ static bool probe_select (probe_id_t probe_id)
     return probe == selected_probe;
 }
 
-static probe_flags_t probe_get_caps (probe_id_t probe_id)
+FLASHMEM static probe_flags_t probe_get_caps (probe_id_t probe_id)
 {
     uint_fast8_t idx = 0;
     probe_t *probe = NULL;
@@ -129,7 +129,7 @@ static probe_flags_t probe_get_caps (probe_id_t probe_id)
     return probe ? probe->flags : (probe_flags_t){0};
 }
 
-static bool is_triggered (probe_id_t probe_id)
+FLASHMEM static bool is_triggered (probe_id_t probe_id)
 {
     uint_fast8_t idx = 0;
     probe_t *probe = NULL;
@@ -143,7 +143,7 @@ static bool is_triggered (probe_id_t probe_id)
 }
 
 // Returns the probe connected and triggered pin states.
-static probe_state_t get_state (void)
+FLASHMEM static probe_state_t get_state (void)
 {
     probe_state_t state = { .value = probe_state.value };
 
@@ -158,7 +158,7 @@ static probe_state_t get_state (void)
     return state;
 }
 
-bool probe_add (probe_id_t probe_id, uint8_t port, pin_irq_mode_t irq_mode, void *input, get_probe_input_ptr get_input)
+FLASHMEM bool probe_add (probe_id_t probe_id, uint8_t port, pin_irq_mode_t irq_mode, void *input, get_probe_input_ptr get_input)
 {
     static uint_fast8_t n_probes = 0;
 
@@ -212,7 +212,7 @@ bool probe_add (probe_id_t probe_id, uint8_t port, pin_irq_mode_t irq_mode, void
     return true;
 }
 
-void probe_connected_event (void *data)
+FLASHMEM void probe_connected_event (void *data)
 {
     if(hal.probe.connected_toggle) {
         if((uintptr_t)data == 2)
diff --git a/protocol.c b/protocol.c
index d472d0f..ad00169 100644
--- a/protocol.c
+++ b/protocol.c
@@ -59,7 +59,7 @@ static bool keep_rt_commands = false;
 static void protocol_exec_rt_suspend (sys_state_t state);
 
 // add gcode to execute not originating from normal input stream
-bool protocol_enqueue_gcode (char *gcode)
+FLASHMEM bool protocol_enqueue_gcode (char *gcode)
 {
     bool ok = xcommand[0] == '\0' &&
                (state_get() == STATE_IDLE || (state_get() & (STATE_ALARM|STATE_JOG|STATE_TOOL_CHANGE))) &&
@@ -74,7 +74,7 @@ bool protocol_enqueue_gcode (char *gcode)
     return ok;
 }
 
-static bool recheck_line (char *line, line_flags_t *flags)
+FLASHMEM static bool recheck_line (char *line, line_flags_t *flags)
 {
     bool keep_rt_commands = false, first_char = true;
 
@@ -173,9 +173,13 @@ bool protocol_main_loop (void)
             protocol_execute_realtime(); // Enter safety door mode. Should return as IDLE state.
         }
 #endif
-        // All systems go!
-        if(!settings.homing.flags.nx_scrips_on_homed_only)
-            task_add_immediate(system_execute_startup, NULL); // Schedule startup script for execution.
+        if(sys.alarm_pending)
+            system_raise_alarm(sys.alarm_pending);
+        else {
+            // All systems go!
+            if(!settings.homing.flags.nx_scrips_on_homed_only)
+                task_add_immediate(system_execute_startup, NULL); // Schedule startup script for execution.
+        }
     }
 
     // Ensure spindle and coolant is switched off on a cold start
@@ -414,7 +418,7 @@ bool protocol_execute_realtime (void)
     return !ABORTED;
 }
 
-static void protocol_poll_cmd (void)
+FLASHMEM static void protocol_poll_cmd (void)
 {
     int16_t c;
 
@@ -562,11 +566,10 @@ bool protocol_exec_rt_system (void)
 
             sys.flags.keep_input = Off;
 
-            if(sys.alarm_pending != Alarm_None) {
+            if(sys.alarm_pending) {
 
                 sys.position_lost = st_is_stepping();
                 system_raise_alarm(sys.alarm_pending);
-                sys.alarm_pending = Alarm_None;
 
             } else if(st_is_stepping()) {
 
@@ -779,7 +782,7 @@ bool protocol_exec_rt_system (void)
 // whatever function that invoked the suspend, such that grblHAL resumes normal operation.
 // This function is written in a way to promote custom parking motions. Simply use this as a
 // template.
-static void protocol_exec_rt_suspend (sys_state_t state)
+FLASHMEM static void protocol_exec_rt_suspend (sys_state_t state)
 {
     if((sys.blocking_event = state == STATE_SLEEP)) {
         *line = '\0';
diff --git a/report.c b/report.c
index 140cdd1..2c9198f 100644
--- a/report.c
+++ b/report.c
@@ -197,7 +197,7 @@ inline static char *control_signals_tostring (char *buf, control_signals_t signa
     return buf;
 }
 
-void report_init (void)
+FLASHMEM void report_init (void)
 {
     get_axis_value = settings.flags.report_inches ? get_axis_value_inches : get_axis_value_mm;
     get_axis_values = settings.flags.report_inches ? get_axis_values_inches : get_axis_values_mm;
@@ -210,7 +210,7 @@ void report_init (void)
 // operation. Errors events can originate from the g-code parser, settings module, or asynchronously
 // from a critical error, such as a triggered hard limit. Interface should always monitor for these
 // responses.
-static status_code_t report_status_message (status_code_t status_code)
+FLASHMEM static status_code_t report_status_message (status_code_t status_code)
 {
     switch(status_code) {
 
@@ -230,7 +230,7 @@ static status_code_t report_status_message (status_code_t status_code)
 }
 
 // Prints alarm messages.
-static alarm_code_t report_alarm_message (alarm_code_t alarm_code)
+FLASHMEM static alarm_code_t report_alarm_message (alarm_code_t alarm_code)
 {
     hal.stream.write_all(appendbuf(3, "ALARM:", uitoa((uint32_t)alarm_code), ASCII_EOL));
     hal.delay_ms(100, NULL); // Force delay to ensure message clears output stream buffer.
@@ -239,7 +239,7 @@ static alarm_code_t report_alarm_message (alarm_code_t alarm_code)
 }
 
 // Prints feedback message, typically from gcode.
-void report_message (const char *msg, message_type_t type)
+FLASHMEM void report_message (const char *msg, message_type_t type)
 {
     if(hal.stream.is_connected()) {
 
@@ -273,19 +273,19 @@ void report_message (const char *msg, message_type_t type)
 }
 
 // Message helper to be run as foreground task
-void report_plain (void *message)
+FLASHMEM void report_plain (void *message)
 {
     report_message((char *)message, Message_Plain);
 }
 
 // Message helper to be run as foreground task
-void report_info (void *message)
+FLASHMEM void report_info (void *message)
 {
     report_message((char *)message, Message_Info);
 }
 
 // Message helper to be run as foreground task
-void report_warning (void *message)
+FLASHMEM void report_warning (void *message)
 {
     report_message((char *)message, Message_Warning);
 }
@@ -295,7 +295,7 @@ void report_warning (void *message)
 // messages such as setup warnings, switch toggling, and how to exit alarms.
 // NOTE: For interfaces, messages are always placed within brackets. And if silent mode
 // is installed, the message number codes are less than zero.
-static message_code_t report_feedback_message (message_code_t id)
+FLASHMEM static message_code_t report_feedback_message (message_code_t id)
 {
     const message_t *msg = message_get(id);
 
@@ -308,7 +308,7 @@ static message_code_t report_feedback_message (message_code_t id)
 }
 
 // Welcome message
-static void report_init_message (stream_write_ptr write)
+FLASHMEM static void report_init_message (stream_write_ptr write)
 {
     hal.stream.report.override_counter = hal.stream.report.wco_counter = 0;
 
@@ -320,13 +320,13 @@ static void report_init_message (stream_write_ptr write)
 }
 
 // grblHAL help message
-static void report_help_message (void)
+FLASHMEM static void report_help_message (void)
 {
     hal.stream.write("[HLP:$$ $# $G $I $N $x=val $Nx=line $J=line $SLP $C $X $H $B ~ ! ? ctrl-x]" ASCII_EOL);
 }
 
 // Prints plugin info.
-void report_plugin (const char *name, const char *version)
+FLASHMEM void report_plugin (const char *name, const char *version)
 {
     hal.stream.write("[PLUGIN:");
     hal.stream.write(name);
@@ -335,7 +335,7 @@ void report_plugin (const char *name, const char *version)
     hal.stream.write("]" ASCII_EOL);
 }
 
-static bool report_group_settings (const setting_group_detail_t *groups, const uint_fast8_t n_groups, char *args)
+FLASHMEM static bool report_group_settings (const setting_group_detail_t *groups, const uint_fast8_t n_groups, char *args)
 {
     bool found = false;
     uint_fast8_t idx;
@@ -365,7 +365,7 @@ static bool report_group_settings (const setting_group_detail_t *groups, const u
     return found;
 }
 
-status_code_t report_help (char *args)
+FLASHMEM status_code_t report_help (char *args)
 {
     if(*args == '\0') {
 
@@ -412,12 +412,12 @@ status_code_t report_help (char *args)
 
 // grblHAL settings print out.
 
-static int cmp_settings (const void *a, const void *b)
+FLASHMEM static int cmp_settings (const void *a, const void *b)
 {
   return (*(setting_detail_t **)(a))->id - (*(setting_detail_t **)(b))->id;
 }
 
-static bool report_setting (const setting_detail_t *setting, uint_fast16_t offset, void *data)
+FLASHMEM static bool report_setting (const setting_detail_t *setting, uint_fast16_t offset, void *data)
 {
     appendbuf(3, "$", uitoa(setting->id + offset), "=");
 
@@ -432,7 +432,7 @@ static bool report_setting (const setting_detail_t *setting, uint_fast16_t offse
     return true;
 }
 
-status_code_t report_grbl_setting (setting_id_t id, void *data)
+FLASHMEM status_code_t report_grbl_setting (setting_id_t id, void *data)
 {
     status_code_t status = Status_OK;
 
@@ -446,7 +446,7 @@ status_code_t report_grbl_setting (setting_id_t id, void *data)
     return status;
 }
 
-static bool print_setting (const setting_detail_t *setting, uint_fast16_t offset, void *data)
+FLASHMEM static bool print_setting (const setting_detail_t *setting, uint_fast16_t offset, void *data)
 {
     if(setting->value != NULL)
         grbl.report.setting(setting, offset, data);
@@ -464,7 +464,7 @@ static inline bool is_hidden (const setting_detail_t *setting)
     return (setting->id == Setting_HomingFeedRate || setting->id == Setting_HomingSeekRate) ? settings.homing.flags.per_axis_feedrates : setting->flags.hidden;
 }
 
-void report_grbl_settings (bool all, void *data)
+FLASHMEM void report_grbl_settings (bool all, void *data)
 {
 
     uint_fast16_t idx, n_settings = 0;
@@ -520,7 +520,7 @@ void report_grbl_settings (bool all, void *data)
 // Prints current probe parameters. Upon a probe command, these parameters are updated upon a
 // successful probe or upon a failed probe with the G38.3 without errors command (if supported).
 // These values are retained until grblHAL is power-cycled, whereby they will be re-zeroed.
-void report_probe_parameters (void)
+FLASHMEM void report_probe_parameters (void)
 {
     // Report in terms of machine position.
     float print_position[N_AXIS];
@@ -533,7 +533,7 @@ void report_probe_parameters (void)
 
 // Prints current home position in terms of machine position.
 // Bitmask for homed axes attached.
-void report_home_position (void)
+FLASHMEM void report_home_position (void)
 {
     hal.stream.write("[HOME:");
     hal.stream.write(get_axis_values(sys.home_position));
@@ -543,7 +543,7 @@ void report_home_position (void)
 }
 
 // Prints current tool offsets.
-void report_tool_offsets (void)
+FLASHMEM void report_tool_offsets (void)
 {
     hal.stream.write("[TLO:");
 #if TOOL_LENGTH_OFFSET_AXIS >= 0
@@ -557,7 +557,7 @@ void report_tool_offsets (void)
 #if NGC_PARAMETERS_ENABLE
 
 // Prints NIST/LinuxCNC NGC parameter value
-status_code_t report_ngc_parameter (ngc_param_id_t id)
+FLASHMEM status_code_t report_ngc_parameter (ngc_param_id_t id)
 {
     float value;
 
@@ -574,7 +574,7 @@ status_code_t report_ngc_parameter (ngc_param_id_t id)
 }
 
 // Prints named LinuxCNC NGC parameter value
-status_code_t report_named_ngc_parameter (char *arg)
+FLASHMEM status_code_t report_named_ngc_parameter (char *arg)
 {
     float value;
 
@@ -593,7 +593,7 @@ status_code_t report_named_ngc_parameter (char *arg)
 #endif
 
 // Prints grblHAL NGC parameters (coordinate offsets, probing, tool table)
-void report_ngc_parameters (void)
+FLASHMEM void report_ngc_parameters (void)
 {
     uint_fast8_t idx;
     coord_system_data_t offset;
@@ -706,7 +706,7 @@ static inline bool is_g92_active (void)
 }
 
 // Convert motion mode (G0, G1, ...) to string.
-static char *motionmode_to_str (char *buf, motion_mode_t mode)
+FLASHMEM static char *motionmode_to_str (char *buf, motion_mode_t mode)
 {
     strcpy(buf, "G");
 
@@ -723,7 +723,7 @@ static char *motionmode_to_str (char *buf, motion_mode_t mode)
 }
 
 // Print current gcode parser mode state
-void report_gcode_modes (stream_write_ptr stream_write)
+FLASHMEM void report_gcode_modes (stream_write_ptr stream_write)
 {
     stream_write("[GC:");
     stream_write(motionmode_to_str(buf, gc_state.modal.motion));
@@ -847,14 +847,14 @@ void report_gcode_modes (stream_write_ptr stream_write)
 }
 
 // Prints specified startup line
-void report_startup_line (uint8_t n, char *line)
+FLASHMEM void report_startup_line (uint8_t n, char *line)
 {
     hal.stream.write(appendbuf(3, "$N", uitoa((uint32_t)n), "="));
     hal.stream.write(line);
     hal.stream.write(ASCII_EOL);
 }
 
-void report_execute_startup_message (char *line, status_code_t status_code)
+FLASHMEM void report_execute_startup_message (char *line, status_code_t status_code)
 {
     hal.stream.write(">");
     hal.stream.write(line);
@@ -863,7 +863,7 @@ void report_execute_startup_message (char *line, status_code_t status_code)
 }
 
 // Prints build info line
-void report_build_info (char *line, bool extended)
+FLASHMEM void report_build_info (char *line, bool extended)
 {
     char buf[100];
 
@@ -1151,7 +1151,7 @@ void report_build_info (char *line, bool extended)
 
 // Prints the character string line grblHAL has received from the user, which has been pre-parsed,
 // and has been sent into protocol_execute_line() routine to be executed by grblHAL.
-void report_echo_line_received (char *line)
+FLASHMEM void report_echo_line_received (char *line)
 {
     hal.stream.write("[echo: ");
     hal.stream.write(line);
@@ -1165,7 +1165,7 @@ typedef struct {
     stream_write_ptr stream_write;
 } spindle_r_t;
 
-static bool report_spindle_num (spindle_info_t *spindle, void *data)
+FLASHMEM static bool report_spindle_num (spindle_info_t *spindle, void *data)
 {
     bool ok;
 
@@ -1630,7 +1630,7 @@ static void report_bitfield (const char *format, bool bitmap)
     }
 }
 
-static void write_quoted (const char *s, const char *sep)
+FLASHMEM static void write_quoted (const char *s, const char *sep)
 {
     hal.stream.write("\"");
     hal.stream.write(s); // TODO: escape double quoutes
@@ -1639,7 +1639,7 @@ static void write_quoted (const char *s, const char *sep)
         hal.stream.write(sep);
 }
 
-static void write_name (const char *s, uint_fast8_t offset)
+FLASHMEM static void write_name (const char *s, uint_fast8_t offset)
 {
     char *q = hal.stream.write_n ? strchr(s, '?') : NULL;
 
@@ -1652,7 +1652,7 @@ static void write_name (const char *s, uint_fast8_t offset)
         hal.stream.write(s);
 }
 
-static void report_settings_detail (settings_format_t format, const setting_detail_t *setting, uint_fast8_t offset)
+FLASHMEM static void report_settings_detail (settings_format_t format, const setting_detail_t *setting, uint_fast8_t offset)
 {
     uint_fast8_t suboffset = setting->flags.subgroups ? offset / setting->flags.increment : offset;
 
@@ -1922,7 +1922,7 @@ typedef struct {
     uint_fast16_t offset;
 } report_args_t;
 
-static bool print_sorted (const setting_detail_t *setting, uint_fast16_t offset, void *args)
+FLASHMEM static bool print_sorted (const setting_detail_t *setting, uint_fast16_t offset, void *args)
 {
     if(!(((report_args_t *)args)->group == setting->group && ((report_args_t *)args)->offset != offset))
         report_settings_detail (((report_args_t *)args)->format, setting, offset);
@@ -1930,7 +1930,7 @@ static bool print_sorted (const setting_detail_t *setting, uint_fast16_t offset,
     return true;
 }
 
-static bool print_unsorted (const setting_detail_t *setting, uint_fast16_t offset, void *args)
+FLASHMEM static bool print_unsorted (const setting_detail_t *setting, uint_fast16_t offset, void *args)
 {
     if(!(((report_args_t *)args)->group == setting->group && ((report_args_t *)args)->offset != offset) &&
        (setting->is_available == NULL ||setting->is_available(setting, 0)))
@@ -1939,7 +1939,7 @@ static bool print_unsorted (const setting_detail_t *setting, uint_fast16_t offse
     return true;
 }
 
-static status_code_t print_settings_details (settings_format_t format, setting_group_t group)
+FLASHMEM static status_code_t print_settings_details (settings_format_t format, setting_group_t group)
 {
     uint_fast16_t idx, n_settings = 0;
     bool reported = group == Group_All;
@@ -2001,7 +2001,7 @@ static status_code_t print_settings_details (settings_format_t format, setting_g
     return reported ? Status_OK : Status_SettingDisabled;
 }
 
-status_code_t report_settings_details (settings_format_t format, setting_id_t id, setting_group_t group)
+FLASHMEM status_code_t report_settings_details (settings_format_t format, setting_id_t id, setting_group_t group)
 {
     if(id != Setting_SettingsAll) {
         status_code_t status = Status_OK;
@@ -2019,7 +2019,7 @@ status_code_t report_settings_details (settings_format_t format, setting_id_t id
     return print_settings_details(format, group);
 }
 
-status_code_t report_setting_description (settings_format_t format, setting_id_t id)
+FLASHMEM status_code_t report_setting_description (settings_format_t format, setting_id_t id)
 {
     const setting_detail_t *setting;
 
@@ -2044,12 +2044,12 @@ status_code_t report_setting_description (settings_format_t format, setting_id_t
     return Status_OK;
 }
 
-static int cmp_alarms (const void *a, const void *b)
+FLASHMEM static int cmp_alarms (const void *a, const void *b)
 {
   return (*(alarm_detail_t **)(a))->id - (*(alarm_detail_t **)(b))->id;
 }
 
-static void print_alarm (const alarm_detail_t *alarm, bool grbl_format)
+FLASHMEM static void print_alarm (const alarm_detail_t *alarm, bool grbl_format)
 {
     if(grbl_format) {
         write_quoted(uitoa(alarm->id), ",");
@@ -2067,7 +2067,7 @@ static void print_alarm (const alarm_detail_t *alarm, bool grbl_format)
     }
 }
 
-status_code_t report_alarm_details (bool grbl_format)
+FLASHMEM status_code_t report_alarm_details (bool grbl_format)
 {
     uint_fast16_t idx, n_alarms = 0;
     alarm_details_t *details = grbl.on_get_alarms();
@@ -2104,12 +2104,12 @@ status_code_t report_alarm_details (bool grbl_format)
     return Status_OK;
 }
 
-static int cmp_errors (const void *a, const void *b)
+FLASHMEM static int cmp_errors (const void *a, const void *b)
 {
   return (*(status_detail_t **)(a))->id - (*(status_detail_t **)(b))->id;
 }
 
-static void print_error (const status_detail_t *error, bool grbl_format)
+FLASHMEM static void print_error (const status_detail_t *error, bool grbl_format)
 {
     if(grbl_format) {
         write_quoted(uitoa(error->id), ",");
@@ -2127,7 +2127,7 @@ static void print_error (const status_detail_t *error, bool grbl_format)
     }
 }
 
-status_code_t report_error_details (bool grbl_format)
+FLASHMEM status_code_t report_error_details (bool grbl_format)
 {
     uint_fast16_t idx, n_errors = 0;
     error_details_t *details = grbl.on_get_errors();
@@ -2164,7 +2164,7 @@ status_code_t report_error_details (bool grbl_format)
     return Status_OK;
 }
 
-static void print_setting_group (const setting_group_detail_t *group, char *prefix)
+FLASHMEM static void print_setting_group (const setting_group_detail_t *group, char *prefix)
 {
     if(settings_is_group_available(group->id)) {
         if(!prefix) {
@@ -2183,17 +2183,17 @@ static void print_setting_group (const setting_group_detail_t *group, char *pref
     }
 }
 
-static int cmp_setting_group_id (const void *a, const void *b)
+FLASHMEM static int cmp_setting_group_id (const void *a, const void *b)
 {
     return (*(setting_group_detail_t **)(a))->id - (*(setting_group_detail_t **)(b))->id;
 }
 
-static int cmp_setting_group_name (const void *a, const void *b)
+FLASHMEM static int cmp_setting_group_name (const void *a, const void *b)
 {
     return strcmp((*(setting_group_detail_t **)(a))->name, (*(setting_group_detail_t **)(b))->name);
 }
 
-static bool group_is_dup (setting_group_detail_t **groups, setting_group_t group)
+FLASHMEM static bool group_is_dup (setting_group_detail_t **groups, setting_group_t group)
 {
     while(*groups) {
         if((*groups)->id == group)
@@ -2204,7 +2204,7 @@ static bool group_is_dup (setting_group_detail_t **groups, setting_group_t group
     return false;
 }
 
-status_code_t report_setting_group_details (bool by_id, char *prefix)
+FLASHMEM status_code_t report_setting_group_details (bool by_id, char *prefix)
 {
     uint_fast16_t idx, n_groups = 0;
     setting_details_t *details = settings_get_details();
@@ -2244,7 +2244,7 @@ status_code_t report_setting_group_details (bool by_id, char *prefix)
     return Status_OK;
 }
 
-static char *add_limits (char *buf, limit_signals_t limits)
+FLASHMEM static char *add_limits (char *buf, limit_signals_t limits)
 {
     buf = axis_signals_tostring(buf, limits.min);
     *buf++ = ',';
@@ -2257,7 +2257,7 @@ static char *add_limits (char *buf, limit_signals_t limits)
     return buf;
 }
 
-status_code_t report_last_signals_event (sys_state_t state, char *args)
+FLASHMEM status_code_t report_last_signals_event (sys_state_t state, char *args)
 {
     char *append = &buf[12];
 
@@ -2273,7 +2273,7 @@ status_code_t report_last_signals_event (sys_state_t state, char *args)
     return Status_OK;
 }
 
-status_code_t report_current_limit_state (sys_state_t state, char *args)
+FLASHMEM status_code_t report_current_limit_state (sys_state_t state, char *args)
 {
     char *append = &buf[8];
 
@@ -2287,7 +2287,7 @@ status_code_t report_current_limit_state (sys_state_t state, char *args)
     return Status_OK;
 }
 
-status_code_t report_current_home_signal_state (sys_state_t state, char *args)
+FLASHMEM status_code_t report_current_home_signal_state (sys_state_t state, char *args)
 {
     char *append = &buf[7];
     home_signals_t home = hal.homing.get_state();
@@ -2307,7 +2307,7 @@ status_code_t report_current_home_signal_state (sys_state_t state, char *args)
 }
 
 // Prints spindle data (encoder pulse and index count, angular position).
-status_code_t report_spindle_data (sys_state_t state, char *args)
+FLASHMEM status_code_t report_spindle_data (sys_state_t state, char *args)
 {
     spindle_t *spindle = gc_spindle_get(-1);
 
@@ -2346,7 +2346,7 @@ typedef struct {
     pin_info_t *pins;
 } pin_data_t;
 
-static void report_pin_info (pin_info_t *pin)
+FLASHMEM static void report_pin_info (pin_info_t *pin)
 {
     hal.stream.write("[PIN:");
     if(*pin->port)
@@ -2361,7 +2361,7 @@ static void report_pin_info (pin_info_t *pin)
     hal.stream.write("]" ASCII_EOL);
 }
 
-static pin_info_t *get_pin_info (xbar_t *pin, pin_info_t *info)
+FLASHMEM static pin_info_t *get_pin_info (xbar_t *pin, pin_info_t *info)
 {
     info->function = pin->function;
     info->pin = pin->pin;
@@ -2375,29 +2375,29 @@ static pin_info_t *get_pin_info (xbar_t *pin, pin_info_t *info)
     return info;
 }
 
-static void count_pins (xbar_t *pin, void *data)
+FLASHMEM static void count_pins (xbar_t *pin, void *data)
 {
     ((pin_data_t *)data)->n_pins++;
 }
 
-static void get_pins (xbar_t *pin, void *data)
+FLASHMEM static void get_pins (xbar_t *pin, void *data)
 {
     get_pin_info(pin, &((pin_data_t *)data)->pins[((pin_data_t *)data)->idx++]);
 }
 
-static int cmp_pins (const void *a, const void *b)
+FLASHMEM static int cmp_pins (const void *a, const void *b)
 {
     return ((pin_info_t *)a)->sortkey - ((pin_info_t *)b)->sortkey;
 }
 
-static void report_pin (xbar_t *pin, void *data)
+FLASHMEM static void report_pin (xbar_t *pin, void *data)
 {
     pin_info_t info;
 
     report_pin_info(get_pin_info(pin, &info));
 }
 
-status_code_t report_pins (sys_state_t state, char *args)
+FLASHMEM status_code_t report_pins (sys_state_t state, char *args)
 {
     pin_data_t pin_data = {0};
 
@@ -2433,13 +2433,13 @@ typedef struct {
     pin_info_t data[2];
 } port_pins_t;
 
-static void get_uart_pins (xbar_t *pin, void *data)
+FLASHMEM static void get_uart_pins (xbar_t *pin, void *data)
 {
     if(pin->group == PinGroup_UART + ((port_pins_t *)data)->instance)
         get_pin_info(pin, &((port_pins_t *)data)->data[((port_pins_t *)data)->n_pins++]);
 }
 
-static bool report_port_info (const io_stream_properties_t *port, void *data)
+FLASHMEM static bool report_port_info (const io_stream_properties_t *port, void *data)
 {
     if(!stream_is_uart(port->type))
         return false;
@@ -2493,7 +2493,7 @@ static bool report_port_info (const io_stream_properties_t *port, void *data)
     return false;
 }
 
-bool get_ports (io_stream_properties_t const *port, void *data)
+FLASHMEM bool get_ports (io_stream_properties_t const *port, void *data)
 {
     if(stream_is_uart(port->type)) {
         ((port_data_t *)data)[((port_data_t *)data)->idx].port = port;
@@ -2503,7 +2503,7 @@ bool get_ports (io_stream_properties_t const *port, void *data)
     return false;
 }
 
-bool count_ports (io_stream_properties_t const *port, void *data)
+FLASHMEM bool count_ports (io_stream_properties_t const *port, void *data)
 {
     if(stream_is_uart(port->type))
         (*(uint32_t *)data)++;
@@ -2511,12 +2511,12 @@ bool count_ports (io_stream_properties_t const *port, void *data)
     return false;
 }
 
-static int cmp_ports (const void *a, const void *b)
+FLASHMEM static int cmp_ports (const void *a, const void *b)
 {
     return ((port_data_t *)a)->port->instance - ((port_data_t *)b)->port->instance;
 }
 
-status_code_t report_uart_ports (sys_state_t state, char *args)
+FLASHMEM status_code_t report_uart_ports (sys_state_t state, char *args)
 {
     uint32_t n_ports = 0;
     port_data_t *port_data;
@@ -2540,7 +2540,7 @@ status_code_t report_uart_ports (sys_state_t state, char *args)
     return Status_OK;
 }
 
-static char *irq_mode (pin_irq_mode_t mode)
+FLASHMEM static char *irq_mode (pin_irq_mode_t mode)
 {
     switch(mode) {
         case IRQ_Mode_Rising:
@@ -2566,7 +2566,7 @@ static char *irq_mode (pin_irq_mode_t mode)
     return "-";
 }
 
-static char *pull_mode (pull_mode_t mode)
+FLASHMEM static char *pull_mode (pull_mode_t mode)
 {
     switch(mode) {
         case PullMode_Up:
@@ -2582,7 +2582,7 @@ static char *pull_mode (pull_mode_t mode)
     return "-";
 }
 
-static bool print_aux_din (xbar_t *port, uint8_t pnum, void *data)
+FLASHMEM static bool print_aux_din (xbar_t *port, uint8_t pnum, void *data)
 {
     hal.stream.write("[PINSTATE:DIN|");
     hal.stream.write(port->description ? port->description : xbar_fn_to_pinname(port->function));
@@ -2605,7 +2605,7 @@ static bool print_aux_din (xbar_t *port, uint8_t pnum, void *data)
     return false;
 }
 
-static bool print_aux_dout (xbar_t *port, uint8_t pnum, void *data)
+FLASHMEM static bool print_aux_dout (xbar_t *port, uint8_t pnum, void *data)
 {
     hal.stream.write("[PINSTATE:DOUT|");
     hal.stream.write(port->description ? port->description : xbar_fn_to_pinname(port->function));
@@ -2626,7 +2626,7 @@ static bool print_aux_dout (xbar_t *port, uint8_t pnum, void *data)
     return false;
 }
 
-static bool print_aux_ain (xbar_t *port, uint8_t pnum, void *data)
+FLASHMEM static bool print_aux_ain (xbar_t *port, uint8_t pnum, void *data)
 {
     hal.stream.write("[PINSTATE:AIN|");
     hal.stream.write(port->description);
@@ -2641,7 +2641,7 @@ static bool print_aux_ain (xbar_t *port, uint8_t pnum, void *data)
     return false;
 }
 
-static bool print_aux_aout (xbar_t *port, uint8_t pnum, void *data)
+FLASHMEM static bool print_aux_aout (xbar_t *port, uint8_t pnum, void *data)
 {
     hal.stream.write("[PINSTATE:AOUT|");
     hal.stream.write(port->description);
@@ -2665,7 +2665,7 @@ static bool print_aux_aout (xbar_t *port, uint8_t pnum, void *data)
     return false;
 }
 
-status_code_t report_pin_states (sys_state_t state, char *args)
+FLASHMEM status_code_t report_pin_states (sys_state_t state, char *args)
 {
     ioports_enumerate(Port_Digital, Port_Input, (pin_cap_t){}, print_aux_din, NULL);
     ioports_enumerate(Port_Digital, Port_Output, (pin_cap_t){}, print_aux_dout, NULL);
@@ -2675,7 +2675,7 @@ status_code_t report_pin_states (sys_state_t state, char *args)
     return Status_OK;
 }
 
-static void print_uito2a (char *prefix, uint32_t v)
+FLASHMEM static void print_uito2a (char *prefix, uint32_t v)
 {
     hal.stream.write(prefix);
     if(v < 10)
@@ -2683,7 +2683,7 @@ static void print_uito2a (char *prefix, uint32_t v)
     hal.stream.write(uitoa(v));
 }
 
-status_code_t report_time (void)
+FLASHMEM status_code_t report_time (void)
 {
     bool ok = false;
 
@@ -2704,7 +2704,7 @@ status_code_t report_time (void)
     return ok ? Status_OK : Status_InvalidStatement;
 }
 
-static bool report_spindle (spindle_info_t *spindle, void *data)
+FLASHMEM static bool report_spindle (spindle_info_t *spindle, void *data)
 {
     if(data) {
         char *caps = buf;
@@ -2778,14 +2778,14 @@ typedef struct {
     spindle_info_t *spindles;
 } spindle_rdata_t;
 
-static bool get_spindles (spindle_info_t *spindle, void *data)
+FLASHMEM static bool get_spindles (spindle_info_t *spindle, void *data)
 {
     memcpy(&((spindle_rdata_t *)data)->spindles[((spindle_rdata_t *)data)->idx++], spindle, sizeof(spindle_info_t));
 
     return false;
 }
 
-static int cmp_spindles (const void *a, const void *b)
+FLASHMEM static int cmp_spindles (const void *a, const void *b)
 {
     uint32_t key_a = ((spindle_info_t *)a)->num == -1 ? ((((spindle_info_t *)a)->hal->type + 1) << 8) | ((spindle_info_t *)a)->id : ((spindle_info_t *)a)->num,
              key_b = ((spindle_info_t *)b)->num == -1 ? ((((spindle_info_t *)b)->hal->type + 1) << 8) | ((spindle_info_t *)b)->id : ((spindle_info_t *)b)->num;
@@ -2795,7 +2795,7 @@ static int cmp_spindles (const void *a, const void *b)
 
 #endif
 
-status_code_t report_spindles (bool machine_readable)
+FLASHMEM status_code_t report_spindles (bool machine_readable)
 {
 #if N_SPINDLE > 1
 
@@ -2825,7 +2825,7 @@ status_code_t report_spindles (bool machine_readable)
     return Status_OK;
 }
 
-status_code_t report_stepper_status (sys_state_t state, char *args)
+FLASHMEM status_code_t report_stepper_status (sys_state_t state, char *args)
 {
     if(hal.stepper.status) {
 
@@ -2858,7 +2858,7 @@ status_code_t report_stepper_status (sys_state_t state, char *args)
     return hal.stepper.status ? Status_OK : Status_InvalidStatement;
 }
 
-void report_pid_log (void)
+FLASHMEM void report_pid_log (void)
 {
 #ifdef PID_LOG
     uint_fast16_t idx = 0;
@@ -2885,7 +2885,7 @@ void report_pid_log (void)
 #endif
 }
 
-static const report_t report_fns = {
+PROGMEM static const report_t report_fns = {
     .init_message = report_init_message,
     .help_message = report_help_message,
     .status_message = report_status_message,
@@ -2894,7 +2894,7 @@ static const report_t report_fns = {
     .setting = report_setting
 };
 
-void report_init_fns (void)
+FLASHMEM void report_init_fns (void)
 {
     memcpy(&grbl.report, &report_fns, sizeof(report_t));
 
@@ -2902,7 +2902,7 @@ void report_init_fns (void)
         grbl.on_report_handlers_init();
 }
 
-report_tracking_flags_t report_get_rt_flags_all (void)
+FLASHMEM report_tracking_flags_t report_get_rt_flags_all (void)
 {
     report_tracking_flags_t report;
 
diff --git a/settings.c b/settings.c
index 14aadce..d80e4d7 100644
--- a/settings.c
+++ b/settings.c
@@ -1723,7 +1723,7 @@ inline static uint8_t get_decimal_places (const char *format)
     return dp ? strchr(format, '\0') - dp - 1 : 1;
 }
 
-char *setting_get_value (const setting_detail_t *setting, uint_fast16_t offset)
+FLASHMEM char *setting_get_value (const setting_detail_t *setting, uint_fast16_t offset)
 {
     char *value = NULL;
 
@@ -1805,7 +1805,7 @@ char *setting_get_value (const setting_detail_t *setting, uint_fast16_t offset)
     return value;
 }
 
-uint32_t setting_get_int_value (const setting_detail_t *setting, uint_fast16_t offset)
+FLASHMEM uint32_t setting_get_int_value (const setting_detail_t *setting, uint_fast16_t offset)
 {
     uint32_t value = 0;
 
@@ -1861,7 +1861,7 @@ uint32_t setting_get_int_value (const setting_detail_t *setting, uint_fast16_t o
     return value;
 }
 
-float setting_get_float_value (const setting_detail_t *setting, uint_fast16_t offset)
+FLASHMEM float setting_get_float_value (const setting_detail_t *setting, uint_fast16_t offset)
 {
     float value = NAN;
 
@@ -1888,7 +1888,7 @@ float setting_get_float_value (const setting_detail_t *setting, uint_fast16_t of
     return value;
 }
 
-static bool is_group_available (const setting_detail_t *setting, uint_fast16_t offset)
+FLASHMEM static bool is_group_available (const setting_detail_t *setting, uint_fast16_t offset)
 {
     return settings_is_group_available(setting->group);
 }
@@ -2452,13 +2452,13 @@ static setting_details_t setting_details = {
 
 static setting_details_t *settingsd = &setting_details;
 
-void settings_register (setting_details_t *details)
+FLASHMEM void settings_register (setting_details_t *details)
 {
     settingsd->next = details;
     settingsd = details;
 }
 
-setting_details_t *settings_get_details (void)
+FLASHMEM setting_details_t *settings_get_details (void)
 {
     return &setting_details;
 }
@@ -2466,14 +2466,14 @@ setting_details_t *settings_get_details (void)
 /**/
 
 // Write build info to persistent storage
-void settings_write_build_info (char *line)
+FLASHMEM void settings_write_build_info (char *line)
 {
     if(hal.nvs.type != NVS_None)
         hal.nvs.memcpy_to_nvs(NVS_ADDR_BUILD_INFO, (uint8_t *)line, sizeof(stored_line_t), true);
 }
 
 // Read build info from persistent storage.
-bool settings_read_build_info(char *line)
+FLASHMEM bool settings_read_build_info(char *line)
 {
     if (!(hal.nvs.type != NVS_None && hal.nvs.memcpy_from_nvs((uint8_t *)line, NVS_ADDR_BUILD_INFO, sizeof(stored_line_t), true) == NVS_TransferResult_OK)) {
         settings_restore((settings_restore_t){ .build_info = On });
@@ -2483,7 +2483,7 @@ bool settings_read_build_info(char *line)
 }
 
 // Write startup line to persistent storage
-void settings_write_startup_line (uint8_t idx, char *line)
+FLASHMEM void settings_write_startup_line (uint8_t idx, char *line)
 {
     assert(idx < N_STARTUP_LINE);
 
@@ -2496,7 +2496,7 @@ void settings_write_startup_line (uint8_t idx, char *line)
 }
 
 // Read startup line to persistent storage.
-bool settings_read_startup_line (uint8_t idx, char *line)
+FLASHMEM bool settings_read_startup_line (uint8_t idx, char *line)
 {
     assert(idx < N_STARTUP_LINE);
 
@@ -2510,7 +2510,7 @@ bool settings_read_startup_line (uint8_t idx, char *line)
 }
 
 // Write selected coordinate data to persistent storage.
-void settings_write_coord_data (coord_system_id_t id, coord_system_data_t *data)
+FLASHMEM void settings_write_coord_data (coord_system_id_t id, coord_system_data_t *data)
 {
     assert(id <= N_CoordinateSystems);
 
@@ -2526,7 +2526,7 @@ void settings_write_coord_data (coord_system_id_t id, coord_system_data_t *data)
 }
 
 // Read selected coordinate data from persistent storage.
-bool settings_read_coord_data (coord_system_id_t id, coord_system_data_t *data)
+FLASHMEM bool settings_read_coord_data (coord_system_id_t id, coord_system_data_t *data)
 {
     assert(id <= N_CoordinateSystems);
 
@@ -2545,7 +2545,7 @@ bool settings_read_coord_data (coord_system_id_t id, coord_system_data_t *data)
 static tool_data_t tool_data[N_TOOLS + 1];
 
 // Write selected tool data to persistent storage.
-static bool settings_set_tool_data (tool_data_t *tool)
+FLASHMEM static bool settings_set_tool_data (tool_data_t *tool)
 {
     bool ok = tool->tool_id <= N_TOOLS;
 
@@ -2556,7 +2556,7 @@ static bool settings_set_tool_data (tool_data_t *tool)
 }
 
 // Read selected tool data from persistent storage.
-static tool_table_entry_t *settings_get_tool_data (tool_id_t tool_id)
+FLASHMEM static tool_table_entry_t *settings_get_tool_data (tool_id_t tool_id)
 {
     static tool_table_entry_t tool = {0};
 
@@ -2575,7 +2575,7 @@ static tool_table_entry_t *settings_get_tool_data (tool_id_t tool_id)
 }
 
 // Clear all tool data in persistent storage.
-static bool settings_clear_tool_data (void)
+FLASHMEM static bool settings_clear_tool_data (void)
 {
     uint_fast8_t idx;
 
@@ -2594,13 +2594,13 @@ static bool settings_clear_tool_data (void)
 static tool_data_t tool_data = {0};
 
 // Write selected tool data to persistent storage.
-static bool settings_set_tool_data (tool_data_t *tool_data)
+FLASHMEM static bool settings_set_tool_data (tool_data_t *tool_data)
 {
     return true;
 }
 
 // Read selected tool data from persistent storage.
-static tool_table_entry_t *settings_get_tool_data (tool_id_t tool_id)
+FLASHMEM static tool_table_entry_t *settings_get_tool_data (tool_id_t tool_id)
 {
     static tool_table_entry_t tool = {0};
 
@@ -2617,7 +2617,7 @@ static tool_table_entry_t *settings_get_tool_data (tool_id_t tool_id)
 }
 
 // Clear all tool data in persistent storage.
-static bool settings_clear_tool_data (void)
+FLASHMEM static bool settings_clear_tool_data (void)
 {
     memset(&tool_data, 0, sizeof(tool_data_t));
 
@@ -2627,7 +2627,7 @@ static bool settings_clear_tool_data (void)
 #endif // N_TOOLS
 
 // Sanity check of settings, board map could have been changed...
-static void sanity_check (void)
+FLASHMEM static void sanity_check (void)
 {
 #if LATHE_UVW_OPTION
     settings.mode = Mode_Lathe;
@@ -2691,7 +2691,7 @@ static void sanity_check (void)
 
 // Read global settings from persistent storage.
 // Checks version-byte of non-volatile storage and global settings copy.
-bool read_global_settings (void)
+FLASHMEM bool read_global_settings (void)
 {
     bool ok = hal.nvs.type != NVS_None && SETTINGS_VERSION == hal.nvs.get_byte(0) && hal.nvs.memcpy_from_nvs((uint8_t *)&settings, NVS_ADDR_GLOBAL, sizeof(settings_t), true) == NVS_TransferResult_OK;
 
@@ -2701,7 +2701,7 @@ bool read_global_settings (void)
 }
 
 // Write global settings to persistent storage
-void settings_write_global (void)
+FLASHMEM void settings_write_global (void)
 {
     if(override_backup.valid)
         restore_override_backup();
@@ -2714,7 +2714,7 @@ void settings_write_global (void)
 
 #if N_SPINDLE > 1
 
-static bool get_default_spindle (spindle_info_t *spindle, void *data)
+FLASHMEM static bool get_default_spindle (spindle_info_t *spindle, void *data)
 {
     bool ok;
 
@@ -2727,7 +2727,7 @@ static bool get_default_spindle (spindle_info_t *spindle, void *data)
 #endif
 
 // Restore global settings to defaults and write to persistent storage
-void settings_restore (settings_restore_t restore)
+FLASHMEM void settings_restore (settings_restore_t restore)
 {
     uint_fast8_t idx;
     stored_line_t empty_line;
@@ -2802,7 +2802,7 @@ inline static bool is_available (const setting_detail_t *setting, uint_fast16_t
     return setting->is_available == NULL || setting->is_available(setting, offset);
 }
 
-bool settings_is_group_available (setting_group_t id)
+FLASHMEM bool settings_is_group_available (setting_group_t id)
 {
     const setting_group_detail_t *group = setting_get_group_details(id);
 
@@ -2857,12 +2857,12 @@ bool settings_is_group_available (setting_group_t id)
     return available;
 }
 
-setting_group_t settings_normalize_group (setting_group_t group)
+FLASHMEM setting_group_t settings_normalize_group (setting_group_t group)
 {
     return (group > Group_Axis0 && group < Group_Axis0 + N_AXIS) ? Group_Axis0 : group;
 }
 
-bool settings_iterator (const setting_detail_t *setting, setting_output_ptr callback, void *data)
+FLASHMEM bool settings_iterator (const setting_detail_t *setting, setting_output_ptr callback, void *data)
 {
     bool ok = false;
 
@@ -2920,7 +2920,7 @@ static inline const setting_detail_t *_setting_get_details (setting_id_t id, uin
     return NULL;
 }
 
-const setting_detail_t *setting_get_details (setting_id_t id, setting_details_t **set)
+FLASHMEM const setting_detail_t *setting_get_details (setting_id_t id, setting_details_t **set)
 {
     const setting_detail_t *detail;
 
@@ -2951,7 +2951,7 @@ const setting_detail_t *setting_get_details (setting_id_t id, setting_details_t
     return detail;
 }
 
-const char *setting_get_description (setting_id_t id)
+FLASHMEM const char *setting_get_description (setting_id_t id)
 {
     const char *description = NULL;
 
@@ -2980,7 +2980,7 @@ const char *setting_get_description (setting_id_t id)
     return description;
 }
 
-const setting_group_detail_t *setting_get_group_details (setting_group_t id)
+FLASHMEM const setting_group_detail_t *setting_get_group_details (setting_group_t id)
 {
     uint_fast16_t idx;
     setting_details_t *details = settings_get_details();
@@ -3005,7 +3005,7 @@ setting_group_t setting_get_parent_group (setting_group_t id)
 }
 */
 
-static status_code_t validate_value (const setting_detail_t *setting, float value)
+FLASHMEM static status_code_t validate_value (const setting_detail_t *setting, float value)
 {
     float val;
     uint_fast8_t set_idx = 0;
@@ -3033,7 +3033,7 @@ static status_code_t validate_value (const setting_detail_t *setting, float valu
     return Status_OK;
 }
 
-static status_code_t validate_uint_value (const setting_detail_t *setting, uint32_t value)
+FLASHMEM static status_code_t validate_uint_value (const setting_detail_t *setting, uint32_t value)
 {
     uint32_t val;
     uint_fast8_t set_idx = 0;
@@ -3062,7 +3062,7 @@ static status_code_t validate_uint_value (const setting_detail_t *setting, uint3
     return Status_OK;
 }
 
-static uint32_t strnumentries (const char *s, const char delimiter)
+FLASHMEM static uint32_t strnumentries (const char *s, const char delimiter)
 {
     if(s == NULL || *s == '\0')
         return 0;
@@ -3078,7 +3078,7 @@ static uint32_t strnumentries (const char *s, const char delimiter)
     return entries;
 }
 
-setting_datatype_t setting_datatype_to_external (setting_datatype_t datatype)
+FLASHMEM setting_datatype_t setting_datatype_to_external (setting_datatype_t datatype)
 {
     switch(datatype) {
 
@@ -3094,17 +3094,17 @@ setting_datatype_t setting_datatype_to_external (setting_datatype_t datatype)
     return datatype;
 }
 
-bool setting_is_list (const setting_detail_t *setting)
+FLASHMEM bool setting_is_list (const setting_detail_t *setting)
 {
     return setting->datatype == Format_Bitfield || setting->datatype == Format_XBitfield || setting->datatype == Format_RadioButtons;
 }
 
-bool setting_is_integer (const setting_detail_t *setting)
+FLASHMEM bool setting_is_integer (const setting_detail_t *setting)
 {
     return setting->datatype == Format_Integer || setting->datatype == Format_Int8 || setting->datatype == Format_Int16;
 }
 
-static char *remove_element (char *s, uint_fast8_t entry)
+FLASHMEM static char *remove_element (char *s, uint_fast8_t entry)
 {
     while(entry && *s) {
         if(*s == ',')
@@ -3127,7 +3127,7 @@ static char *remove_element (char *s, uint_fast8_t entry)
     return s;
 }
 
-static void setting_remove_element (setting_id_t id, uint_fast8_t pos)
+FLASHMEM static void setting_remove_element (setting_id_t id, uint_fast8_t pos)
 {
     const setting_detail_t *setting = setting_get_details(id, NULL);
 
@@ -3137,7 +3137,7 @@ static void setting_remove_element (setting_id_t id, uint_fast8_t pos)
 
 // Flag setting elements for bitfields as N/A according to a mask
 // Note: setting format string has to reside in RAM.
-void setting_remove_elements (setting_id_t id, uint32_t mask, bool trim)
+FLASHMEM void setting_remove_elements (setting_id_t id, uint32_t mask, bool trim)
 {
     const setting_detail_t *setting;
 
@@ -3171,7 +3171,7 @@ inline static bool setting_is_core (setting_type_t type)
     return !(type == Setting_NonCore || type == Setting_NonCoreFn);
 }
 
-static status_code_t setting_validate_me_uint (const setting_detail_t *setting, char *svalue)
+FLASHMEM static status_code_t setting_validate_me_uint (const setting_detail_t *setting, char *svalue)
 {
     uint_fast8_t idx = 0;
     uint32_t value;
@@ -3216,7 +3216,7 @@ static status_code_t setting_validate_me_uint (const setting_detail_t *setting,
     return status;
 }
 
-status_code_t setting_validate_me (const setting_detail_t *setting, float value, char *svalue)
+FLASHMEM status_code_t setting_validate_me (const setting_detail_t *setting, float value, char *svalue)
 {
     status_code_t status = Status_OK;
 
@@ -3262,7 +3262,7 @@ status_code_t setting_validate_me (const setting_detail_t *setting, float value,
     return status;
 }
 
-status_code_t setting_validate (setting_id_t id, float value, char *svalue)
+FLASHMEM status_code_t setting_validate (setting_id_t id, float value, char *svalue)
 {
     const setting_detail_t *setting = setting_get_details(id, NULL);
 
@@ -3270,7 +3270,7 @@ status_code_t setting_validate (setting_id_t id, float value, char *svalue)
     return setting == NULL ? Status_OK : setting_validate_me(setting, value, svalue);
 }
 
-static bool settings_changed_spindle (void)
+FLASHMEM static bool settings_changed_spindle (void)
 {
     static spindle_settings_t spindle_settings = {};
     static spindle_pwm_settings_t spindle_pwm_settings = {};
@@ -3411,7 +3411,7 @@ FLASHMEM status_code_t settings_store_setting (setting_id_t id, char *svalue)
     return status;
 }
 
-bool settings_add_spindle_type (const char *type)
+FLASHMEM bool settings_add_spindle_type (const char *type)
 {
     bool ok;
 
@@ -3424,7 +3424,7 @@ bool settings_add_spindle_type (const char *type)
     return ok;
 }
 
-void onFileDemarcate (bool start)
+FLASHMEM void onFileDemarcate (bool start)
 {
     if(!start) {
 
@@ -3442,14 +3442,14 @@ void onFileDemarcate (bool start)
 }
 
 // Clear settings chain
-void settings_clear (void)
+FLASHMEM void settings_clear (void)
 {
     setting_details.next = NULL;
     settingsd = &setting_details;
 }
 
 // Initialize the config subsystem
-void settings_init (void)
+FLASHMEM void settings_init (void)
 {
     settings_changed_flags_t changed = {0};
 
diff --git a/spindle_control.c b/spindle_control.c
index 5b73fec..e5f3f7a 100644
--- a/spindle_control.c
+++ b/spindle_control.c
@@ -56,7 +56,7 @@ static spindle_sys_t sys_spindle[N_SYS_SPINDLE] = {0};
 static spindle_reg_t spindles[N_SPINDLE] = {0}, *pwm_spindle = NULL;
 static const spindle_data_ptrs_t *encoder;
 
-static void spindle_init (void *data)
+FLASHMEM static void spindle_init (void *data)
 {
     spindle_reg_t *spindle = (spindle_reg_t *)data;
 
@@ -69,7 +69,7 @@ static void spindle_init (void *data)
 \param spindle_num spindle number to set as enabled as a \ref spindle_num_t.
 \returns \a true if succsesful, \a false if not.
 */
-static bool spindle_activate (spindle_id_t spindle_id, spindle_num_t spindle_num)
+FLASHMEM static bool spindle_activate (spindle_id_t spindle_id, spindle_num_t spindle_num)
 {
     bool ok;
     spindle_reg_t *spindle;
@@ -143,7 +143,7 @@ static bool spindle_activate (spindle_id_t spindle_id, spindle_num_t spindle_num
 __NOTE:__ The first spindle registered will become the default active spindle.
 __NOTE:__ up to \ref N_SPINDLE spindles can be registered at a time.
 */
-spindle_id_t spindle_register (const spindle_ptrs_t *spindle, const char *name)
+FLASHMEM spindle_id_t spindle_register (const spindle_ptrs_t *spindle, const char *name)
 {
     if(n_spindle == 1 && spindles[0].cfg->type == SpindleType_Null)
         n_spindle = 0;
@@ -178,7 +178,7 @@ spindle_id_t spindle_register (const spindle_ptrs_t *spindle, const char *name)
 
 __NOTE:__ up to \ref N_SYS_SPINDLE spindles can be enabled at a time.
 */
-spindle_num_t spindle_enable (spindle_id_t spindle_id)
+FLASHMEM spindle_num_t spindle_enable (spindle_id_t spindle_id)
 {
     uint_fast8_t idx = 0;
     spindle_num_t spindle_num = -1;
@@ -195,7 +195,7 @@ spindle_num_t spindle_enable (spindle_id_t spindle_id)
 \param spindle_id spindle id as a \ref spindle_id_t.
 \returns \a true if succsesful, \a false if not.
 */
-bool spindle_select (spindle_id_t spindle_id)
+FLASHMEM bool spindle_select (spindle_id_t spindle_id)
 {
     if(n_spindle == 0 && spindle_id >= 0) {
         spindle_id = 0;
@@ -216,7 +216,7 @@ bool spindle_select (spindle_id_t spindle_id)
 
 __NOTE:__ do not modify the returned structure!
 */
-spindle_ptrs_t *spindle_get_hal (spindle_id_t spindle_id, spindle_hal_t hal)
+FLASHMEM spindle_ptrs_t *spindle_get_hal (spindle_id_t spindle_id, spindle_hal_t hal)
 {
     spindle_ptrs_t *spindle = NULL;
 
@@ -239,7 +239,7 @@ spindle_ptrs_t *spindle_get_hal (spindle_id_t spindle_id, spindle_hal_t hal)
 /*! \brief Get the spindle id of the default spindle (spindle number 0).
 \returns spindle id as a \ref spindle_id_t if successful, \a -2 if not (no spindle available).
 */
-spindle_id_t spindle_get_default (void)
+FLASHMEM spindle_id_t spindle_get_default (void)
 {
     return sys_spindle[0].enabled ? sys_spindle[0].hal.id : -2;
 }
@@ -248,7 +248,7 @@ spindle_id_t spindle_get_default (void)
 \param active true to return active capabilities, false to return default capabilities.
 \returns capabilities in a \ref spindle_cap_t structure.
 */
-spindle_cap_t spindle_get_caps (bool active)
+FLASHMEM spindle_cap_t spindle_get_caps (bool active)
 {
     spindle_cap_t caps = {0};
     uint_fast8_t idx = n_spindle;
@@ -265,7 +265,7 @@ spindle_cap_t spindle_get_caps (bool active)
 \param spindle_id spindle id as a \ref spindle_id_t.
 \returns pointer to a null terminated string if succesful, \a NULL if not.
 */
-const char *spindle_get_name (spindle_id_t spindle_id)
+FLASHMEM const char *spindle_get_name (spindle_id_t spindle_id)
 {
     return spindle_id >= 0 && spindle_id < n_spindle && spindles[spindle_id].cfg ? spindles[spindle_id].name : NULL;
 }
@@ -275,7 +275,7 @@ May be used by the driver on spindle initialization or when spindle settings has
 \param spindle pointer to a \ref spindle_ptrs_t structure.
 \param pwm_caps pointer to a \ref spindle_pwm_t structure.
 */
-void spindle_update_caps (spindle_ptrs_t *spindle, spindle_pwm_t *pwm_caps)
+FLASHMEM void spindle_update_caps (spindle_ptrs_t *spindle, spindle_pwm_t *pwm_caps)
 {
     uint_fast8_t idx = N_SYS_SPINDLE;
 
@@ -304,7 +304,7 @@ void spindle_update_caps (spindle_ptrs_t *spindle, spindle_pwm_t *pwm_caps)
 /*! \brief Get number of registered spindles.
 \returns number of registered spindles.
 */
-uint8_t spindle_get_count (void)
+FLASHMEM uint8_t spindle_get_count (void)
 {
     if(n_spindle == 0)
         spindle_select(0);
@@ -312,7 +312,7 @@ uint8_t spindle_get_count (void)
     return n_spindle == 1 && spindles[0].cfg->type == SpindleType_Null ? 0 : n_spindle;
 }
 
-bool spindle_get_id (uint8_t ref_id, spindle_id_t *spindle_id)
+FLASHMEM bool spindle_get_id (uint8_t ref_id, spindle_id_t *spindle_id)
 {
     bool ok = false;
     uint_fast8_t idx;
@@ -329,7 +329,7 @@ bool spindle_get_id (uint8_t ref_id, spindle_id_t *spindle_id)
     return ok;
 }
 
-static spindle_num_t spindle_get_num (spindle_id_t spindle_id)
+FLASHMEM static spindle_num_t spindle_get_num (spindle_id_t spindle_id)
 {
     spindle_num_t spindle_num;
 
@@ -350,7 +350,7 @@ static spindle_num_t spindle_get_num (spindle_id_t spindle_id)
     return spindle_num;
 }
 
-void spindle_bind_encoder (const spindle_data_ptrs_t *encoder_data)
+FLASHMEM void spindle_bind_encoder (const spindle_data_ptrs_t *encoder_data)
 {
     uint_fast8_t idx;
     spindle_ptrs_t *spindle;
@@ -379,7 +379,7 @@ void spindle_bind_encoder (const spindle_data_ptrs_t *encoder_data)
     }
 }
 
-bool spindle_set_at_speed_range (spindle_ptrs_t *spindle, spindle_data_t *spindle_data, float rpm)
+FLASHMEM bool spindle_set_at_speed_range (spindle_ptrs_t *spindle, spindle_data_t *spindle_data, float rpm)
 {
     spindle_data->rpm_programmed = rpm;
     spindle_data->state_programmed.at_speed = false;
@@ -397,7 +397,7 @@ bool spindle_set_at_speed_range (spindle_ptrs_t *spindle, spindle_data_t *spindl
 \param data pointer to optional data to pass to the callback function.
 \returns \a true if spindles are registered and a callback function was provided, \a false otherwise.
 */
-bool spindle_enumerate_spindles (spindle_enumerate_callback_ptr callback, void *data)
+FLASHMEM bool spindle_enumerate_spindles (spindle_enumerate_callback_ptr callback, void *data)
 {
     if(callback == NULL || n_spindle == 0)
         return false;
@@ -433,7 +433,7 @@ bool spindle_enumerate_spindles (spindle_enumerate_callback_ptr callback, void *
 \param spindle_num spindle number as a \ref spindle_num_t.
 \returns \a true if the spindle is enabled, \a false otherwise.
 */
-bool spindle_is_enabled (spindle_num_t spindle_num)
+FLASHMEM bool spindle_is_enabled (spindle_num_t spindle_num)
 {
     if(spindle_num == -1)
         spindle_num = 0;
@@ -447,7 +447,7 @@ bool spindle_is_enabled (spindle_num_t spindle_num)
 
 __NOTE:__ do not modify the returned structure!
 */
-spindle_ptrs_t *spindle_get (spindle_num_t spindle_num)
+FLASHMEM spindle_ptrs_t *spindle_get (spindle_num_t spindle_num)
 {
     return spindle_num >= 0 && spindle_num < N_SYS_SPINDLE && sys_spindle[spindle_num].enabled ? &sys_spindle[spindle_num].hal : NULL;
 }
@@ -458,14 +458,14 @@ spindle_ptrs_t *spindle_get (spindle_num_t spindle_num)
 // Null (dummy) spindle, automatically installed if no spindles are registered.
 //
 
-static void null_set_state (spindle_ptrs_t *spindle, spindle_state_t state, float rpm)
+FLASHMEM static void null_set_state (spindle_ptrs_t *spindle, spindle_state_t state, float rpm)
 {
     UNUSED(spindle);
     UNUSED(state);
     UNUSED(rpm);
 }
 
-static spindle_state_t null_get_state (spindle_ptrs_t *spindle)
+FLASHMEM static spindle_state_t null_get_state (spindle_ptrs_t *spindle)
 {
     UNUSED(spindle);
 
@@ -473,13 +473,13 @@ static spindle_state_t null_get_state (spindle_ptrs_t *spindle)
 }
 
 // Sets spindle speed
-static void null_update_pwm (spindle_ptrs_t *spindle, uint_fast16_t pwm_value)
+FLASHMEM static void null_update_pwm (spindle_ptrs_t *spindle, uint_fast16_t pwm_value)
 {
     UNUSED(spindle);
     UNUSED(pwm_value);
 }
 
-static uint_fast16_t null_get_pwm (spindle_ptrs_t *spindle, float rpm)
+FLASHMEM static uint_fast16_t null_get_pwm (spindle_ptrs_t *spindle, float rpm)
 {
     UNUSED(spindle);
     UNUSED(rpm);
@@ -487,7 +487,7 @@ static uint_fast16_t null_get_pwm (spindle_ptrs_t *spindle, float rpm)
     return 0;
 }
 
-static void null_update_rpm (spindle_ptrs_t *spindle, float rpm)
+FLASHMEM static void null_update_rpm (spindle_ptrs_t *spindle, float rpm)
 {
     UNUSED(spindle);
     UNUSED(rpm);
@@ -504,9 +504,9 @@ static void null_esp32_off (spindle_ptrs_t *spindle)
 This is done automatically on startup if no spindle can be succesfully enabled.
 \returns assigned spindle id as a \ref spindle_id_t if successful, \a -1 if not.
 */
-spindle_id_t spindle_add_null (void)
+FLASHMEM spindle_id_t spindle_add_null (void)
 {
-    static const spindle_ptrs_t spindle = {
+    PROGMEM static const spindle_ptrs_t spindle = {
         .type = SpindleType_Null,
         .cap.variable = Off,
         .cap.at_speed = Off,
@@ -546,7 +546,7 @@ typedef struct {
     float rpm_target;
 } rpm_override_t;
 
-static void spindle_ramp_task (void *data)
+FLASHMEM static void spindle_ramp_task (void *data)
 {
     bool ok;
     rpm_override_t *ramp = (rpm_override_t *)data;
@@ -569,7 +569,7 @@ static void spindle_ramp_task (void *data)
     ramp->spindle->update_pwm(ramp->spindle, ramp->spindle->get_pwm(ramp->spindle, ramp->rpm));
 }
 
-static uint16_t spindle_get_ramp (spindle_ptrs_t *spindle, float rpm, float target_rpm, uint16_t delay_ms, rpm_override_t *ramp)
+FLASHMEM static uint16_t spindle_get_ramp (spindle_ptrs_t *spindle, float rpm, float target_rpm, uint16_t delay_ms, rpm_override_t *ramp)
 {
     ramp->spindle = spindle;
     ramp->rpm = rpm;
@@ -587,7 +587,7 @@ static uint16_t spindle_get_ramp (spindle_ptrs_t *spindle, float rpm, float targ
     return dly;
 }
 
-static bool spindle_ramp_override (spindle_ptrs_t *spindle, float rpm, float target_rpm)
+FLASHMEM static bool spindle_ramp_override (spindle_ptrs_t *spindle, float rpm, float target_rpm)
 {
     static rpm_override_t ramp;
 
@@ -607,7 +607,7 @@ static bool spindle_ramp_override (spindle_ptrs_t *spindle, float rpm, float tar
 \returns overridden RPM
 __NOTE:__ Unlike motion overrides, spindle overrides do not require a planner reinitialization.
 */
-float spindle_set_override (spindle_ptrs_t *spindle, override_t speed_override)
+FLASHMEM float spindle_set_override (spindle_ptrs_t *spindle, override_t speed_override)
 {
     if(speed_override == DEFAULT_SPINDLE_RPM_OVERRIDE || !spindle->param->state.override_disable) {
 
@@ -645,7 +645,7 @@ float spindle_set_override (spindle_ptrs_t *spindle, override_t speed_override)
     return spindle->param->rpm_overridden;
 }
 
-bool spindle_override_disable (spindle_ptrs_t *spindle, bool disable)
+FLASHMEM bool spindle_override_disable (spindle_ptrs_t *spindle, bool disable)
 {
     if(disable && !spindle->param->state.override_disable)
         spindle_set_override(spindle, DEFAULT_SPINDLE_RPM_OVERRIDE);
@@ -660,7 +660,7 @@ bool spindle_override_disable (spindle_ptrs_t *spindle, bool disable)
 \param state a \ref spindle_state_t structure.
 \returns \a true if on and ccw fields are equal, \a false otherwise.
 */
-bool spindle_check_state (spindle_ptrs_t *spindle, spindle_state_t state)
+FLASHMEM bool spindle_check_state (spindle_ptrs_t *spindle, spindle_state_t state)
 {
     static const spindle_state_t mask = {
        .on = On,
@@ -670,7 +670,7 @@ bool spindle_check_state (spindle_ptrs_t *spindle, spindle_state_t state)
     return (state.value & mask.value) == (spindle->get_state(spindle).value & mask.value);
 }
 
-static void spindle_ramp (spindle_ptrs_t *spindle, spindle_state_t state, float rpm, float target_rpm, uint16_t delay_ms)
+FLASHMEM static void spindle_ramp (spindle_ptrs_t *spindle, spindle_state_t state, float rpm, float target_rpm, uint16_t delay_ms)
 {
     bool ok = true;
 
@@ -717,7 +717,7 @@ sleep, and spindle stop override.
 \param rpm the spindle RPM to set.
 \returns \a true if successful, \a false if the current controller state is \ref ABORTED.
 */
-static bool _spindle_set_state (spindle_ptrs_t *spindle, spindle_state_t state, float rpm, uint16_t on_delay_ms)
+FLASHMEM static bool _spindle_set_state (spindle_ptrs_t *spindle, spindle_state_t state, float rpm, uint16_t on_delay_ms)
 {
     if(!ABORTED) { // Block during abort.
 
@@ -755,7 +755,7 @@ static bool _spindle_set_state (spindle_ptrs_t *spindle, spindle_state_t state,
     return !ABORTED;
 }
 
-bool spindle_set_state (spindle_ptrs_t *spindle, spindle_state_t state, float rpm)
+FLASHMEM bool spindle_set_state (spindle_ptrs_t *spindle, spindle_state_t state, float rpm)
 {
     return _spindle_set_state(spindle, state, rpm, 0);
 }
@@ -767,7 +767,7 @@ for it to reach the speed and raise an alarm if the speed is not reached within
 \param rpm the spindle RPM to set.
 \returns \a true if successful, \a false if the current controller state is \ref ABORTED.
 */
-static bool spindle_set_state_wait (spindle_ptrs_t *spindle, spindle_state_t state, float rpm, uint16_t delay_ms)
+FLASHMEM static bool spindle_set_state_wait (spindle_ptrs_t *spindle, spindle_state_t state, float rpm, uint16_t delay_ms)
 {
     bool ok;
 
@@ -818,7 +818,7 @@ for it to reach the speed and raise an alarm if the speed is not reached within
 \param rpm the spindle RPM to set.
 \returns \a true if successful, \a false if the current controller state is \ref ABORTED.
 */
-bool spindle_set_state_synced (spindle_ptrs_t *spindle, spindle_state_t state, float rpm, spindle_rpm_mode_t mode)
+FLASHMEM bool spindle_set_state_synced (spindle_ptrs_t *spindle, spindle_state_t state, float rpm, spindle_rpm_mode_t mode)
 {
     bool ok;
 
@@ -839,7 +839,7 @@ bool spindle_set_state_synced (spindle_ptrs_t *spindle, spindle_state_t state, f
 \param rpm the spindle RPM to set.
 \returns \a true if successful, \a false if the current controller state is \ref ABORTED.
 */
-bool spindle_restore (spindle_ptrs_t *spindle, spindle_state_t state, float rpm, uint16_t delay_ms)
+FLASHMEM bool spindle_restore (spindle_ptrs_t *spindle, spindle_state_t state, float rpm, uint16_t delay_ms)
 {
     bool ok;
 
@@ -872,7 +872,7 @@ float spindle_set_rpm (spindle_ptrs_t *spindle, float rpm, override_t override_p
 
 /*! \brief Turn off all enabled spindles.
 */
-void spindle_all_off (bool reset)
+FLASHMEM void spindle_all_off (bool reset)
 {
     spindle_ptrs_t *spindle;
     uint_fast8_t spindle_num = N_SYS_SPINDLE;
@@ -899,7 +899,7 @@ void spindle_all_off (bool reset)
 /*! \brief Check if any of the enabled spindles is running.
 \returns \a true if a spindle is running, \a false otherwise.
 */
-bool spindle_is_on (void)
+FLASHMEM bool spindle_is_on (void)
 {
     bool on = false;
 
@@ -1000,7 +1000,7 @@ static void set_laser_overdrive (struct spindle_pwm *pwm_data, float overdrive_p
 \param clock_hz timer clock frequency used for PWM generation.
 \returns \a true if successful, \a false if no PWM range possible - driver should then revert to simple on/off spindle control.
 */
-bool spindle_precompute_pwm_values (spindle_ptrs_t *spindle, spindle_pwm_t *pwm_data, spindle_pwm_settings_t *settings, uint32_t clock_hz)
+FLASHMEM bool spindle_precompute_pwm_values (spindle_ptrs_t *spindle, spindle_pwm_t *pwm_data, spindle_pwm_settings_t *settings, uint32_t clock_hz)
 {
     pwm_data->settings = settings;
     pwm_data->off_value = pwm_data->pwm_overdrive = 0;
@@ -1067,7 +1067,7 @@ static spindle1_settings_changed_ptr on_spindle1_settings_changed;
 
 #if ENABLE_SPINDLE_LINEARIZATION
 
-static status_code_t set_linear_piece (setting_id_t id, char *svalue)
+FLASHMEM static status_code_t set_linear_piece (setting_id_t id, char *svalue)
 {
     uint32_t idx = id - Setting_LinearSpindle1Piece1;
     float rpm, start, end;
@@ -1088,7 +1088,7 @@ static status_code_t set_linear_piece (setting_id_t id, char *svalue)
     return Status_OK;
 }
 
-static char *get_linear_piece (setting_id_t id)
+FLASHMEM static char *get_linear_piece (setting_id_t id)
 {
     static char buf[40];
 
@@ -1104,7 +1104,7 @@ static char *get_linear_piece (setting_id_t id)
 
 #endif
 
-static status_code_t set_spindle_invert (setting_id_t id, uint_fast16_t int_value)
+FLASHMEM static status_code_t set_spindle_invert (setting_id_t id, uint_fast16_t int_value)
 {
     sp1_settings.cfg.invert.mask = int_value;
     if(sp1_settings.cfg.invert.pwm && !spindle_cap.pwm_invert) {
@@ -1115,7 +1115,7 @@ static status_code_t set_spindle_invert (setting_id_t id, uint_fast16_t int_valu
     return Status_OK;
 }
 
-static status_code_t set_pwm_options (setting_id_t id, uint_fast16_t int_value)
+FLASHMEM static status_code_t set_pwm_options (setting_id_t id, uint_fast16_t int_value)
 {
     if(int_value & 0b0001) {
         if(int_value > 0b1111)
@@ -1134,7 +1134,7 @@ static status_code_t set_pwm_options (setting_id_t id, uint_fast16_t int_value)
     return Status_OK;
 }
 
-static uint32_t get_int (setting_id_t id)
+FLASHMEM static uint32_t get_int (setting_id_t id)
 {
     uint32_t value = 0;
 
@@ -1160,7 +1160,7 @@ static uint32_t get_int (setting_id_t id)
     return value;
 }
 
-static float get_port (setting_id_t id)
+FLASHMEM static float get_port (setting_id_t id)
 {
     uint8_t port;
 
@@ -1182,12 +1182,12 @@ static float get_port (setting_id_t id)
     return port == IOPORT_UNASSIGNED ? -1.0f : (float)port;
 }
 
-bool pwm_port_validate (xbar_t *properties, uint8_t port, void *data)
+FLASHMEM bool pwm_port_validate (xbar_t *properties, uint8_t port, void *data)
 {
     return port == *(uint8_t *)data;
 }
 
-static status_code_t set_port (setting_id_t id, float value)
+FLASHMEM static status_code_t set_port (setting_id_t id, float value)
 {
     bool ok = true;
     uint8_t port = value < 0.0f ? IOPORT_UNASSIGNED : (uint8_t)value;
@@ -1212,22 +1212,22 @@ static status_code_t set_port (setting_id_t id, float value)
     return ok ? Status_OK : Status_SettingValueOutOfRange;
 }
 
-static bool has_pwm (const setting_detail_t *setting, uint_fast16_t offset)
+FLASHMEM static bool has_pwm (const setting_detail_t *setting, uint_fast16_t offset)
 {
     return spindle_cap.variable;
 }
 
-static bool has_freq (const setting_detail_t *setting, uint_fast16_t offset)
+FLASHMEM static bool has_freq (const setting_detail_t *setting, uint_fast16_t offset)
 {
     return spindle_cap.variable && !spindle_cap.cloned;
 }
 
-static bool has_ports (const setting_detail_t *setting, uint_fast16_t offset)
+FLASHMEM static bool has_ports (const setting_detail_t *setting, uint_fast16_t offset)
 {
     return ports_ok;
 }
 
-static const setting_detail_t spindle1_settings[] = {
+PROGMEM static const setting_detail_t spindle1_settings[] = {
     { Setting_Spindle_OnPort, Group_AuxPorts, "PWM2 spindle on port", NULL, Format_Decimal, "-#0", "0", max_dport, Setting_NonCoreFn, set_port, get_port, has_ports, { .reboot_required = On } },
     { Setting_Spindle_DirPort, Group_AuxPorts, "PWM2 spindle direction port", NULL, Format_Decimal, "-#0", "-1", max_dport, Setting_NonCoreFn, set_port, get_port, has_ports, { .reboot_required = On } },
     { Setting_SpindleInvertMask1, Group_Spindle, "PWM2 spindle signals invert", NULL, Format_Bitfield, spindle_signals, NULL, NULL, Setting_IsExtendedFn, set_spindle_invert, get_int, NULL, { .reboot_required = On } },
@@ -1253,7 +1253,7 @@ static const setting_detail_t spindle1_settings[] = {
 #endif
 };
 
-static const setting_descr_t spindle1_settings_descr[] = {
+PROGMEM static const setting_descr_t spindle1_settings_descr[] = {
     { Setting_Spindle_OnPort, "On/off aux port." },
     { Setting_Spindle_DirPort, "Direction aux port, set to -1 if not required." },
     { Setting_SpindleInvertMask1, "Inverts the spindle on, counterclockwise and PWM signals (active low)." },
@@ -1282,7 +1282,7 @@ static const setting_descr_t spindle1_settings_descr[] = {
 #endif
 };
 
-static void spindle1_settings_changed (settings_t *settings, settings_changed_flags_t changed)
+FLASHMEM static void spindle1_settings_changed (settings_t *settings, settings_changed_flags_t changed)
 {
     UNUSED(changed);
 
@@ -1290,7 +1290,7 @@ static void spindle1_settings_changed (settings_t *settings, settings_changed_fl
         on_spindle1_settings_changed(&sp1_settings);
 }
 
-static void onSettingsChanged (settings_t *settings, settings_changed_flags_t changed)
+FLASHMEM static void onSettingsChanged (settings_t *settings, settings_changed_flags_t changed)
 {
     if(on_settings_changed)
         on_settings_changed(settings, changed);
@@ -1299,14 +1299,14 @@ static void onSettingsChanged (settings_t *settings, settings_changed_flags_t ch
         spindle1_settings_changed(settings, changed);
 }
 
-static void spindle1_settings_save (void)
+FLASHMEM static void spindle1_settings_save (void)
 {
     hal.nvs.memcpy_to_nvs(nvs_address, (uint8_t *)&sp1_settings, sizeof(spindle1_pwm_settings_t), true);
 }
 
-static void spindle1_settings_restore (void)
+FLASHMEM static void spindle1_settings_restore (void)
 {
-    static const spindle_pwm_settings_t defaults = {
+    PROGMEM static const spindle_pwm_settings_t defaults = {
         .rpm_max = DEFAULT_SPINDLE1_RPM_MAX,
         .rpm_min = DEFAULT_SPINDLE1_RPM_MIN,
         .flags.pwm_disable = false,
@@ -1359,13 +1359,13 @@ static void spindle1_settings_restore (void)
     hal.nvs.memcpy_to_nvs(nvs_address, (uint8_t *)&sp1_settings, sizeof(spindle1_pwm_settings_t), true);
 }
 
-static void spindle1_settings_load (void)
+FLASHMEM static void spindle1_settings_load (void)
 {
     if((hal.nvs.memcpy_from_nvs((uint8_t *)&sp1_settings, nvs_address, sizeof(spindle1_pwm_settings_t), true) != NVS_TransferResult_OK))
         spindle1_settings_restore();
 }
 
-spindle1_pwm_settings_t *spindle1_settings_add (bool claim_ports)
+FLASHMEM spindle1_pwm_settings_t *spindle1_settings_add (bool claim_ports)
 {
     uint8_t a_out = IOPORT_UNASSIGNED;
 
@@ -1380,7 +1380,7 @@ spindle1_pwm_settings_t *spindle1_settings_add (bool claim_ports)
     return nvs_address == 0 && (!claim_ports || ports_ok) && (nvs_address = nvs_alloc(sizeof(spindle1_pwm_settings_t))) ? &sp1_settings : NULL;
 }
 
-void spindle1_settings_register (spindle_cap_t cap, spindle1_settings_changed_ptr on_changed)
+FLASHMEM void spindle1_settings_register (spindle_cap_t cap, spindle1_settings_changed_ptr on_changed)
 {
     static setting_details_t spindle1_setting_details = {
         .is_core = true,
diff --git a/state_machine.c b/state_machine.c
index c9739f2..5b9d87e 100644
--- a/state_machine.c
+++ b/state_machine.c
@@ -74,7 +74,7 @@ typedef struct {
 // Declare and initialize parking local variables
 static parking_data_t park = {0};
 
-static void state_spindle_restore (spindle_t *spindle, uint16_t on_delay_ms)
+FLASHMEM static void state_spindle_restore (spindle_t *spindle, uint16_t on_delay_ms)
 {
     if(spindle->hal) {
         if(grbl.on_spindle_programmed)
@@ -83,7 +83,7 @@ static void state_spindle_restore (spindle_t *spindle, uint16_t on_delay_ms)
     }
 }
 
-static void state_restore_conditions (restore_condition_t *condition)
+FLASHMEM static void state_restore_conditions (restore_condition_t *condition)
 {
     if(!settings.parking.flags.enabled || !park.flags.restart) {
 
@@ -108,7 +108,7 @@ static void state_restore_conditions (restore_condition_t *condition)
     }
 }
 
-static void enter_sleep (void)
+FLASHMEM static void enter_sleep (void)
 {
     st_go_idle();
     spindle_all_off(false);
@@ -117,7 +117,7 @@ static void enter_sleep (void)
     stateHandler = state_noop;
 }
 
-static bool initiate_hold (uint_fast16_t new_state)
+FLASHMEM static bool initiate_hold (uint_fast16_t new_state)
 {
     spindle_t *spindle;
     spindle_num_t spindle_num = N_SYS_SPINDLE;
@@ -181,7 +181,7 @@ static bool initiate_hold (uint_fast16_t new_state)
     return sys_state == STATE_CYCLE;
 }
 
-bool state_door_reopened (void)
+FLASHMEM bool state_door_reopened (void)
 {
     return settings.parking.flags.enabled && park.flags.restart;
 }
@@ -360,7 +360,7 @@ void state_set (sys_state_t new_state)
 }
 
 // Suspend manager. Controls spindle overrides in hold states.
-void state_suspend_manager (void)
+FLASHMEM void state_suspend_manager (void)
 {
     if(stateHandler != state_await_resume || !gc_spindle_get(0)->state.on)
         return;
@@ -454,7 +454,7 @@ static void state_cycle (uint_fast16_t rt_exec)
 
 /*! /brief Waits for tool change cycle to end then restarts the cycle.
  */
-static void state_await_toolchanged (uint_fast16_t rt_exec)
+FLASHMEM static void state_await_toolchanged (uint_fast16_t rt_exec)
 {
     if (rt_exec & EXEC_CYCLE_START) {
         if (!gc_state.tool_change) {
@@ -477,7 +477,7 @@ static void state_await_toolchanged (uint_fast16_t rt_exec)
 
 /*! /brief Waits for motion to end to complete then executes actions depending on the current sys_state.
  */
-static void state_await_motion_cancel (uint_fast16_t rt_exec)
+FLASHMEM static void state_await_motion_cancel (uint_fast16_t rt_exec)
 {
     if (rt_exec & EXEC_CYCLE_COMPLETE) {
         if (sys_state == STATE_JOG) {
@@ -490,17 +490,16 @@ static void state_await_motion_cancel (uint_fast16_t rt_exec)
 
         state_set(pending_state);
 
-        if(sys.alarm_pending) {
+        if(sys.alarm_pending)
             system_set_exec_alarm(sys.alarm_pending);
-            sys.alarm_pending = Alarm_None;
-        } else if(gc_state.tool_change)
+        else if(gc_state.tool_change)
             state_set(STATE_TOOL_CHANGE);
     }
 }
 
 /*! /brief Waits for feed hold to complete then executes actions depending on the current sys_state.
  */
-static void state_await_hold (uint_fast16_t rt_exec)
+FLASHMEM static void state_await_hold (uint_fast16_t rt_exec)
 {
     if (rt_exec & EXEC_CYCLE_COMPLETE) {
 
@@ -509,10 +508,8 @@ static void state_await_hold (uint_fast16_t rt_exec)
         plan_cycle_reinitialize();
         sys.step_control.flags = 0;
 
-        if (sys.alarm_pending) {
+        if(sys.alarm_pending)
             system_set_exec_alarm(sys.alarm_pending);
-            sys.alarm_pending = Alarm_None;
-        }
 
         switch (sys_state) {
 
@@ -610,7 +607,7 @@ static void state_await_hold (uint_fast16_t rt_exec)
 
 /*! /brief Waits for action to execute when in feed hold state.
  */
-static void state_await_resume (uint_fast16_t rt_exec)
+FLASHMEM static void state_await_resume (uint_fast16_t rt_exec)
 {
     if ((rt_exec & EXEC_CYCLE_COMPLETE) && settings.parking.flags.enabled) {
         if (sys.step_control.execute_sys_motion) {
@@ -715,7 +712,7 @@ static void state_await_resume (uint_fast16_t rt_exec)
 /*! /brief Waits until plunge motion abort is completed then calls state_await_hold() to restart retraction.
 state_await_hold() is set to handle the cycle complete event.
  */
-static void state_await_restart_retract (uint_fast16_t rt_exec)
+FLASHMEM static void state_await_restart_retract (uint_fast16_t rt_exec)
 {
     if (rt_exec & EXEC_CYCLE_COMPLETE) {
 
@@ -732,7 +729,7 @@ static void state_await_restart_retract (uint_fast16_t rt_exec)
 /*! /brief Sets up a feed hold to abort plunge motion.
 state_await_restart_retract() is set to handle the cycle complete event.
  */
-static void restart_retract (void)
+FLASHMEM static void restart_retract (void)
 {
     grbl.report.feedback_message(Message_SafetyDoorAjar);
 
@@ -752,7 +749,7 @@ static void restart_retract (void)
 /*! /brief Waits until slow plunge motion is completed then deenergize spindle and coolant and execute fast retract motion.
 state_await_resume() is set to handle the cycle complete event.
  */
-static void state_await_waypoint_retract (uint_fast16_t rt_exec)
+FLASHMEM static void state_await_waypoint_retract (uint_fast16_t rt_exec)
 {
     if (rt_exec & EXEC_CYCLE_COMPLETE) {
 
@@ -793,7 +790,7 @@ static void state_await_waypoint_retract (uint_fast16_t rt_exec)
 state_await_resumed() is set to handle the cycle complete event.
 Note: A safety door event during restoration or motion will halt it and restart the retract sequence.
  */
-static void state_restore (uint_fast16_t rt_exec)
+FLASHMEM static void state_restore (uint_fast16_t rt_exec)
 {
     if (rt_exec & EXEC_SAFETY_DOOR) {
         if(park.flags.restoring)
@@ -848,7 +845,7 @@ static void state_restore (uint_fast16_t rt_exec)
 /*! /brief Waits until slow plunge motion is complete then restart the cycle.
 Note: A safety door event during the motion will halt it and restart the retract sequence.
  */
-static void state_await_resumed (uint_fast16_t rt_exec)
+FLASHMEM static void state_await_resumed (uint_fast16_t rt_exec)
 {
     if (rt_exec & EXEC_SAFETY_DOOR)
         restart_retract();
diff --git a/stepdir_map.h b/stepdir_map.h
index e6f1d2f..a890f61 100644
--- a/stepdir_map.h
+++ b/stepdir_map.h
@@ -27,7 +27,7 @@
 
 #if STEP_OUTMODE == GPIO_MAP
 
-static const uint32_t c_step_outmap[] = {
+PROGMEM static const uint32_t c_step_outmap[] = {
     0,
     X_STEP_BIT,
     Y_STEP_BIT,
@@ -302,7 +302,7 @@ static uint32_t step_outmap[sizeof(c_step_outmap) / sizeof(uint32_t)];
 
 #if DIRECTION_OUTMODE == GPIO_MAP
 
-static const uint32_t c_dir_outmap[] = {
+PROGMEM static const uint32_t c_dir_outmap[] = {
     0,
     X_DIRECTION_BIT,
     Y_DIRECTION_BIT,
diff --git a/stepper.c b/stepper.c
index 0050a59..21e8cf9 100644
--- a/stepper.c
+++ b/stepper.c
@@ -223,7 +223,7 @@ extern void gc_output_message (char *message);
 
 
 // Callback from delay to deenergize steppers after movement, might been cancelled
-void st_deenergize (void *data)
+FLASHMEM void st_deenergize (void *data)
 {
     if(sys.steppers_deenergize) {
         hal.stepper.enable(settings.steppers.energize, true);
@@ -233,7 +233,7 @@ void st_deenergize (void *data)
 
 // Stepper state initialization. Cycle should only start if the st.cycle_start flag is
 // enabled. Startup init and limits call this function but shouldn't start the cycle.
-void st_wake_up (void)
+FLASHMEM void st_wake_up (void)
 {
     // Initialize stepper data to ensure first ISR call does not step and
     // cancel any pending steppers deenergize
@@ -302,7 +302,7 @@ typedef struct {
 static spindle_sync_t spindle_tracker;
 static on_settings_changed_ptr on_settings_changed = NULL;
 
-void st_spindle_sync_cfg (settings_t *settings, settings_changed_flags_t changed)
+FLASHMEM void st_spindle_sync_cfg (settings_t *settings, settings_changed_flags_t changed)
 {
     if(!on_settings_changed) {
         on_settings_changed = grbl.on_settings_changed;
@@ -722,7 +722,7 @@ ISR_CODE void ISR_FUNC(stepper_driver_interrupt_handler)(void)
 //! \endcond
 
 // Reset and clear stepper subsystem variables
-void st_reset (void)
+FLASHMEM void st_reset (void)
 {
     if(hal.probe.configure)
         hal.probe.configure(false, false);
@@ -830,7 +830,7 @@ void st_update_plan_block_parameters (bool fast_hold)
 }
 
 // Changes the run state of the step segment buffer to execute the special parking motion.
-void st_parking_setup_buffer (void)
+FLASHMEM void st_parking_setup_buffer (void)
 {
     // Store step execution data of partially completed block, if necessary.
     if (prep.recalculate.hold_partial_block && !prep.recalculate.parking) {
@@ -848,7 +848,7 @@ void st_parking_setup_buffer (void)
 
 
 // Restores the step segment buffer to the normal run state after a parking motion.
-void st_parking_restore_buffer (void)
+FLASHMEM void st_parking_restore_buffer (void)
 {
     // Restore step execution data and flags of partially completed block, if necessary.
     if (prep.recalculate.hold_partial_block) {
diff --git a/stream.c b/stream.c
index d0ff4a5..99be6e1 100644
--- a/stream.c
+++ b/stream.c
@@ -83,7 +83,7 @@ static struct {
     on_gcode_mode_changed_ptr on_gcode_mode_changed;
 } mpg;
 
-void stream_register_streams (io_stream_details_t *details)
+FLASHMEM void stream_register_streams (io_stream_details_t *details)
 {
     if(details->n_streams) {
         details->next = streams;
@@ -91,7 +91,7 @@ void stream_register_streams (io_stream_details_t *details)
     }
 }
 
-bool stream_enumerate_streams (stream_enumerate_callback_ptr callback, void *data)
+FLASHMEM bool stream_enumerate_streams (stream_enumerate_callback_ptr callback, void *data)
 {
     if(callback == NULL)
         return false;
@@ -127,7 +127,7 @@ int32_t stream_get_null (void)
     return SERIAL_NO_DATA;
 }
 
-const io_stream_status_t *stream_get_uart_status (uint8_t instance)
+FLASHMEM const io_stream_status_t *stream_get_uart_status (uint8_t instance)
 {
     const io_stream_status_t *status = NULL;
 
@@ -166,12 +166,12 @@ ISR_CODE static bool ISR_FUNC(await_toolchange_ack)(uint8_t c)
     return true;
 }
 
-stream_suspend_state_t stream_is_rx_suspended (void)
+FLASHMEM stream_suspend_state_t stream_is_rx_suspended (void)
 {
     return stream.rxbuffer ? (stream.rxbuffer->backup ? StreamSuspend_Active : StreamSuspend_Pending) : StreamSuspend_Off;
 }
 
-bool stream_rx_suspend (stream_rx_buffer_t *rxbuffer, bool suspend)
+FLASHMEM bool stream_rx_suspend (stream_rx_buffer_t *rxbuffer, bool suspend)
 {
     bool ok = false;
 
@@ -223,7 +223,7 @@ static bool is_not_connected (void)
     return false;
 }
 
-static bool connection_is_up (io_stream_t *stream)
+FLASHMEM static bool connection_is_up (io_stream_t *stream)
 {
     if(stream->is_connected)
         return stream->is_connected();
@@ -257,7 +257,7 @@ static void stream_write_all (const char *s)
     }
 }
 
-static stream_connection_t *add_connection (const io_stream_t *stream)
+FLASHMEM static stream_connection_t *add_connection (const io_stream_t *stream)
 {
     stream_connection_t *connection, *last = connections;
 
@@ -285,7 +285,7 @@ static stream_connection_t *add_connection (const io_stream_t *stream)
     return connection;
 }
 
-static bool stream_select (const io_stream_t *stream, bool add)
+FLASHMEM static bool stream_select (const io_stream_t *stream, bool add)
 {
     static const io_stream_t *active_stream = NULL;
 
@@ -404,12 +404,12 @@ static bool stream_select (const io_stream_t *stream, bool add)
     return true;
 }
 
-const io_stream_t *stream_get_base (void)
+FLASHMEM const io_stream_t *stream_get_base (void)
 {
     return base.stream;
 }
 
-io_stream_flags_t stream_get_flags (io_stream_t stream)
+FLASHMEM io_stream_flags_t stream_get_flags (io_stream_t stream)
 {
     io_stream_flags_t flags = {0};
     io_stream_details_t *details = streams;
@@ -428,7 +428,7 @@ io_stream_flags_t stream_get_flags (io_stream_t stream)
     return flags;
 }
 
-bool stream_set_description (const io_stream_t *stream, const char *description)
+FLASHMEM bool stream_set_description (const io_stream_t *stream, const char *description)
 {
     bool ok;
 
@@ -440,7 +440,7 @@ bool stream_set_description (const io_stream_t *stream, const char *description)
     return ok;
 }
 
-bool stream_connect (const io_stream_t *stream)
+FLASHMEM bool stream_connect (const io_stream_t *stream)
 {
     bool ok;
 
@@ -456,7 +456,7 @@ typedef struct {
     io_stream_t const *stream;
 } connection_t;
 
-static bool _open_instance (io_stream_properties_t const *stream, void *data)
+FLASHMEM static bool _open_instance (io_stream_properties_t const *stream, void *data)
 {
     connection_t *connection = (connection_t *)data;
 
@@ -468,7 +468,7 @@ static bool _open_instance (io_stream_properties_t const *stream, void *data)
     return connection->stream != NULL;
 }
 
-bool stream_connect_instance (uint8_t instance, uint32_t baud_rate)
+FLASHMEM bool stream_connect_instance (uint8_t instance, uint32_t baud_rate)
 {
     connection_t connection = {
         .instance = instance,
@@ -478,13 +478,13 @@ bool stream_connect_instance (uint8_t instance, uint32_t baud_rate)
     return stream_enumerate_streams(_open_instance, &connection) && stream_connect(connection.stream);
 }
 
-void stream_disconnect (const io_stream_t *stream)
+FLASHMEM void stream_disconnect (const io_stream_t *stream)
 {
     if(stream)
         stream_select(stream, false);
 }
 
-io_stream_t const *stream_open_instance (uint8_t instance, uint32_t baud_rate, stream_write_char_ptr rx_handler, const char *description)
+FLASHMEM io_stream_t const *stream_open_instance (uint8_t instance, uint32_t baud_rate, stream_write_char_ptr rx_handler, const char *description)
 {
     connection_t connection = {
         .instance = instance,
@@ -500,7 +500,7 @@ io_stream_t const *stream_open_instance (uint8_t instance, uint32_t baud_rate, s
     return connection.stream;
 }
 
-bool stream_close (io_stream_t const *stream)
+FLASHMEM bool stream_close (io_stream_t const *stream)
 {
     bool released = false;
     io_stream_details_t *details = streams;
@@ -524,7 +524,7 @@ bool stream_close (io_stream_t const *stream)
 
 // UART style streams
 
-void stream_set_defaults (const io_stream_t *stream, uint32_t baud_rate)
+FLASHMEM void stream_set_defaults (const io_stream_t *stream, uint32_t baud_rate)
 {
     stream->set_enqueue_rt_handler(protocol_enqueue_realtime_command);
 
@@ -556,7 +556,7 @@ ISR_CODE static void mpg_rt_report_add (report_tracking_flags_t report)
         mpg.stream.report.flags.value |= report.value;
 }
 
-static void mpg_gcode_mode_changed (void)
+FLASHMEM static void mpg_gcode_mode_changed (void)
 {
     if(mpg.on_gcode_mode_changed)
         mpg.on_gcode_mode_changed();
@@ -565,7 +565,7 @@ static void mpg_gcode_mode_changed (void)
         report_gcode_modes(mpg.stream.write);
 }
 
-void stream_mpg_set_mode (void *data)
+FLASHMEM void stream_mpg_set_mode (void *data)
 {
     stream_mpg_enable(data != NULL);
 }
@@ -614,7 +614,7 @@ ISR_CODE bool ISR_FUNC(stream_mpg_check_enable)(uint8_t c)
     return true;
 }
 
-bool stream_mpg_register (const io_stream_t *stream, bool rx_only, stream_write_char_ptr write_char)
+FLASHMEM bool stream_mpg_register (const io_stream_t *stream, bool rx_only, stream_write_char_ptr write_char)
 {
     if(stream == NULL || !stream_is_uart(stream->type) || stream->disable_rx == NULL)
         return false;
@@ -667,12 +667,12 @@ bool stream_mpg_register (const io_stream_t *stream, bool rx_only, stream_write_
     return connection != NULL;
 }
 
-static void report_mpg_mode (void *data)
+FLASHMEM static void report_mpg_mode (void *data)
 {
     protocol_enqueue_realtime_command((uint8_t)((uintptr_t)data));
 }
 
-bool stream_mpg_enable (bool on)
+FLASHMEM bool stream_mpg_enable (bool on)
 {
     static io_stream_t org_stream = {
         .type = StreamType_Redirected
@@ -735,45 +735,45 @@ bool stream_mpg_enable (bool on)
 
 static enqueue_realtime_command_ptr enqueue_realtime_command = protocol_enqueue_realtime_command;
 
-static uint16_t null_rx_free (void)
+FLASHMEM static uint16_t null_rx_free (void)
 {
     return RX_BUFFER_SIZE;
 }
 
-static uint16_t null_count (void)
+FLASHMEM static uint16_t null_count (void)
 {
     return 0;
 }
 
-static bool null_put_c (const uint8_t c)
+FLASHMEM static bool null_put_c (const uint8_t c)
 {
     return true;
 }
 
-static void null_write_string (const char *s)
+FLASHMEM static void null_write_string (const char *s)
 {
 }
 
-static void null_write(const uint8_t *s, uint16_t length)
+FLASHMEM static void null_write(const uint8_t *s, uint16_t length)
 {
 }
 
-static bool null_suspend_disable (bool suspend)
+FLASHMEM static bool null_suspend_disable (bool suspend)
 {
     return true;
 }
 
-static bool null_set_baudrate (uint32_t baud_rate)
+FLASHMEM static bool null_set_baudrate (uint32_t baud_rate)
 {
     return true;
 }
 
-static bool null_enqueue_rt_command (uint8_t c)
+FLASHMEM static bool null_enqueue_rt_command (uint8_t c)
 {
     return enqueue_realtime_command(c);
 }
 
-static enqueue_realtime_command_ptr null_set_rt_handler (enqueue_realtime_command_ptr handler)
+FLASHMEM static enqueue_realtime_command_ptr null_set_rt_handler (enqueue_realtime_command_ptr handler)
 {
     enqueue_realtime_command_ptr prev = enqueue_realtime_command;
 
@@ -783,9 +783,9 @@ static enqueue_realtime_command_ptr null_set_rt_handler (enqueue_realtime_comman
     return prev;
 }
 
-const io_stream_t *stream_null_init (uint32_t baud_rate)
+FLASHMEM const io_stream_t *stream_null_init (uint32_t baud_rate)
 {
-    static const io_stream_t stream = {
+    PROGMEM static const io_stream_t stream = {
         .type = StreamType_Null,
         .is_connected = stream_connected,
         .read = stream_get_null,
@@ -937,7 +937,7 @@ void debug_printf (const char *fmt, ...)
 
 #else
 
-void debug_printf (const char *fmt, ...)
+FLASHMEM void debug_printf (const char *fmt, ...)
 {
     // NOOP
 }
diff --git a/stream_file.c b/stream_file.c
index ad96cd0..5c7ac2b 100644
--- a/stream_file.c
+++ b/stream_file.c
@@ -67,7 +67,7 @@ static int32_t stream_read_file (void)
     return (int32_t)c;
 }
 
-static status_code_t onFileEnd (vfs_file_t *file, status_code_t status)
+FLASHMEM static status_code_t onFileEnd (vfs_file_t *file, status_code_t status)
 {
     rd_stream_t *stream;
 
@@ -88,7 +88,7 @@ static status_code_t onFileEnd (vfs_file_t *file, status_code_t status)
 // This code will be executed after each command is sent to the parser,
 // If an error is detected reading from file(s) will be stopped and the
 // status_code reported, if not a "ok" status reply will not be output.
-static status_code_t trap_status_messages (status_code_t status)
+FLASHMEM static status_code_t trap_status_messages (status_code_t status)
 {
     gc_state.last_error = status;
 
@@ -111,7 +111,7 @@ static status_code_t trap_status_messages (status_code_t status)
     return status;
 }
 
-static void onReportHandlersInit (void)
+FLASHMEM static void onReportHandlersInit (void)
 {
     if(on_report_handlers_init)
         on_report_handlers_init();
@@ -120,19 +120,19 @@ static void onReportHandlersInit (void)
     grbl.report.status_message = trap_status_messages;
 }
 
-void stream_set_type (stream_type_t type, vfs_file_t *file)
+FLASHMEM void stream_set_type (stream_type_t type, vfs_file_t *file)
 {
     hal.stream.type = type;
     if(!(hal.stream.file = file))
         gc_state.file_stream = false;
 }
 
-bool stream_is_file (void)
+FLASHMEM bool stream_is_file (void)
 {
     return hal.stream.type == StreamType_File;
 }
 
-vfs_file_t *stream_redirect_read (char *filename, status_message_ptr status_handler, on_file_end_ptr eof_handler)
+FLASHMEM vfs_file_t *stream_redirect_read (char *filename, status_message_ptr status_handler, on_file_end_ptr eof_handler)
 {
     static bool error_handler_ok = false;
 
@@ -180,7 +180,7 @@ vfs_file_t *stream_redirect_read (char *filename, status_message_ptr status_hand
     return file;
 }
 
-void stream_redirect_close (vfs_file_t *file)
+FLASHMEM void stream_redirect_close (vfs_file_t *file)
 {
     rd_stream_t *stream = rd_streams, *prev_stream = NULL;
 
diff --git a/system.c b/system.c
index f21b996..83839e7 100644
--- a/system.c
+++ b/system.c
@@ -159,7 +159,7 @@ ISR_CODE void ISR_FUNC(control_interrupt_handler)(control_signals_t signals)
 
 /*! \brief Executes user startup scripts, if stored.
 */
-void system_execute_startup (void *data)
+FLASHMEM void system_execute_startup (void *data)
 {
     if(hal.nvs.type != NVS_None) {
 
@@ -179,7 +179,7 @@ void system_execute_startup (void *data)
     }
 }
 
-static status_code_t read_int (char *s, int32_t *value)
+FLASHMEM static status_code_t read_int (char *s, int32_t *value)
 {
     uint_fast8_t counter = 0;
     float parameter;
@@ -199,7 +199,7 @@ static status_code_t read_int (char *s, int32_t *value)
 //
 
 // Reset spindle encoder data
-static status_code_t spindle_reset_data (sys_state_t state, char *args)
+FLASHMEM static status_code_t spindle_reset_data (sys_state_t state, char *args)
 {
     spindle_t *spindle = gc_spindle_get(-1);
 
@@ -209,7 +209,7 @@ static status_code_t spindle_reset_data (sys_state_t state, char *args)
     return spindle->hal->reset_data ? Status_OK : Status_InvalidStatement;
 }
 
-static status_code_t jog (sys_state_t state, char *args)
+FLASHMEM static status_code_t jog (sys_state_t state, char *args)
 {
     if(!(state == STATE_IDLE || (state & (STATE_JOG|STATE_TOOL_CHANGE))))
          return Status_IdleError;
@@ -222,47 +222,47 @@ static status_code_t jog (sys_state_t state, char *args)
     return args == NULL ? Status_InvalidStatement : gc_execute_block(args); // NOTE: $J= is ignored inside g-code parser and used to detect jog motions.
 }
 
-static status_code_t enumerate_alarms (sys_state_t state, char *args)
+FLASHMEM static status_code_t enumerate_alarms (sys_state_t state, char *args)
 {
     return report_alarm_details(false);
 }
 
-static status_code_t enumerate_alarms_grblformatted (sys_state_t state, char *args)
+FLASHMEM static status_code_t enumerate_alarms_grblformatted (sys_state_t state, char *args)
 {
     return report_alarm_details(true);
 }
 
-static status_code_t enumerate_errors (sys_state_t state, char *args)
+FLASHMEM static status_code_t enumerate_errors (sys_state_t state, char *args)
 {
     return report_error_details(false);
 }
 
-static status_code_t enumerate_errors_grblformatted (sys_state_t state, char *args)
+FLASHMEM static status_code_t enumerate_errors_grblformatted (sys_state_t state, char *args)
 {
     return report_error_details(true);
 }
 
-static status_code_t enumerate_groups (sys_state_t state, char *args)
+FLASHMEM static status_code_t enumerate_groups (sys_state_t state, char *args)
 {
     return report_setting_group_details(true, NULL);
 }
 
-static status_code_t enumerate_settings (sys_state_t state, char *args)
+FLASHMEM static status_code_t enumerate_settings (sys_state_t state, char *args)
 {
     return report_settings_details(SettingsFormat_MachineReadable, Setting_SettingsAll, Group_All);
 }
 
-static status_code_t enumerate_settings_grblformatted (sys_state_t state, char *args)
+FLASHMEM static status_code_t enumerate_settings_grblformatted (sys_state_t state, char *args)
 {
     return report_settings_details(SettingsFormat_Grbl, Setting_SettingsAll, Group_All);
 }
 
-static status_code_t enumerate_settings_halformatted (sys_state_t state, char *args)
+FLASHMEM static status_code_t enumerate_settings_halformatted (sys_state_t state, char *args)
 {
     return report_settings_details(SettingsFormat_grblHAL, Setting_SettingsAll, Group_All);
 }
 
-static status_code_t enumerate_all (sys_state_t state, char *args)
+FLASHMEM static status_code_t enumerate_all (sys_state_t state, char *args)
 {
     report_alarm_details(false);
     report_error_details(false);
@@ -270,7 +270,7 @@ static status_code_t enumerate_all (sys_state_t state, char *args)
     return report_settings_details(SettingsFormat_MachineReadable, Setting_SettingsAll, Group_All);
 }
 
-static status_code_t output_settings (sys_state_t state, char *args)
+FLASHMEM static status_code_t output_settings (sys_state_t state, char *args)
 {
     status_code_t retval = Status_OK;
 
@@ -291,7 +291,7 @@ static status_code_t output_settings (sys_state_t state, char *args)
     return retval;
 }
 
-static status_code_t output_setting_description (sys_state_t state, char *args)
+FLASHMEM static status_code_t output_setting_description (sys_state_t state, char *args)
 {
     status_code_t retval = Status_BadNumberFormat;
 
@@ -305,7 +305,7 @@ static status_code_t output_setting_description (sys_state_t state, char *args)
     return retval;
 }
 
-static status_code_t output_all_settings (sys_state_t state, char *args)
+FLASHMEM static status_code_t output_all_settings (sys_state_t state, char *args)
 {
     status_code_t retval = Status_OK;
 
@@ -322,7 +322,7 @@ static status_code_t output_all_settings (sys_state_t state, char *args)
     return retval;
 }
 
-static status_code_t output_parser_state (sys_state_t state, char *args)
+FLASHMEM static status_code_t output_parser_state (sys_state_t state, char *args)
 {
     report_gcode_modes(hal.stream.write);
     report_add_realtime(Report_Homed); // Report homed state on next realtime report
@@ -330,7 +330,7 @@ static status_code_t output_parser_state (sys_state_t state, char *args)
     return Status_OK;
 }
 
-static status_code_t toggle_single_block (sys_state_t state, char *args)
+FLASHMEM static status_code_t toggle_single_block (sys_state_t state, char *args)
 {
     if(!hal.signals_cap.single_block) {
         sys.flags.single_block = !sys.flags.single_block;
@@ -342,7 +342,7 @@ static status_code_t toggle_single_block (sys_state_t state, char *args)
     return hal.signals_cap.single_block ? Status_InvalidStatement : Status_OK;
 }
 
-static status_code_t toggle_block_delete (sys_state_t state, char *args)
+FLASHMEM static status_code_t toggle_block_delete (sys_state_t state, char *args)
 {
     if(!hal.signals_cap.block_delete) {
         sys.flags.block_delete_enabled = !sys.flags.block_delete_enabled;
@@ -354,7 +354,7 @@ static status_code_t toggle_block_delete (sys_state_t state, char *args)
     return hal.signals_cap.block_delete ? Status_InvalidStatement : Status_OK;
 }
 
-static status_code_t toggle_optional_stop (sys_state_t state, char *args)
+FLASHMEM static status_code_t toggle_optional_stop (sys_state_t state, char *args)
 {
     if(!hal.signals_cap.stop_disable) {
         sys.flags.optional_stop_disable = !sys.flags.optional_stop_disable;
@@ -366,7 +366,7 @@ static status_code_t toggle_optional_stop (sys_state_t state, char *args)
     return hal.signals_cap.stop_disable ? Status_InvalidStatement : Status_OK;
 }
 
-static status_code_t check_mode (sys_state_t state, char *args)
+FLASHMEM static status_code_t check_mode (sys_state_t state, char *args)
 {
     if (state == STATE_CHECK_MODE) {
         // Perform reset when toggling off. Check g-code mode should only work if grblHAL
@@ -383,33 +383,52 @@ static status_code_t check_mode (sys_state_t state, char *args)
     return Status_OK;
 }
 
-static status_code_t disable_lock (sys_state_t state, char *args)
+FLASHMEM static status_code_t check_status (bool homing)
+{
+    status_code_t status = Status_OK;
+
+    control_signals_t control_signals = hal.control.get_state();
+
+    // Block if self-test failed
+    if(sys.alarm == Alarm_SelftestFailed)
+        status = Status_SelfTestFailed;
+    // Block if e-stop is active.
+    else if(control_signals.e_stop)
+        status = Status_EStop;
+    // Block if safety door is ajar.
+    else if(control_signals.safety_door_ajar && !settings.safety_door.flags.ignore_when_idle)
+        status = Status_CheckDoor;
+    // Block if safety reset is active.
+    else if(control_signals.reset)
+        status = Status_Reset;
+
+    if(status == Status_OK) {
+        if(homing) {
+            if(control_signals.motor_fault)
+                status = Status_MotorFault;
+            else if(!(settings.homing.flags.enabled && (sys.homing.mask || settings.homing.flags.single_axis_commands || settings.homing.flags.manual)))
+                status = Status_HomingDisabled;
+        } else if(settings.limits.flags.hard_enabled && settings.limits.flags.check_at_init && limit_signals_merge(hal.limits.get_state()).value)
+            status = Status_LimitsEngaged;
+        else if(limits_homing_required())
+            status = Status_HomingRequired;
+    }
+
+    return status;
+}
+
+FLASHMEM static status_code_t disable_lock (sys_state_t state, char *args)
 {
     status_code_t retval = Status_OK;
 
     if(state & (STATE_ALARM|STATE_ESTOP)) {
 
-        control_signals_t control_signals = hal.control.get_state();
-
-        // Block if self-test failed
-        if(sys.alarm == Alarm_SelftestFailed)
-            retval = Status_SelfTestFailed;
-        // Block if e-stop is active.
-        else if (control_signals.e_stop)
-            retval = Status_EStop;
-        // Block if safety door is ajar.
-        else if (control_signals.safety_door_ajar)
-            retval = Status_CheckDoor;
-        // Block if safety reset is active.
-        else if(control_signals.reset)
-            retval = Status_Reset;
-        else if(settings.limits.flags.hard_enabled && settings.limits.flags.check_at_init && limit_signals_merge(hal.limits.get_state()).value)
-            retval = Status_LimitsEngaged;
-        else if(limits_homing_required())
-            retval = Status_HomingRequired;
-        else {
-            grbl.report.feedback_message(Message_AlarmUnlock);
+        if((retval = check_status(false)) == Status_OK) {
             state_set(STATE_IDLE);
+            if(sys.alarm_pending)
+                system_raise_alarm(sys.alarm_pending);
+            else
+                grbl.report.feedback_message(Message_AlarmUnlock);
         }
         // Don't run startup script. Prevents stored moves in startup from causing accidents.
     } // Otherwise, no effect.
@@ -417,59 +436,40 @@ static status_code_t disable_lock (sys_state_t state, char *args)
     return retval;
 }
 
-static status_code_t output_help (sys_state_t state, char *args)
+FLASHMEM static status_code_t output_help (sys_state_t state, char *args)
 {
     return report_help(args);
 }
 
-static status_code_t enumerate_spindles (sys_state_t state, char *args)
+FLASHMEM static status_code_t enumerate_spindles (sys_state_t state, char *args)
 {
     return report_spindles(false);
 }
 
-static status_code_t enumerate_spindles_mr (sys_state_t state, char *args)
+FLASHMEM static status_code_t enumerate_spindles_mr (sys_state_t state, char *args)
 {
     return report_spindles(true);
 }
 
-static status_code_t go_home (sys_state_t state, axes_signals_t axes)
+FLASHMEM static status_code_t go_home (sys_state_t state, axes_signals_t axes)
 {
     if(axes.mask && !settings.homing.flags.single_axis_commands)
         return Status_HomingDisabled;
 
-    if(!(state_get() == STATE_IDLE || (state_get() & (STATE_ALARM|STATE_ESTOP))))
+    if(!(state_get() == STATE_IDLE || state_get() == STATE_ALARM))
         return Status_IdleError;
 
     status_code_t retval = Status_OK;
 
-    control_signals_t control_signals = hal.control.get_state();
-
-    // Block if self-test failed
-    if(sys.alarm == Alarm_SelftestFailed)
-        retval = Status_SelfTestFailed;
-    // Block if e-stop is active.
-    else if (control_signals.e_stop)
-        retval = Status_EStop;
-    else if(control_signals.motor_fault)
-        retval = Status_MotorFault;
-    else if (!(settings.homing.flags.enabled && (sys.homing.mask || settings.homing.flags.single_axis_commands || settings.homing.flags.manual)))
-        retval = Status_HomingDisabled;
-    // Block if safety door is ajar.
-    else if (control_signals.safety_door_ajar && !settings.safety_door.flags.ignore_when_idle)
-        retval = Status_CheckDoor;
-    // Block if safety reset is active.
-    else if(control_signals.reset)
-        retval = Status_Reset;
-
-    if(retval == Status_OK)
+    if((retval = check_status(true)) == Status_OK)
         retval = mc_homing_cycle(axes); // Home axes according to configuration
 
-    if (retval == Status_OK && !sys.abort) {
+    if(retval == Status_OK && !sys.abort) {
         state_set(STATE_IDLE);  // Set to IDLE when complete.
         st_go_idle();           // Set steppers to the settings idle state before returning.
         grbl.report.feedback_message(Message_None);
         // Execute startup scripts after successful homing.
-        if (sys.homing.mask && (sys.homing.mask & sys.homed.mask) == sys.homing.mask)
+        if(sys.homing.mask && (sys.homing.mask & sys.homed.mask) == sys.homing.mask)
             system_execute_startup(NULL);
         else if(limits_homing_required()) { // Keep alarm state active if homing is required and not all axes homed.
             sys.alarm = Alarm_HomingRequired;
@@ -480,69 +480,69 @@ static status_code_t go_home (sys_state_t state, axes_signals_t axes)
     return retval == Status_Unhandled ? Status_OK : retval;
 }
 
-static status_code_t home (sys_state_t state, char *args)
+FLASHMEM static status_code_t home (sys_state_t state, char *args)
 {
     return go_home(state, (axes_signals_t){0});
 }
 
-static status_code_t home_x (sys_state_t state, char *args)
+FLASHMEM static status_code_t home_x (sys_state_t state, char *args)
 {
     return go_home(state, (axes_signals_t){X_AXIS_BIT});
 }
 
-static status_code_t home_y (sys_state_t state, char *args)
+FLASHMEM static status_code_t home_y (sys_state_t state, char *args)
 {
     return go_home(state, (axes_signals_t){Y_AXIS_BIT});
 }
 
-static status_code_t home_z (sys_state_t state, char *args)
+FLASHMEM static status_code_t home_z (sys_state_t state, char *args)
 {
     return go_home(state, (axes_signals_t){Z_AXIS_BIT});
 }
 
 #ifdef A_AXIS
-static status_code_t home_a (sys_state_t state, char *args)
+FLASHMEM static status_code_t home_a (sys_state_t state, char *args)
 {
     return go_home(state, (axes_signals_t){A_AXIS_BIT});
 }
 #endif
 
 #ifdef B_AXIS
-static status_code_t home_b (sys_state_t state, char *args)
+FLASHMEM static status_code_t home_b (sys_state_t state, char *args)
 {
     return go_home(state, (axes_signals_t){B_AXIS_BIT});
 }
 #endif
 
 #ifdef C_AXIS
-static status_code_t home_c (sys_state_t state, char *args)
+FLASHMEM static status_code_t home_c (sys_state_t state, char *args)
 {
     return go_home(state, (axes_signals_t){C_AXIS_BIT});
 }
 #endif
 
 #ifdef U_AXIS
-static status_code_t home_u (sys_state_t state, char *args)
+FLASHMEM static status_code_t home_u (sys_state_t state, char *args)
 {
     return go_home(state, (axes_signals_t){U_AXIS_BIT});
 }
 #endif
 
 #ifdef V_AXIS
-static status_code_t home_v (sys_state_t state, char *args)
+FLASHMEM static status_code_t home_v (sys_state_t state, char *args)
 {
     return go_home(state, (axes_signals_t){V_AXIS_BIT});
 }
 #endif
 
 #ifdef W_AXIS
-static status_code_t home_w (sys_state_t state, char *args)
+FLASHMEM static status_code_t home_w (sys_state_t state, char *args)
 {
     return go_home(state, (axes_signals_t){W_AXIS_BIT});
 }
 #endif
 
-static status_code_t enter_sleep (sys_state_t state, char *args)
+FLASHMEM static status_code_t enter_sleep (sys_state_t state, char *args)
 {
     if(!settings.flags.sleep_enable)
         return Status_InvalidStatement;
@@ -554,7 +554,7 @@ static status_code_t enter_sleep (sys_state_t state, char *args)
     return Status_OK;
 }
 
-static status_code_t set_tool_reference (sys_state_t state, char *args)
+FLASHMEM static status_code_t set_tool_reference (sys_state_t state, char *args)
 {
 #if TOOL_LENGTH_OFFSET_AXIS >= 0
     if(sys.flags.probe_succeeded) {
@@ -577,12 +577,12 @@ static status_code_t set_tool_reference (sys_state_t state, char *args)
     return Status_OK;
 }
 
-static status_code_t tool_probe_workpiece (sys_state_t state, char *args)
+FLASHMEM static status_code_t tool_probe_workpiece (sys_state_t state, char *args)
 {
     return tc_probe_workpiece();
 }
 
-static status_code_t output_ngc_parameters (sys_state_t state, char *args)
+FLASHMEM static status_code_t output_ngc_parameters (sys_state_t state, char *args)
 {
     status_code_t retval = Status_OK;
 
@@ -603,7 +603,7 @@ static status_code_t output_ngc_parameters (sys_state_t state, char *args)
     return retval;
 }
 
-static status_code_t build_info (sys_state_t state, char *args)
+FLASHMEM static status_code_t build_info (sys_state_t state, char *args)
 {
     if (!(state == STATE_IDLE || (state & (STATE_ALARM|STATE_ESTOP|STATE_SLEEP|STATE_CHECK_MODE))))
         return Status_IdleError;
@@ -623,7 +623,7 @@ static status_code_t build_info (sys_state_t state, char *args)
     return Status_OK;
 }
 
-static status_code_t output_all_build_info (sys_state_t state, char *args)
+FLASHMEM static status_code_t output_all_build_info (sys_state_t state, char *args)
 {
     char info[sizeof(stored_line_t)];
 
@@ -633,7 +633,7 @@ static status_code_t output_all_build_info (sys_state_t state, char *args)
     return Status_OK;
 }
 
-static status_code_t settings_downgrade (sys_state_t state, char *args)
+FLASHMEM static status_code_t settings_downgrade (sys_state_t state, char *args)
 {
     settings.version.build = settings.version.build == 0 ? (GRBL_BUILD - 20000000UL) : 0;
 
@@ -646,7 +646,7 @@ static status_code_t settings_downgrade (sys_state_t state, char *args)
     return Status_OK;
 }
 
-static status_code_t settings_reset (sys_state_t state, char *args)
+FLASHMEM static status_code_t settings_reset (sys_state_t state, char *args)
 {
     settings_restore_t restore = {0};
     status_code_t retval = Status_OK;
@@ -694,7 +694,7 @@ static status_code_t settings_reset (sys_state_t state, char *args)
     return retval;
 }
 
-static status_code_t output_startup_lines (sys_state_t state, char *args)
+FLASHMEM static status_code_t output_startup_lines (sys_state_t state, char *args)
 {
     if (!(state == STATE_IDLE || (state & (STATE_ALARM|STATE_ESTOP|STATE_CHECK_MODE))))
         return Status_IdleError;
@@ -714,7 +714,7 @@ static status_code_t output_startup_lines (sys_state_t state, char *args)
     return Status_OK;
 }
 
-static status_code_t set_startup_line (sys_state_t state, char *args, uint_fast8_t lnr)
+FLASHMEM static status_code_t set_startup_line (sys_state_t state, char *args, uint_fast8_t lnr)
 {
     if(args == NULL)
         return Status_InvalidStatement;
@@ -752,17 +752,17 @@ static status_code_t set_startup_line (sys_state_t state, char *args, uint_fast8
     return retval;
 }
 
-static status_code_t set_startup_line0 (sys_state_t state, char *args)
+FLASHMEM static status_code_t set_startup_line0 (sys_state_t state, char *args)
 {
     return set_startup_line(state, args, 0);
 }
 
-static status_code_t set_startup_line1 (sys_state_t state, char *args)
+FLASHMEM static status_code_t set_startup_line1 (sys_state_t state, char *args)
 {
     return set_startup_line(state, args, 1);
 }
 
-static status_code_t rtc_action (sys_state_t state, char *args)
+FLASHMEM static status_code_t rtc_action (sys_state_t state, char *args)
 {
     status_code_t retval;
 
@@ -785,7 +785,7 @@ static status_code_t rtc_action (sys_state_t state, char *args)
     return retval;
 }
 
-static status_code_t reboot_system (sys_state_t state, char *args)
+FLASHMEM static status_code_t reboot_system (sys_state_t state, char *args)
 {
     report_message("Rebooting controller, communication may be lost", Message_Warning);
     hal.delay_ms(100, NULL);
@@ -799,7 +799,7 @@ static status_code_t reboot_system (sys_state_t state, char *args)
 
 #include "nvs_buffer.h"
 
-static status_code_t output_memmap (sys_state_t state, char *args)
+FLASHMEM static status_code_t output_memmap (sys_state_t state, char *args)
 {
     nvs_memmap();
 
@@ -808,7 +808,7 @@ static status_code_t output_memmap (sys_state_t state, char *args)
 
 #endif
 
-const char *help_rst (const char *cmd)
+FLASHMEM const char *help_rst (const char *cmd)
 {
 #if ENABLE_RESTORE_NVS_WIPE_ALL
     hal.stream.write("$RST=* - restore/reset all settings." ASCII_EOL);
@@ -830,7 +830,7 @@ const char *help_rst (const char *cmd)
     return NULL;
 }
 
-const char *help_rtc (const char *cmd)
+FLASHMEM const char *help_rtc (const char *cmd)
 {
     if(hal.driver_cap.rtc) {
         hal.stream.write("$RTC - output current time." ASCII_EOL);
@@ -840,7 +840,7 @@ const char *help_rtc (const char *cmd)
     return NULL;
 }
 
-const char *help_spindle (const char *cmd)
+FLASHMEM const char *help_spindle (const char *cmd)
 {
     spindle_ptrs_t *spindle = gc_spindle_get(0)->hal;
 
@@ -853,22 +853,22 @@ const char *help_spindle (const char *cmd)
     return NULL;
 }
 
-const char *help_steppers (const char *cmd)
+FLASHMEM const char *help_steppers (const char *cmd)
 {
     return hal.stepper.status ? "output stepper driver status" : NULL;
 }
 
-const char *help_pins (const char *cmd)
+FLASHMEM const char *help_pins (const char *cmd)
 {
     return hal.enumerate_pins ? "enumerate pin bindings" : NULL;
 }
 
-const char *help_pin_state (const char *cmd)
+FLASHMEM const char *help_pin_state (const char *cmd)
 {
     return ioports_can_do().io ? "output auxiliary pin states" : NULL;
 }
 
-const char *help_switches (const char *cmd)
+FLASHMEM const char *help_switches (const char *cmd)
 {
     const char *help = NULL;
 
@@ -893,7 +893,7 @@ const char *help_switches (const char *cmd)
     return help;
 }
 
-const char *help_homing (const char *cmd)
+FLASHMEM const char *help_homing (const char *cmd)
 {
     if(settings.homing.flags.enabled)
         hal.stream.write("$H - home configured axes." ASCII_EOL);
@@ -904,7 +904,7 @@ const char *help_homing (const char *cmd)
     return NULL;
 }
 
-const char *help_reboot (const char *cmd)
+FLASHMEM const char *help_reboot (const char *cmd)
 {
     return hal.reboot ? "reboot (hard reset) controller" : NULL;
 }
@@ -1008,13 +1008,13 @@ static sys_commands_t core_commands = {
 
 static sys_commands_t *commands_root = &core_commands;
 
-void system_register_commands (sys_commands_t *commands)
+FLASHMEM void system_register_commands (sys_commands_t *commands)
 {
     commands->next = commands_root;
     commands_root = commands;
 }
 
-void _system_output_help (sys_commands_t *commands, bool traverse)
+FLASHMEM void _system_output_help (sys_commands_t *commands, bool traverse)
 {
     const char *help;
     uint_fast8_t idx;
@@ -1045,7 +1045,7 @@ void _system_output_help (sys_commands_t *commands, bool traverse)
 }
 
 // Deprecated, to be removed.
-void system_output_help (const sys_command_t *commands, uint32_t num_commands)
+FLASHMEM void system_output_help (const sys_command_t *commands, uint32_t num_commands)
 {
     sys_commands_t cmd = {
          .commands = commands,
@@ -1055,7 +1055,7 @@ void system_output_help (const sys_command_t *commands, uint32_t num_commands)
     _system_output_help(&cmd, false);
 }
 
-void system_command_help (void)
+FLASHMEM void system_command_help (void)
 {
     _system_output_help(&core_commands, false);
     if(commands_root != &core_commands)
@@ -1081,7 +1081,7 @@ __NOTE:__ Code calling this function needs to provide the command in a writable
 \param line pointer to the command string.
 \returns \a status_code_t enum value; #Status_OK if successfully handled, another relevant status code if not.
 */
-status_code_t system_execute_line (char *line)
+FLASHMEM status_code_t system_execute_line (char *line)
 {
     if(line[1] == '\0') {
         grbl.report.help_message();
@@ -1167,7 +1167,7 @@ Clears the tool length offset (TLO) when linear axis or X- or Z-axis is homed in
 Typically called on the grbl.on_homing complete event.
 \param id a \a axes_signals_t holding the axis flags that homed status was changed for.
 */
-void system_clear_tlo_reference (axes_signals_t homing_cycle)
+FLASHMEM void system_clear_tlo_reference (axes_signals_t homing_cycle)
 {
     plane_t plane;
 
@@ -1190,7 +1190,7 @@ If configured waits for the planner buffer to empty then fires the
 grbl.on_wco_changed event and sets the #Report_WCO flag to add
 the WCO report element to the next status report.
 */
-void system_flag_wco_change (void)
+FLASHMEM void system_flag_wco_change (void)
 {
     if(settings.status_report.sync_on_wco_change)
         protocol_buffer_synchronize();
@@ -1226,7 +1226,7 @@ void system_convert_array_steps_to_mpos (float *position, int32_t *steps)
 \param tolerance as the allowed radius the current position has to be within.
 \returns \a false if tolerance is 0 or position is outside the allowed radius, otherwise \a true.
 */
-bool system_xy_at_fixture (coord_system_id_t id, float tolerance)
+FLASHMEM bool system_xy_at_fixture (coord_system_id_t id, float tolerance)
 {
     bool ok = false;
 
@@ -1249,10 +1249,14 @@ void system_raise_alarm (alarm_code_t alarm)
     if(state_get() == STATE_HOMING && !(sys.rt_exec_state & EXEC_RESET))
         system_set_exec_alarm(alarm);
     else if(sys.alarm != alarm) {
-        sys.alarm = alarm;
-        sys.blocking_event = alarm_is_critical(sys.alarm);
-        state_set(alarm == Alarm_EStop ? STATE_ESTOP : STATE_ALARM);
-        if(sys.driver_started || sys.alarm == Alarm_SelftestFailed)
-            grbl.report.alarm_message(alarm);
+        if(!(state_get() == STATE_ESTOP || sys.blocking_event)) {
+            if((sys.alarm = alarm) == sys.alarm_pending)
+                sys.alarm_pending = Alarm_None;
+            sys.blocking_event = alarm_is_critical(sys.alarm);
+            state_set(alarm == Alarm_EStop ? STATE_ESTOP : STATE_ALARM);
+            if(sys.driver_started || sys.alarm == Alarm_SelftestFailed)
+                grbl.report.alarm_message(alarm);
+        } else
+            sys.alarm_pending = alarm;
     }
 }
diff --git a/system.h b/system.h
index af17f5a..fbfa3d6 100644
--- a/system.h
+++ b/system.h
@@ -240,7 +240,6 @@ typedef struct system {
     bool reset_pending;                     //!< Set when reset processing is underway.
     bool blocking_event;                    //!< Set when a blocking event that requires reset to clear is active.
     volatile bool steppers_deenergize;      //!< Set to true to deenergize stepperes
-    alarm_code_t alarm_pending;             //!< Delayed alarm, currently used for probe protection
     volatile system_flags_t flags;                   //!< Assorted state flags
     step_control_t step_control;            //!< Governs the step segment generator depending on system state.
     axes_signals_t homing_axis_lock;        //!< Locks axes when limits engage. Used as an axis motion mask in the stepper ISR.
@@ -269,6 +268,7 @@ typedef struct system {
 //!  @name The following variables are not cleared upon soft reset, do NOT move. alarm must be first!
 //@{
     alarm_code_t alarm;                     //!< Current alarm, only valid if system state is STATE_ALARM.
+    alarm_code_t alarm_pending;             //!< Delayed alarm, currently used for probe protection
     bool cold_start;                        //!< Set to true on boot, is false on subsequent soft resets.
     bool ioinit_pending;
     bool driver_started;                    //!< Set to true when driver initialization is completed.
diff --git a/tool_change.c b/tool_change.c
index df6ac38..e125ff9 100644
--- a/tool_change.c
+++ b/tool_change.c
@@ -53,7 +53,7 @@ static on_probe_completed_ptr on_probe_completed;
 static on_wco_saved_ptr on_wco_saved;
 
 // Clear tool length offset on homing
-static void onHomingComplete (axes_signals_t homing_cycle, bool success)
+FLASHMEM static void onHomingComplete (axes_signals_t homing_cycle, bool success)
 {
     if(on_homing_completed)
         on_homing_completed(homing_cycle, success);
@@ -62,7 +62,7 @@ static void onHomingComplete (axes_signals_t homing_cycle, bool success)
         system_clear_tlo_reference(homing_cycle);
 }
 
-static void onWcoSaved (coord_system_id_t id, coord_system_data_t *data)
+FLASHMEM static void onWcoSaved (coord_system_id_t id, coord_system_data_t *data)
 {
     if(on_wco_saved)
         on_wco_saved(id, data);
@@ -73,7 +73,7 @@ static void onWcoSaved (coord_system_id_t id, coord_system_data_t *data)
 
 // Set tool offset on successful $TPW probe, prompt for retry on failure.
 // Called via probe completed event.
-static void onProbeCompleted (void)
+FLASHMEM static void onProbeCompleted (void)
 {
     if(!sys.flags.probe_succeeded)
         grbl.report.feedback_message(Message_ProbeFailedRetry);
@@ -83,7 +83,7 @@ static void onProbeCompleted (void)
 }
 
 // Restore HAL pointers on completion or reset.
-static void change_completed (void)
+FLASHMEM static void change_completed (void)
 {
     if(enqueue_realtime_command) {
         while(spin_lock);
@@ -117,7 +117,7 @@ static void change_completed (void)
 
 // Reset claimed HAL entry points and restore previous tool if needed on soft restart.
 // Called from EXEC_RESET and EXEC_STOP handlers (via HAL).
-static void reset (void)
+FLASHMEM static void reset (void)
 {
     if(next_tool) { //TODO: move to gc_xxx() function?
         // Restore previous tool if reset is during change
@@ -137,7 +137,7 @@ static void reset (void)
 }
 
 // Restore coolant and spindle status, return controlled point to original position.
-static bool restore (void)
+FLASHMEM static bool restore (void)
 {
     bool ok;
     plan_line_data_t plan_data;
@@ -181,7 +181,7 @@ static bool restore (void)
     return !ABORTED;
 }
 
-static bool go_linear_home (plan_line_data_t *pl_data)
+FLASHMEM static bool go_linear_home (plan_line_data_t *pl_data)
 {
     system_convert_array_steps_to_mpos(target.values, sys.position);
 
@@ -196,7 +196,7 @@ static bool go_linear_home (plan_line_data_t *pl_data)
 }
 #if COMPATIBILITY_LEVEL <= 1
 
-static bool go_toolsetter (plan_line_data_t *pl_data)
+FLASHMEM static bool go_toolsetter (plan_line_data_t *pl_data)
 {
     coord_system_data_t g59_3_offset;
 
@@ -228,14 +228,14 @@ static bool go_toolsetter (plan_line_data_t *pl_data)
 
 // Issue warning on cycle start event if touch off by $TPW is pending.
 // Used in Manual and Manual_G59_3 modes ($341=1 or $341=2). Called from the foreground process.
-static void execute_warning (void *data)
+FLASHMEM static void execute_warning (void *data)
 {
     grbl.report.feedback_message(Message_ExecuteTPW);
 }
 
 // Execute restore position after tool change, either back to original or to toolsetter for touch off.
 // Used when G30 position is used for changing the tool. Called from the foreground process.
-static void execute_return_from_g30 (void *data)
+FLASHMEM static void execute_return_from_g30 (void *data)
 {
     bool ok;
     plan_line_data_t plan_data;
@@ -267,7 +267,7 @@ static void execute_return_from_g30 (void *data)
 
 // Execute restore position after touch off (on cycle start event).
 // Used in Manual and Manual_G59_3 modes ($341=1 or $341=2). Called from the foreground process.
-static void execute_restore (void *data)
+FLASHMEM static void execute_restore (void *data)
 {
     // Get current position.
     system_convert_array_steps_to_mpos(target.values, sys.position);
@@ -283,7 +283,7 @@ static void execute_restore (void *data)
 }
 
 // Set and limit probe travel to be within machine limits.
-static void set_probe_target (coord_data_t *target, uint8_t axis)
+FLASHMEM static void set_probe_target (coord_data_t *target, uint8_t axis)
 {
     target->values[axis] -= settings.tool_change.probing_distance;
 
@@ -293,7 +293,7 @@ static void set_probe_target (coord_data_t *target, uint8_t axis)
 
 // Execute touch off on cycle start event from @ G59.3 position.
 // Used in SemiAutomatic mode ($341=3) only. Called from the foreground process.
-static void execute_probe (void *data)
+FLASHMEM static void execute_probe (void *data)
 {
 #if COMPATIBILITY_LEVEL <= 1
     bool ok;
@@ -428,7 +428,7 @@ ISR_CODE static void ISR_FUNC(on_toolchange_ack)(void)
 }
 
 // Set next and/or current tool. Called by gcode.c on on a Tn or M61 command (via HAL).
-static void onToolSelect (tool_data_t *tool, bool next)
+FLASHMEM static void onToolSelect (tool_data_t *tool, bool next)
 {
     next_tool = tool;
 
@@ -440,7 +440,7 @@ static void onToolSelect (tool_data_t *tool, bool next)
 }
 
 // Start a tool change sequence. Called by gcode.c on a M6 command (via HAL).
-static status_code_t tool_change (parser_state_t *parser_state)
+FLASHMEM static status_code_t tool_change (parser_state_t *parser_state)
 {
     if(next_tool == NULL)
         return Status_GCodeToolError;
@@ -562,7 +562,7 @@ static status_code_t tool_change (parser_state_t *parser_state)
 
 // Claim HAL tool change entry points and clear current tool offsets.
 // TODO: change to survive a warm reset?
-void tc_init (void)
+FLASHMEM void tc_init (void)
 {
     static bool on_homing_subscribed = false;
 
@@ -603,7 +603,7 @@ void tc_init (void)
 // Perform a probe cycle: set tool length offset and restart job if successful.
 // Note: tool length offset is set by the onProbeCompleted event handler.
 // Called by the $TPW system command.
-status_code_t tc_probe_workpiece (void)
+FLASHMEM status_code_t tc_probe_workpiece (void)
 {
     if(!(settings.tool_change.mode == ToolChange_Manual || settings.tool_change.mode == ToolChange_Manual_G59_3) || enqueue_realtime_command == NULL)
         return Status_InvalidStatement;
diff --git a/vfs.c b/vfs.c
index 48f39bd..9f8c2ea 100644
--- a/vfs.c
+++ b/vfs.c
@@ -178,7 +178,7 @@ static char *fs_getcwd (char *buf, size_t size)
     return "/";
 }
 
-static const vfs_t fs_null = {
+PROGMEM static const vfs_t fs_null = {
     .fopen = fs_open,
     .fclose = fs_close,
     .fread = fs_read,