diff --git a/CMakeLists.txt b/CMakeLists.txt
index cfb1bf6..f0340c4 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -16,6 +16,7 @@ target_sources(grbl INTERFACE
  ${CMAKE_CURRENT_LIST_DIR}/nuts_bolts.c
  ${CMAKE_CURRENT_LIST_DIR}/override.c
  ${CMAKE_CURRENT_LIST_DIR}/planner.c
+ ${CMAKE_CURRENT_LIST_DIR}/probe.c
  ${CMAKE_CURRENT_LIST_DIR}/protocol.c
  ${CMAKE_CURRENT_LIST_DIR}/report.c
  ${CMAKE_CURRENT_LIST_DIR}/settings.c
diff --git a/README.md b/README.md
index 7e52e18..cd15e98 100644
--- a/README.md
+++ b/README.md
@@ -1,6 +1,6 @@
 ## grblHAL ##
 
-Latest build date is 20251011, see the [changelog](changelog.md) for details.  
+Latest build date is 20251016, 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.  
diff --git a/changelog.md b/changelog.md
index f1d8d6f..ac0dfd7 100644
--- a/changelog.md
+++ b/changelog.md
@@ -1,5 +1,28 @@
 ## grblHAL changelog
 
+<a name="20251016">Build 20251016
+
+Core:
+
+* Refactored assignment of of auxiliary I/O for "standard" inputs and outputs to make it more robust and easier to follow for developers.
+
+* Moved probe signal handling from drivers to the core, improved handling of probe disconnected signal.
+
+> [!NOTE]
+> The changes above are quite large, please verify probe operation after installation.
+
+Drivers:
+
+* All: updated for move of probe signal handling to the core.
+
+* RP2040: fixed "leak" of stepper signals between axes when plasma plugin is enabled, affects the PicoCNC board.
+
+* ESP32: fix for compilation failure for boards using the SDIO interface for SD card interface.
+
+* STM32F4xx: SuperLongBoards, now returns unambigous values for `_probe_state` and `_toolsetter_state` system parameters.
+
+---
+
 <a name="20251015">20251015
 
 Drivers:
@@ -8,7 +31,7 @@ Drivers:
 
 Plugins:
 
-* Keypad, macros: "hardened" code to avoid hardfault when macros settings data is bad.
+* Keypad, macros: "hardened" code to avoid hardfault when macros settings data are bad.
 
 ---
 
diff --git a/config.h b/config.h
index ef63f9b..0026e53 100644
--- a/config.h
+++ b/config.h
@@ -736,7 +736,7 @@ If set to \ref Off or 0 the `|DTG:` distance-to-go element is not included in th
 \internal Bit 13 in settings.status_report.
 */
 #if !defined DEFAULT_REPORT_DISTANCE_TO_GO || defined __DOXYGEN__
-#define DEFAULT_REPORT_DISTANCE_TO_GO On // Default on. Set to \ref Off or 0 to disable.
+#define DEFAULT_REPORT_DISTANCE_TO_GO Off // Default off. Set to \ref On or 1 to enable.
 #endif
 
 ///@}
@@ -933,14 +933,13 @@ Enable this setting to feedrate override over a soft reset.
 ///@}
 
 
-
 // Control signals settings (Group_ControlSignals)
 
 #ifndef __DOXYGEN__ // For now do not include in documentation
 
 /*! @name Control signals bit definitions and mask.
 
-__NOTE:__ these definitions are only referenced in this file. Do __NOT__ change!
+__NOTE:__  Do __NOT__ change! Definitions MUST match #control_signals_t struct bit order.
 */
 ///@{
 #define SIGNALS_RESET_BIT (1<<0)
@@ -952,6 +951,12 @@ __NOTE:__ these definitions are only referenced in this file. Do __NOT__ change!
 #define SIGNALS_ESTOP_BIT (1<<6)
 #define SIGNALS_PROBE_CONNECTED_BIT (1<<7)
 #define SIGNALS_MOTOR_FAULT_BIT (1<<8)
+#define SIGNALS_MOTOR_WARNING_BIT (1<<9)
+#define SIGNALS_LIMITS_OVERRIDE_BIT (1<<10)
+#define SIGNALS_SINGLE_BLOCK_BIT (1<<11)
+#define SIGNALS_TLS_OVERTRAVEL_BIT (1<<12)
+#define SIGNALS_PROBE_OVERTRAVEL (1<<13)
+#define SIGNALS_PROBE_TRIGGERED_BIT (1<<14)
 #define SIGNALS_BITMASK (SIGNALS_RESET_BIT|SIGNALS_FEEDHOLD_BIT|SIGNALS_CYCLESTART_BIT|SIGNALS_SAFETYDOOR_BIT|SIGNALS_BLOCKDELETE_BIT|SIGNALS_STOPDISABLE_BIT|SIGNALS_ESTOP_BIT|SIGNALS_PROBE_CONNECTED_BIT|SIGNALS_MOTOR_FAULT_BIT)
 ///@}
 
diff --git a/crossbar.h b/crossbar.h
index 87a5777..9e3fd66 100644
--- a/crossbar.h
+++ b/crossbar.h
@@ -38,7 +38,7 @@ typedef enum {
     Input_MotorWarning,
     Input_LimitsOverride,
     Input_SingleBlock,
-    Input_Unassigned,
+    Input_ToolsetterOvertravel,
     Input_ProbeOvertravel,
     Input_Probe,
 // end control_signals_t sequence
@@ -56,7 +56,6 @@ typedef enum {
     Input_Probe2,
     Input_Probe2Overtravel,
     Input_Toolsetter,
-    Input_ToolsetterOvertravel,
     Input_MPGSelect,
     Input_ModeSelect = Input_MPGSelect, // Deprecated
     Input_LimitX,
@@ -726,22 +725,27 @@ typedef bool (*xbar_set_function_ptr)(struct xbar *pin, pin_function_t function)
 typedef void (*xbar_event_ptr)(bool on);
 typedef bool (*xbar_config_ptr)(struct xbar *pin, xbar_cfg_ptr_t cfg_data, bool persistent);
 
+// MCU port base address and pin number
 typedef struct {
-    pin_function_t function;
-    uint8_t aux_port;
-    pin_irq_mode_t irq_mode;
-    control_signals_t cap;
-    uint8_t pin;
-    void *port;
-    void *input;
+    void *port;     //!< MCU port address (may be NULL).
+    uint8_t pin;    //!< MCU pin number.
+} aux_gpio_t;
+
+typedef struct {
+    pin_function_t function;    //!< Pin function.
+    uint8_t port;               //!< Auxiliary port number, post claimed.
+    pin_irq_mode_t irq_mode;    //!< Required IRQ mode for the input.
+    control_signals_t signal;   //!< Set to the pin the input maps to, 0 if none.
+    aux_gpio_t gpio;            //!< MCU port base address (may be NULL) and pin number.
+    void *input;                //!< Pointer to the driver input array entry for the pin.
+    bool scan;                  //!< true if the pin is to be scanned when control state is requested.
 } aux_ctrl_t;
 
 typedef struct {
-    pin_function_t function;
-    uint8_t aux_port;
-    uint8_t pin;
-    void *port;
-    void *output;
+    pin_function_t function;    //!< Pin function.
+    uint8_t port;               //!< Auxiliary port number, post claimed.
+    aux_gpio_t gpio;            //!< MCU port base address (may be NULL) and pin number.
+    void *output;               //!< Pointer to the driver input array entry for the pin.
 } aux_ctrl_out_t;
 
 typedef struct xbar {
@@ -806,6 +810,11 @@ static inline bool xbar_stepper_state_get (stepper_state_t state, uint8_t axis,
     return bit_istrue(b ? state.details.b.bits : state.details.a.bits, bit(axis));
 }
 
+static inline bool xbar_is_probe_in (pin_function_t fn)
+{
+    return fn == Input_Probe || fn == Input_Probe2 || fn == Input_Toolsetter;
+}
+
 void xbar_set_homing_source (void);
 limit_signals_t xbar_get_homing_source (void);
 limit_signals_t xbar_get_homing_source_from_cycle (axes_signals_t homing_cycle);
diff --git a/grbl.h b/grbl.h
index 4b8c4c3..ec5bb58 100644
--- a/grbl.h
+++ b/grbl.h
@@ -42,7 +42,7 @@
 #else
 #define GRBL_VERSION "1.1f"
 #endif
-#define GRBL_BUILD 20251011
+#define GRBL_BUILD 20251016
 
 #define GRBL_URL "https://github.com/grblHAL"
 
diff --git a/grbllib.c b/grbllib.c
index a35f3db..9f81fab 100644
--- a/grbllib.c
+++ b/grbllib.c
@@ -411,6 +411,9 @@ int grbl_enter (void)
     if(grbl.on_probe_toolsetter == NULL && hal.driver_cap.toolsetter && hal.probe.select)
         grbl.on_probe_toolsetter = onProbeToolsetter;
 
+    if(hal.driver_cap.probe && hal.signals_cap.probe_disconnected)
+        task_run_on_startup(probe_connected_event, hal.control.get_state().probe_disconnected ? NULL : (void *)1);
+
     // Initialization loop upon power-up or a system abort. For the latter, all processes
     // will return to this loop to be cleanly re-initialized.
     while(looping) {
diff --git a/hal.h b/hal.h
index b632b42..69ba0a0 100644
--- a/hal.h
+++ b/hal.h
@@ -373,6 +373,12 @@ __NOTE:__ this function will be called from an interrupt context.
 */
 typedef probe_state_t (*probe_get_state_ptr)(void);
 
+/*! \brief Pointer to function for getting triggered status for a specific probe.
+\param probe_id probe number. 0 - default probe, 1 - toolsetter, ...
+\returns \a true if triggered, \a false otherwise.
+*/
+typedef bool (*probe_is_triggered_ptr)(probe_id_t probe_id);
+
 /*! \brief Pointer to function for setting probe operation mode.
 \param is_probe_away true if probing away from the workpiece, false otherwise. When probing away the signal must be inverted in the probe_get_state_ptr() implementation.
 \param probing true if probe cycle is active, false otherwise.
@@ -381,13 +387,17 @@ typedef void (*probe_configure_ptr)(bool is_probe_away, bool probing);
 
 /*! \brief Pointer to function for selecting probe input.
 \param probe_id probe number. 0 - default probe, 1 - toolsetter, ...
-\returns true if selectetion succeded, false otherwise.
+\returns true if selection succeded, false otherwise.
 */
 typedef bool (*probe_select_ptr)(probe_id_t probe_id);
 
+/*! \brief Pointer to function for getting probe capabilities.
+\param probe_id probe number. 0 - default probe, 1 - toolsetter, ...
+\returns a \a probe_flags_t struct.
+*/
+typedef probe_flags_t (*probe_get_caps_ptr)(probe_id_t probe_id);
+
 /*! \brief Pointer to function for toggling probe connected status.
-
-
 If the driver does not support a probe connected input signal this can be used to implement
 toggling of probe connected status via a #CMD_PROBE_CONNECTED_TOGGLE real time command.
 */
@@ -397,7 +407,9 @@ typedef void (*probe_connected_toggle_ptr)(void);
 typedef struct {
     probe_configure_ptr configure;                  //!< Optional handler for setting probe operation mode.
     probe_get_state_ptr get_state;                  //!< Optional handler for getting probe status. Called from interrupt context.
+    probe_is_triggered_ptr is_triggered;            //!< Optional handler for getting probe triggered status.
     probe_select_ptr select;                        //!< Optional handler for selecting probe to use.
+    probe_get_caps_ptr get_caps;                    //!< Optional handler for getting probe capabilities.
     probe_connected_toggle_ptr connected_toggle;    //!< Optional handler for toggling probe connected status.
 } probe_ptrs_t;
 
diff --git a/ioports.c b/ioports.c
index bde94f9..9617c05 100644
--- a/ioports.c
+++ b/ioports.c
@@ -352,12 +352,12 @@ void ioport_assign_function (aux_ctrl_t *aux_ctrl, pin_function_t *function)
 {
     xbar_t *input;
 
-    if((input = hal.port.get_pin_info(Port_Digital, Port_Input, aux_ctrl->aux_port))) {
+    if((input = hal.port.get_pin_info(Port_Digital, Port_Input, aux_ctrl->port))) {
 
         *function = aux_ctrl->function;
         ports_cfg[Port_DigitalIn].bus.mask &= ~(1 << input->id);
         ports_cfg[Port_DigitalIn].count = ports_cfg[Port_DigitalIn].free = -1;
-        hal.signals_cap.mask |= aux_ctrl->cap.mask;
+        hal.signals_cap.mask |= aux_ctrl->signal.mask;
 
         if(aux_ctrl->function == Input_Probe || xbar_fn_to_signals_mask(aux_ctrl->function).mask)
             setting_remove_elements(Settings_IoPort_InvertIn, ports_cfg[Port_DigitalIn].bus.mask);
@@ -369,7 +369,7 @@ void ioport_assign_out_function (aux_ctrl_out_t *aux_ctrl, pin_function_t *funct
 {
     xbar_t *output;
 
-    if((output = hal.port.get_pin_info(Port_Digital, Port_Output, aux_ctrl->aux_port))) {
+    if((output = hal.port.get_pin_info(Port_Digital, Port_Output, aux_ctrl->port))) {
 
         *function = aux_ctrl->function;
         ports_cfg[Port_DigitalOut].bus.mask &= ~(1UL << output->id);
diff --git a/motion_control.c b/motion_control.c
index 9be0cf3..4bf62ef 100644
--- a/motion_control.c
+++ b/motion_control.c
@@ -409,10 +409,10 @@ static inline float interp (const float a, const float b, const float t)
 }
 
 /**
- * Compute a B�zier curve using the De Casteljau's algorithm (see
+ * Compute a Bezier curve using the De Casteljau's algorithm (see
  * https://en.wikipedia.org/wiki/De_Casteljau's_algorithm), which is
  * easy to code and has good numerical stability (very important,
- * since Arudino works with limited precision real numbers).
+ * since Arduino works with limited precision real numbers).
  */
 static inline float eval_bezier (const float a, const float b, const float c, const float d, const float t)
 {
diff --git a/ngc_params.c b/ngc_params.c
index 3b29094..36cb98d 100644
--- a/ngc_params.c
+++ b/ngc_params.c
@@ -612,40 +612,11 @@ float ngc_named_param_get_by_id (ncg_name_param_id_t id)
      // grblHAL extensions
 
         case NGCParam_probe_state:
-            value = -1.0f;
-            if(hal.probe.get_state /*&& hal.driver_cap.probe*/) {
-
-                probe_state_t probe_state = hal.probe.get_state();
-
-                if((probe_id_t)probe_state.probe_id == Probe_Default)
-                    value = (float)probe_state.triggered;
-                else if(hal.probe.select) {
-
-                    if(hal.probe.select(Probe_Default))
-                        value = (float)hal.probe.get_state().triggered;
-
-                    hal.probe.select((probe_id_t)probe_state.probe_id);
-                }
-            }
+            value = hal.driver_cap.probe && hal.probe.is_triggered ? (float)hal.probe.is_triggered(Probe_Default) : -1.0f;
             break;
 
         case NGCParam_toolsetter_state:
-            value = -1.0f;
-            if(hal.probe.get_state && hal.driver_cap.toolsetter) {
-
-                probe_state_t probe_state = hal.probe.get_state();
-
-                if((probe_id_t)probe_state.probe_id == Probe_Toolsetter)
-                    value = (float)probe_state.triggered;
-                else if(hal.probe.select) {
-
-                    if(hal.probe.select(Probe_Toolsetter))
-                        value = (float)hal.probe.get_state().triggered;
-
-                    hal.probe.select((probe_id_t)probe_state.probe_id);
-                } else
-                    value = (float)probe_state.tls_triggered;
-            }
+            value = hal.driver_cap.toolsetter && hal.probe.is_triggered  ? (float)hal.probe.is_triggered(Probe_Toolsetter) : -1.0f;
             break;
 
         case NGCParam_homed_state:
diff --git a/nuts_bolts.h b/nuts_bolts.h
index 150eec3..aa62747 100644
--- a/nuts_bolts.h
+++ b/nuts_bolts.h
@@ -246,6 +246,31 @@ typedef struct {
 
 #pragma pack(pop)
 
+// NOTE: the pin_function_t enum must be kept in sync with any changes!
+typedef union {
+    uint16_t bits;
+    uint16_t mask;
+    uint16_t value;
+    struct {
+        uint16_t reset                :1,
+                 feed_hold            :1,
+                 cycle_start          :1,
+                 safety_door_ajar     :1,
+                 block_delete         :1,
+                 stop_disable         :1, //! M1
+                 e_stop               :1,
+                 probe_disconnected   :1,
+                 motor_fault          :1,
+                 motor_warning        :1,
+                 limits_override      :1,
+                 single_block         :1,
+                 tls_overtravel       :1, //! used for probe (toolsetter) protection
+                 probe_overtravel     :1, //! used for probe protection
+                 probe_triggered      :1, //! used for probe protection
+                 deasserted           :1; //! this flag is set if signals are deasserted.
+    };
+} control_signals_t;
+
 typedef enum {
     DelayMode_Dwell = 0,
     DelayMode_SysSuspend
diff --git a/pin_bits_masks.h b/pin_bits_masks.h
index 1266e04..74ef1ab 100644
--- a/pin_bits_masks.h
+++ b/pin_bits_masks.h
@@ -111,110 +111,90 @@
 #define CONTROL_ENABLE 0
 #endif
 
+#define a_cap(pin) .cap.pin
+
+#if defined(ESP_PLATFORM) || defined(RP2040) || defined(__IMXRT1062__)
+#define add_aux_input(fn, aux, irq, signal_bit) { .function = fn, .irq_mode = irq, .signal.value = signal_bit, .port = IOPORT_UNASSIGNED, .gpio.pin = aux##_PIN },
+#else
+#define add_aux_input(fn, aux, irq, signal_bit) { .function = fn, .irq_mode = irq, .signal.value = signal_bit, .port = IOPORT_UNASSIGNED, .gpio.port = (void *)aux##_PORT, .gpio.pin = aux##_PIN },
+#endif
+#if defined(__IMXRT1062__) || defined(ESP_PLATFORM)
+#define add_aux_output(fn, aux) { .function = fn, .port = IOPORT_UNASSIGNED, .gpio.pin = aux##_PIN },
+#else
+#define add_aux_output(fn, aux) { .function = fn, .port = IOPORT_UNASSIGNED, .gpio.port = (void *)aux##_PORT, .gpio.pin = aux##_PIN },
+#endif
+#define add_aux_input_scan(fn, irq, signal_bit) { .function = fn, .irq_mode = irq, .signal.value = signal_bit, .port = IOPORT_UNASSIGNED, .gpio.pin = 0xFF, .scan = On },
+#define add_aux_input_no_signal(fn, irq) { .function = fn, .irq_mode = irq, .port = IOPORT_UNASSIGNED, .gpio.pin = 0xFE },
+#define add_aux_output_exp(fn, aux) { .function = fn, .port = IOPORT_UNASSIGNED, .gpio.port = (void *)aux##_PORT, .gpio.pin = aux##_PIN },
+
 static aux_ctrl_t aux_ctrl[] = {
 // The following pins are bound explicitly to aux input pins.
-#if (CONTROL_ENABLE & CONTROL_ESTOP) && defined(RESET_PIN)
- #ifndef RESET_PORT
-  #define RESET_PORT 0
- #endif
-    { .function = Input_EStop,           .aux_port = IOPORT_UNASSIGNED, .irq_mode = IRQ_Mode_RisingFalling, .cap = { .e_stop = On }, .pin = RESET_PIN, .port = (void *)RESET_PORT },
-#elif (CONTROL_ENABLE & CONTROL_RESET) && defined(RESET_PIN)
- #ifndef RESET_PORT
-  #define RESET_PORT 0
- #endif
-    { .function = Input_Reset,           .aux_port = IOPORT_UNASSIGNED, .irq_mode = IRQ_Mode_RisingFalling, .cap = { .reset = On }, .pin = RESET_PIN, .port = (void *)RESET_PORT },
+#ifdef RESET_PIN
+  #if (CONTROL_ENABLE & CONTROL_ESTOP)
+    add_aux_input(Input_EStop, RESET, IRQ_Mode_RisingFalling, SIGNALS_ESTOP_BIT)
+  #elif (CONTROL_ENABLE & CONTROL_RESET)
+    add_aux_input(Input_Reset, RESET, IRQ_Mode_RisingFalling, SIGNALS_RESET_BIT)
+  #endif
 #endif
 #if (CONTROL_ENABLE & CONTROL_FEED_HOLD) && defined(FEED_HOLD_PIN)
- #ifndef FEED_HOLD_PORT
-  #define FEED_HOLD_PORT 0
- #endif
-    { .function = Input_FeedHold,        .aux_port = IOPORT_UNASSIGNED, .irq_mode = IRQ_Mode_RisingFalling, .cap = { .feed_hold = On }, .pin = FEED_HOLD_PIN, .port = (void *)FEED_HOLD_PORT },
+    add_aux_input(Input_FeedHold, FEED_HOLD, IRQ_Mode_RisingFalling, SIGNALS_FEEDHOLD_BIT)
 #endif
 #if (CONTROL_ENABLE & CONTROL_CYCLE_START) && defined(CYCLE_START_PIN)
- #ifndef CYCLE_START_PORT
-  #define CYCLE_START_PORT 0
- #endif
-    { .function = Input_CycleStart,      .aux_port = IOPORT_UNASSIGNED, .irq_mode = IRQ_Mode_RisingFalling, .cap = { .cycle_start = On }, .pin = CYCLE_START_PIN, .port = (void *)CYCLE_START_PORT },
+    add_aux_input(Input_CycleStart, CYCLE_START, IRQ_Mode_RisingFalling, SIGNALS_CYCLESTART_BIT)
 #endif
 #if SAFETY_DOOR_ENABLE && defined(SAFETY_DOOR_PIN)
- #ifndef SAFETY_DOOR_PORT
-  #define SAFETY_DOOR_PORT 0
- #endif
-    { .function = Input_SafetyDoor,      .aux_port = IOPORT_UNASSIGNED, .irq_mode = IRQ_Mode_RisingFalling, .cap = { .safety_door_ajar = On }, .pin = SAFETY_DOOR_PIN, .port = (void *)SAFETY_DOOR_PORT },
+    add_aux_input(Input_SafetyDoor, SAFETY_DOOR, IRQ_Mode_RisingFalling, SIGNALS_SAFETYDOOR_BIT)
 #endif
 #if MOTOR_FAULT_ENABLE && defined(MOTOR_FAULT_PIN)
- #ifndef MOTOR_FAULT_PORT
-  #define MOTOR_FAULT_PORT 0
- #endif
-    { .function = Input_MotorFault,      .aux_port = IOPORT_UNASSIGNED, .irq_mode = IRQ_Mode_RisingFalling, .cap = { .motor_fault = On }, .pin = MOTOR_FAULT_PIN, .port = (void *)MOTOR_FAULT_PORT },
+    add_aux_input(Input_MotorFault, MOTOR_FAULT, IRQ_Mode_RisingFalling, SIGNALS_MOTOR_FAULT_BIT)
 #endif
 #if MOTOR_WARNING_ENABLE && defined(MOTOR_WARNING_PIN)
- #ifndef MOTOR_WARNING_PORT
-  #define MOTOR_WARNING_PORT 0
- #endif
-    { .function = Input_MotorWarning,    .aux_port = IOPORT_UNASSIGNED, .irq_mode = IRQ_Mode_RisingFalling, .cap = { .motor_fault = On }, .pin = MOTOR_WARNING_PIN, .port = (void *)MOTOR_WARNING_PORT },
+    add_aux_input(Input_MotorWarning, MOTOR_WARNING, IRQ_Mode_RisingFalling, SIGNALS_MOTOR_WARNING_BIT)
 #endif
-
 #if I2C_STROBE_ENABLE && defined(I2C_STROBE_PIN)
- #ifndef I2C_STROBE_PORT
-  #define I2C_STROBE_PORT 0
- #endif
-    { .function = Input_I2CStrobe,       .aux_port = IOPORT_UNASSIGNED, .irq_mode = IRQ_Mode_Change,        .cap = { .value = 0 }, .pin = I2C_STROBE_PIN, .port = (void *)I2C_STROBE_PORT },
+    add_aux_input(Input_I2CStrobe, I2C_STROBE, IRQ_Mode_Change, 0)
 #endif
 #if MPG_ENABLE == 1 && defined(MPG_MODE_PIN)
- #ifndef MPG_MODE_PORT
-  #define MPG_MODE_PORT 0
- #endif
-     { .function = Input_MPGSelect,      .aux_port = IOPORT_UNASSIGNED, .irq_mode = IRQ_Mode_Change,        .cap = { .value = 0 }, .pin = MPG_MODE_PIN, .port = (void *)MPG_MODE_PORT },
+    add_aux_input(Input_MPGSelect, MPG_MODE, IRQ_Mode_RisingFalling, 0)
 #endif
 #if QEI_SELECT_ENABLE && defined(QEI_SELECT_PIN)
- #ifndef QEI_SELECT_PORT
-  #define QEI_SELECT_PORT 0
- #endif
-    { .function = Input_QEI_Select,      .aux_port = IOPORT_UNASSIGNED, .irq_mode = IRQ_Mode_RisingFalling, .cap = { .value = 0 }, .pin = QEI_SELECT_PIN, .port = (void *)QEI_SELECT_PORT },
+    add_aux_input(Input_QEI_Select, QEI_SELECT, IRQ_Mode_RisingFalling, 0)
 #endif
-
 // Probe pins can be bound explicitly and can be "degraded" to not interrupt capable.
 #if PROBE_ENABLE && defined(PROBE_PIN)
- #ifndef PROBE_PORT
-  #define PROBE_PORT 0
- #endif
-    { .function = Input_Probe,           .aux_port = IOPORT_UNASSIGNED, .irq_mode = IRQ_Mode_RisingFalling, .cap = { .value = 0 }, .pin = PROBE_PIN, .port = (void *)PROBE_PORT },
+    add_aux_input(Input_Probe, PROBE, IRQ_Mode_RisingFalling, 0)
 #endif
 #if PROBE2_ENABLE && defined(PROBE2_PIN)
- #ifndef PROBE2_PORT
-  #define PROBE2_PORT 0
- #endif
-    { .function = Input_Probe2,          .aux_port = IOPORT_UNASSIGNED, .irq_mode = IRQ_Mode_RisingFalling, .cap = { .value = 0 }, .pin = PROBE2_PIN, .port = (void *)PROBE2_PORT },
+    add_aux_input(Input_Probe2, PROBE2, IRQ_Mode_RisingFalling, 0)
 #endif
 #if TOOLSETTER_ENABLE && defined(TOOLSETTER_PIN)
- #ifndef TOOLSETTER_PORT
-  #define TOOLSETTER_PORT 0
- #endif
-    { .function = Input_Toolsetter,      .aux_port = IOPORT_UNASSIGNED, .irq_mode = IRQ_Mode_RisingFalling, .cap = { .value = 0 }, .pin = TOOLSETTER_PIN, .port = (void *)TOOLSETTER_PORT },
+    add_aux_input(Input_Toolsetter, TOOLSETTER, IRQ_Mode_RisingFalling, 0)
 #endif
 
 // The following pins are allocated from remaining aux inputs pool
 #if TOOLSETTER_ENABLE && !defined(TOOLSETTER_PIN)
-    { .function = Input_Toolsetter,      .aux_port = IOPORT_UNASSIGNED, .irq_mode = IRQ_Mode_RisingFalling, .cap = { .value = 0 }, .pin = 0xFE, .port = NULL },
+    add_aux_input_no_signal(Input_Toolsetter, IRQ_Mode_RisingFalling)
 #endif
 #if PROBE2_ENABLE && !defined(PROBE2_PIN)
-    { .function = Input_Probe2,          .aux_port = IOPORT_UNASSIGNED, .irq_mode = IRQ_Mode_RisingFalling, .cap = { .value = 0 }, .pin = 0xFE, .port = NULL },
+    add_aux_input_no_signal(Input_Probe2, IRQ_Mode_RisingFalling)
+#endif
+#if TLS_OVERTRAVEL_ENABLE
+    add_aux_input_scan(Input_ToolsetterOvertravel, IRQ_Mode_Change, SIGNALS_TLS_OVERTRAVEL_BIT)
 #endif
 #if LIMITS_OVERRIDE_ENABLE
-    { .function = Input_LimitsOverride,  .aux_port = IOPORT_UNASSIGNED, .irq_mode = IRQ_Mode_None,          .cap = { .limits_override = On }, .pin = 0xFF, .port = NULL },
+    add_aux_input_scan(Input_LimitsOverride, IRQ_Mode_Change, SIGNALS_LIMITS_OVERRIDE_BIT)
 #endif
 #if STOP_DISABLE_ENABLE
-    { .function = Input_StopDisable,     .aux_port = IOPORT_UNASSIGNED, .irq_mode = IRQ_Mode_Change,        .cap = { .stop_disable = On }, .pin = 0xFF, .port = NULL },
+    add_aux_input_scan(Input_StopDisable, IRQ_Mode_Change, SIGNALS_STOPDISABLE_BIT)
 #endif
 #if BLOCK_DELETE_ENABLE
-    { .function = Input_BlockDelete,     .aux_port = IOPORT_UNASSIGNED, .irq_mode = IRQ_Mode_Change,        .cap = { .block_delete = On }, .pin = 0xFF, .port = NULL },
+    add_aux_input_scan(Input_BlockDelete, IRQ_Mode_Change, SIGNALS_BLOCKDELETE_BIT)
 #endif
 #if SINGLE_BLOCK_ENABLE
-    { .function = Input_SingleBlock,     .aux_port = IOPORT_UNASSIGNED, .irq_mode = IRQ_Mode_Change,        .cap = { .single_block = On }, .pin = 0xFF, .port = NULL },
+    add_aux_input_scan(Input_SingleBlock, IRQ_Mode_Change, SIGNALS_SINGLE_BLOCK_BIT)
 #endif
 #if PROBE_DISCONNECT_ENABLE
-    { .function = Input_ProbeDisconnect, .aux_port = IOPORT_UNASSIGNED, .irq_mode = IRQ_Mode_RisingFalling, .cap = { .probe_disconnected = On }, .pin = 0xFF, .port = NULL },
+    add_aux_input_scan(Input_ProbeDisconnect, IRQ_Mode_Change, SIGNALS_PROBE_CONNECTED_BIT)
 #endif
 };
 
@@ -225,14 +205,14 @@ static inline bool aux_ctrl_is_probe (pin_function_t function)
 
 #ifdef STM32_PLATFORM
 
-static inline aux_ctrl_t *aux_ctrl_get_fn (void *port, uint8_t pin)
+static inline aux_ctrl_t *aux_ctrl_get_fn (aux_gpio_t gpio)
 {
     aux_ctrl_t *ctrl_pin = NULL;
 
     if(sizeof(aux_ctrl) / sizeof(aux_ctrl_t)) {
         uint_fast8_t idx;
-        for(idx = 0; ctrl_pin == NULL && aux_ctrl[idx].pin != 0xFF && idx < sizeof(aux_ctrl) / sizeof(aux_ctrl_t); idx++) {
-            if(aux_ctrl[idx].pin == pin && aux_ctrl[idx].port == port)
+        for(idx = 0; ctrl_pin == NULL && aux_ctrl[idx].gpio.pin != 0xFF && idx < sizeof(aux_ctrl) / sizeof(aux_ctrl_t); idx++) {
+            if(aux_ctrl[idx].gpio.pin == gpio.pin && aux_ctrl[idx].gpio.port == gpio.port)
                 ctrl_pin = &aux_ctrl[idx];
         }
     }
@@ -240,9 +220,27 @@ static inline aux_ctrl_t *aux_ctrl_get_fn (void *port, uint8_t pin)
     return ctrl_pin;
 }
 
-#endif
+#endif // STM32_PLATFORM
 
-static inline aux_ctrl_t *aux_ctrl_remap_explicit (void *port, uint8_t pin, uint8_t aux_port, void *input)
+static inline xbar_t *aux_ctrl_claim_port (aux_ctrl_t *aux_ctrl)
+{
+    xbar_t *pin = NULL;
+
+    if(aux_ctrl) {
+        if(aux_ctrl->port != IOPORT_UNASSIGNED && (pin = ioport_claim(Port_Digital, Port_Input, &aux_ctrl->port, NULL))) {
+
+            aux_ctrl->gpio.port = pin->port;
+            aux_ctrl->gpio.pin = pin->pin;
+
+            ioport_set_function(pin, aux_ctrl->function, &aux_ctrl->signal);
+        } else
+            aux_ctrl->port = IOPORT_UNASSIGNED;
+    }
+
+    return pin;
+}
+
+static inline aux_ctrl_t *aux_ctrl_remap_explicit (aux_gpio_t gpio, uint8_t port, void *input)
 {
     aux_ctrl_t *ctrl_pin = NULL;
 
@@ -250,10 +248,10 @@ static inline aux_ctrl_t *aux_ctrl_remap_explicit (void *port, uint8_t pin, uint
 
         uint_fast8_t idx;
 
-        for(idx = 0; ctrl_pin == NULL && idx < sizeof(aux_ctrl) / sizeof(aux_ctrl_t) && aux_ctrl[idx].pin != 0xFF; idx++) {
-            if(aux_ctrl[idx].pin == pin && aux_ctrl[idx].port == port) {
+        for(idx = 0; ctrl_pin == NULL && idx < sizeof(aux_ctrl) / sizeof(aux_ctrl_t) && aux_ctrl[idx].gpio.pin != 0xFF; idx++) {
+            if(aux_ctrl[idx].gpio.pin == gpio.pin && aux_ctrl[idx].gpio.port == gpio.port) {
                 ctrl_pin = &aux_ctrl[idx];
-                ctrl_pin->aux_port = aux_port;
+                ctrl_pin->port = port;
                 ctrl_pin->input = input;
                 break;
             }
@@ -263,14 +261,14 @@ static inline aux_ctrl_t *aux_ctrl_remap_explicit (void *port, uint8_t pin, uint
     return ctrl_pin;
 }
 
-static inline aux_ctrl_t *aux_ctrl_get_pin (uint8_t aux_port)
+static inline aux_ctrl_t *aux_ctrl_in_get (uint8_t port)
 {
     aux_ctrl_t *ctrl_pin = NULL;
 
     uint_fast8_t idx = sizeof(aux_ctrl) / sizeof(aux_ctrl_t);
 
     if(idx) do {
-        if(aux_ctrl[--idx].aux_port == aux_port)
+        if(aux_ctrl[--idx].port == port)
             ctrl_pin = &aux_ctrl[idx];
     } while(idx && ctrl_pin == NULL);
 
@@ -282,12 +280,12 @@ static inline void aux_ctrl_irq_enable (settings_t *settings, ioport_interrupt_c
     uint_fast8_t idx = sizeof(aux_ctrl) / sizeof(aux_ctrl_t);
 
     if(idx) do {
-        if(aux_ctrl[--idx].aux_port != 0xFF && aux_ctrl[idx].irq_mode != IRQ_Mode_None) {
+        if(aux_ctrl[--idx].port != 0xFF && aux_ctrl[idx].irq_mode != IRQ_Mode_None) {
             if(!aux_ctrl_is_probe(aux_ctrl[idx].function)) {
                 pin_irq_mode_t irq_mode;
                 if((irq_mode = aux_ctrl[idx].irq_mode) & IRQ_Mode_RisingFalling)
-                    irq_mode = (settings->control_invert.mask & aux_ctrl[idx].cap.mask) ? IRQ_Mode_Falling : IRQ_Mode_Rising;
-                hal.port.register_interrupt_handler(aux_ctrl[idx].aux_port, irq_mode, aux_irq_handler);
+                    irq_mode = (settings->control_invert.mask & aux_ctrl[idx].signal.mask) ? IRQ_Mode_Falling : IRQ_Mode_Rising;
+                hal.port.register_interrupt_handler(aux_ctrl[idx].port, irq_mode, aux_irq_handler);
             }
         }
     } while(idx);
@@ -295,45 +293,56 @@ static inline void aux_ctrl_irq_enable (settings_t *settings, ioport_interrupt_c
 
 typedef bool (*aux_claim_explicit_ptr)(aux_ctrl_t *aux_ctrl);
 
-static bool aux_ctrl_claim_port (xbar_t *properties, uint8_t port, void *data)
+// Default/internal functions for aux_ctrl_claim_ports()
+
+static bool __claim_in_port (xbar_t *properties, uint8_t port, void *data)
 {
     if(ioport_claim(Port_Digital, Port_Input, &port, NULL)) {
-        ((aux_ctrl_t *)data)->aux_port = port;
-        ioport_set_function(properties, ((aux_ctrl_t *)data)->function, &((aux_ctrl_t *)data)->cap);
+        ((aux_ctrl_t *)data)->port = port;
+        ((aux_ctrl_t *)data)->gpio.port = properties->port;
+        ((aux_ctrl_t *)data)->gpio.pin = properties->pin;
+        ioport_set_function(properties, ((aux_ctrl_t *)data)->function, &((aux_ctrl_t *)data)->signal);
     }
 
-    return ((aux_ctrl_t *)data)->aux_port != IOPORT_UNASSIGNED;
+    return ((aux_ctrl_t *)data)->port != IOPORT_UNASSIGNED;
 }
 
-static bool aux_ctrl_find_port (xbar_t *properties, uint8_t port, void *data)
+static bool __find_in_port (xbar_t *properties, uint8_t port, void *data)
 {
-    ((aux_ctrl_t *)data)->aux_port = port;
+    ((aux_ctrl_t *)data)->port = port;
 
     return true;
 }
 
+// --
+
 static inline void aux_ctrl_claim_ports (aux_claim_explicit_ptr aux_claim_explicit, ioports_enumerate_callback_ptr aux_claim)
 {
     uint_fast8_t idx;
 
     if(aux_claim == NULL)
-        aux_claim = aux_ctrl_claim_port;
+        aux_claim = __claim_in_port;
 
     for(idx = 0; idx < sizeof(aux_ctrl) / sizeof(aux_ctrl_t); idx++) {
 
-        pin_cap_t cap = { .irq_mode = aux_ctrl[idx].irq_mode, .claimable = On };
-
-        if(aux_ctrl[idx].pin == 0xFE) // Toolsetter and Probe2
-            ioports_enumerate(Port_Digital, Port_Input, cap, aux_ctrl_find_port, (void *)&aux_ctrl[idx]);
-#ifdef STM32_PLATFORM
-        if(aux_ctrl[idx].irq_mode == IRQ_Mode_None && !(aux_ctrl[idx].function == Input_Probe || aux_ctrl[idx].function == Input_LimitsOverride))
-            continue;
-#endif
-        if(aux_ctrl[idx].pin == 0xFF) {
-            if(ioports_enumerate(Port_Digital, Port_Input, cap, aux_claim, (void *)&aux_ctrl[idx]))
-                hal.signals_cap.mask |= aux_ctrl[idx].cap.mask;
-        } else if(aux_ctrl[idx].aux_port != IOPORT_UNASSIGNED)
+        if(aux_ctrl[idx].port != IOPORT_UNASSIGNED)
             aux_claim_explicit(&aux_ctrl[idx]);
+
+        else {
+
+            pin_cap_t cap = { .irq_mode = aux_ctrl[idx].irq_mode, .claimable = On };
+
+            if(aux_ctrl[idx].gpio.pin == 0xFE) // Toolsetter and Probe2
+                ioports_enumerate(Port_Digital, Port_Input, cap, __find_in_port, (void *)&aux_ctrl[idx]);
+#ifdef STM32_PLATFORM
+            if(aux_ctrl[idx].irq_mode == IRQ_Mode_None && !(aux_ctrl[idx].function == Input_Probe || aux_ctrl[idx].function == Input_LimitsOverride))
+                continue;
+#endif
+            if(aux_ctrl[idx].gpio.pin == 0xFF) {
+                if(ioports_enumerate(Port_Digital, Port_Input, cap, aux_claim, (void *)&aux_ctrl[idx]))
+                    hal.signals_cap.mask |= aux_ctrl[idx].signal.mask;
+            }
+        }
     }
 }
 
@@ -344,16 +353,16 @@ static inline control_signals_t aux_ctrl_scan_status (control_signals_t signals)
     uint_fast8_t idx =  sizeof(aux_ctrl) / sizeof(aux_ctrl_t);
 
     if(idx) do {
-        if(aux_ctrl[--idx].pin != 0xFF)
+        if(!aux_ctrl[--idx].scan)
             break;
-        if(aux_ctrl[idx].aux_port != IOPORT_UNASSIGNED) {
-            signals.mask &= ~aux_ctrl[idx].cap.mask;
+        signals.mask &= ~aux_ctrl[idx].signal.mask;
+        if(aux_ctrl[idx].port != IOPORT_UNASSIGNED) {
   #ifdef GRBL_ESP32 // Snowflake guru workaround
-            if(hal.port.wait_on_input(Port_Digital, aux_ctrl[idx].aux_port, WaitMode_Immediate, FZERO) == 1)
+            if(hal.port.wait_on_input(Port_Digital, aux_ctrl[idx].port, WaitMode_Immediate, FZERO) == 1)
                 signals.mask |= aux_ctrl[idx].cap.mask;
   #else
-            if(hal.port.wait_on_input(Port_Digital, aux_ctrl[idx].aux_port, WaitMode_Immediate, 0.0f) == 1)
-                signals.mask |= aux_ctrl[idx].cap.mask;
+            if(hal.port.wait_on_input(Port_Digital, aux_ctrl[idx].port, WaitMode_Immediate, 0.0f) == 1)
+                signals.mask |= aux_ctrl[idx].signal.mask;
   #endif
         }
     } while(idx);
@@ -367,116 +376,85 @@ static inline control_signals_t aux_ctrl_scan_status (control_signals_t signals)
 static aux_ctrl_out_t aux_ctrl_out[] = {
 #if defined(ESP_PLATFORM) || defined(RP2040) // for now
 #if defined(STEPPERS_ENABLE_PIN) && STEPPERS_ENABLE_PORT == EXPANDER_PORT
-    { .function = Output_StepperEnable,  .aux_port = IOPORT_UNASSIGNED,  .pin = STEPPERS_ENABLE_PIN, .port = (void *)STEPPERS_ENABLE_PORT },
+    add_aux_output_exp(Output_StepperEnable, STEPPERS_ENABLE)
 #endif
 #if defined(X_ENABLE_PIN) && X_ENABLE_PORT == EXPANDER_PORT
-    { .function = Output_StepperEnableX, .aux_port = IOPORT_UNASSIGNED,  .pin = X_ENABLE_PIN,   .port = (void *)X_ENABLE_PORT },
+    add_aux_output_exp(Output_StepperEnableX, X_ENABLE)
 #endif
 #if defined(X2_ENABLE_PIN) && X2_ENABLE_PORT == EXPANDER_PORT
-    { .function = Output_StepperEnableX2, .aux_port = IOPORT_UNASSIGNED, .pin = X2_ENABLE_PIN,  .port = (void *)X2_ENABLE_PORT },
+    add_aux_output_exp(Output_StepperEnableX2, X2_ENABLE)
 #endif
 #if defined(Y_ENABLE_PIN) && Y_ENABLE_PORT == EXPANDER_PORT
-    { .function = Output_StepperEnableY,  .aux_port = IOPORT_UNASSIGNED, .pin = Y_ENABLE_PIN,   .port = (void *)Y_ENABLE_PORT },
+    add_aux_output_exp(Output_StepperEnableY, Y_ENABLE)
 #endif
 #if defined(Y2_ENABLE_PIN) && Y2_ENABLE_PORT == EXPANDER_PORT
-    { .function = Output_StepperEnableY2, .aux_port = IOPORT_UNASSIGNED, .pin = Y2_ENABLE_PIN,  .port = (void *)Y2_ENABLE_PORT },
+    add_aux_output_exp(Output_StepperEnableY2, Y2_ENABLE)
 #endif
 #if defined(XY_ENABLE_PIN) && XY_ENABLE_PORT == EXPANDER_PORT
-    { .function = Output_StepperEnableXY, .aux_port = IOPORT_UNASSIGNED, .pin = XY_ENABLE_PIN,  .port = (void *)XY_ENABLE_PORT },
+    add_aux_output_exp(Output_StepperEnableXY, XY_ENABLE)
 #endif
 #if defined(Z_ENABLE_PIN) && Z_ENABLE_PORT == EXPANDER_PORT
-    { .function = Output_StepperEnableZ,  .aux_port = IOPORT_UNASSIGNED, .pin = Z_ENABLE_PIN,   .port = (void *)Z_ENABLE_PORT },
+    add_aux_output_exp(Output_StepperEnableZ, Z_ENABLE)
 #endif
 #if defined(Z2_ENABLE_PIN) && Z2_ENABLE_PORT == EXPANDER_PORT
-    { .function = Output_StepperEnableZ2, .aux_port = IOPORT_UNASSIGNED, .pin = Z2_ENABLE_PIN,  .port = (void *)Z2_ENABLE_PORT },
+    add_aux_output_exp(Output_StepperEnableZ2, Z2_ENABLE)
 #endif
 #if defined(A_ENABLE_PIN) && A_ENABLE_PORT == EXPANDER_PORT
-    { .function = Output_StepperEnableA,  .aux_port = IOPORT_UNASSIGNED, .pin = A_ENABLE_PIN,   .port = (void *)A_ENABLE_PORT },
+    add_aux_output_exp(Output_StepperEnableA, A_ENABLE)
 #endif
 #if defined(B_ENABLE_PIN) && B_ENABLE_PORT == EXPANDER_PORT
-    { .function = Output_StepperEnableB,  .aux_port = IOPORT_UNASSIGNED, .pin = B_ENABLE_PIN,   .port = (void *)B_ENABLE_PORT },
+    add_aux_output_exp(Output_StepperEnableB, B_ENABLE)
 #endif
 #if defined(C_ENABLE_PIN) && C_ENABLE_PORT == EXPANDER_PORT
-    { .function = Output_StepperEnableC,  .aux_port = IOPORT_UNASSIGNED, .pin = C_ENABLE_PIN,   .port = (void *)C_ENABLE_PORT },
+    add_aux_output_exp(Output_StepperEnableC, C_ENABLE)
 #endif
 #if defined(U_ENABLE_PIN) && U_ENABLE_PORT == EXPANDER_PORT
-    { .function = Output_StepperEnableU,  .aux_port = IOPORT_UNASSIGNED, .pin = U_ENABLE_PIN,   .port = (void *)U_ENABLE_PORT },
+    add_aux_output_exp(Output_StepperEnableU, U_ENABLE)
 #endif
-#if defined(V_ENABLE_PIN) && AV_ENABLE_PORT == EXPANDER_PORT
-    { .function = Output_StepperEnableV,  .aux_port = IOPORT_UNASSIGNED, .pin = V_ENABLE_PIN,   .port = (void *)V_ENABLE_PORT },
+#if defined(V_ENABLE_PIN) && V_ENABLE_PORT == EXPANDER_PORT
+    add_aux_output_exp(Output_StepperEnableV, V_ENABLE)
 #endif
 #endif //
+
 #ifdef SPINDLE_ENABLE_PIN
- #ifndef SPINDLE_ENABLE_PORT
-  #define SPINDLE_ENABLE_PORT 0
- #endif
-    { .function = Output_SpindleOn,    .aux_port = IOPORT_UNASSIGNED, .pin = SPINDLE_ENABLE_PIN,     .port = (void *)SPINDLE_ENABLE_PORT },
+    add_aux_output(Output_SpindleOn, SPINDLE_ENABLE)
 #endif
 #ifdef SPINDLE_PWM_PIN
- #ifndef SPINDLE_PWM_PORT
-  #define SPINDLE_PWM_PORT 0
- #endif
-    { .function = Output_SpindlePWM,   .aux_port = IOPORT_UNASSIGNED, .pin = SPINDLE_PWM_PIN,        .port = (void *)SPINDLE_PWM_PORT },
+    add_aux_output(Output_SpindlePWM, SPINDLE_PWM)
 #endif
 #ifdef SPINDLE_DIRECTION_PIN
- #ifndef SPINDLE_DIRECTION_PORT
-  #define SPINDLE_DIRECTION_PORT 0
- #endif
-    { .function = Output_SpindleDir,   .aux_port = IOPORT_UNASSIGNED, .pin = SPINDLE_DIRECTION_PIN,  .port = (void *)SPINDLE_DIRECTION_PORT },
+    add_aux_output(Output_SpindleDir, SPINDLE_DIRECTION)
 #endif
-
 #ifdef SPINDLE1_ENABLE_PIN
- #ifndef SPINDLE1_ENABLE_PORT
-  #define SPINDLE1_ENABLE_PORT 0
- #endif
-    { .function = Output_Spindle1On,   .aux_port = IOPORT_UNASSIGNED, .pin = SPINDLE1_ENABLE_PIN,    .port = (void *)SPINDLE1_ENABLE_PORT },
-#endif
-#ifdef SPINDLE1_PWM_PIN
- #ifndef SPINDLE1_PWM_PORT
-  #define SPINDLE1_PWM_PORT 0
- #endif
-    { .function = Output_Spindle1PWM,  .aux_port = IOPORT_UNASSIGNED, .pin = SPINDLE1_PWM_PIN,       .port = (void *)SPINDLE1_PWM_PORT },
+    add_aux_output(Output_Spindle1On, SPINDLE1_ENABLE)
 #endif
 #ifdef SPINDLE1_DIRECTION_PIN
- #ifndef SPINDLE1_DIRECTION_PORT
-  #define SPINDLE1_DIRECTION_PORT 0
- #endif
-    { .function = Output_Spindle1Dir,  .aux_port = IOPORT_UNASSIGNED, .pin = SPINDLE1_DIRECTION_PIN, .port = (void *)SPINDLE1_DIRECTION_PORT },
+    add_aux_output(Output_Spindle1Dir, SPINDLE1_DIRECTION)
+#endif
+#ifdef SPINDLE1_PWM_PIN
+    add_aux_output(Output_Spindle1PWM, SPINDLE1_PWM)
 #endif
-
 #ifdef COOLANT_FLOOD_PIN
- #ifndef COOLANT_FLOOD_PORT
-  #define COOLANT_FLOOD_PORT 0
- #endif
-    { .function = Output_CoolantFlood, .aux_port = IOPORT_UNASSIGNED, .pin = COOLANT_FLOOD_PIN,      .port = (void *)COOLANT_FLOOD_PORT },
+    add_aux_output(Output_CoolantFlood, COOLANT_FLOOD)
 #endif
 #ifdef COOLANT_MIST_PIN
- #ifndef COOLANT_MIST_PORT
-  #define COOLANT_MIST_PORT 0
- #endif
-    { .function = Output_CoolantMist,  .aux_port = IOPORT_UNASSIGNED, .pin = COOLANT_MIST_PIN,       .port = (void *)COOLANT_MIST_PORT },
+    add_aux_output(Output_CoolantMist, COOLANT_MIST)
 #endif
-
 #ifdef COPROC_RESET_PIN
- #ifndef COPROC_RESET_PORT
-  #define COPROC_RESET_PORT 0
- #endif
-    { .function = Output_CoProc_Reset, .aux_port = IOPORT_UNASSIGNED, .pin = COPROC_RESET_PIN,       .port = (void *)COPROC_RESET_PORT },
+    add_aux_output(Output_CoProc_Reset, COPROC_RESET)
 #endif
 #ifdef COPROC_BOOT0_PIN
- #ifndef COPROC_BOOT0_PORT
-  #define COPROC_BOOT0_PORT 0
- #endif
-    { .function = Output_CoProc_Boot0, .aux_port = IOPORT_UNASSIGNED, .pin = COPROC_BOOT0_PIN,       .port = (void *)COPROC_BOOT0_PORT },
+    add_aux_output(Output_CoProc_Boot0, COPROC_BOOT0)
 #endif
 #if defined(SPI_RST_PIN) && defined(RP2040)
- #if SPI_RST_PORT == EXPANDER_PORT
-    { .function = Output_SPIRST,       .aux_port = IOPORT_UNASSIGNED, .pin = SPI_RST_PIN,            .port = (void *)SPI_RST_PORT },
+ #ifndef SPI_RST_PORT
+  #define SPI_RST_PORT 0
  #endif
+    add_aux_output(Output_SPIRST, SPI_RST)
 #endif
 };
 
-static inline aux_ctrl_out_t *aux_out_remap_explicit (void *port, uint8_t pin, uint8_t aux_port, void *output)
+static inline aux_ctrl_out_t *aux_out_remap_explicit (aux_gpio_t gpio, uint8_t port, void *output)
 {
     aux_ctrl_out_t *ctrl_pin = NULL;
 
@@ -484,9 +462,9 @@ static inline aux_ctrl_out_t *aux_out_remap_explicit (void *port, uint8_t pin, u
 
     if(idx) do {
         idx--;
-        if(aux_ctrl_out[idx].port == port && aux_ctrl_out[idx].pin == pin) {
+        if(aux_ctrl_out[idx].gpio.port == gpio.port && aux_ctrl_out[idx].gpio.pin == gpio.pin) {
             ctrl_pin = &aux_ctrl_out[idx];
-            ctrl_pin->aux_port = aux_port;
+            ctrl_pin->port = port;
             ctrl_pin->output = output;
         }
     } while(idx && ctrl_pin == NULL);
@@ -496,37 +474,56 @@ static inline aux_ctrl_out_t *aux_out_remap_explicit (void *port, uint8_t pin, u
 
 typedef bool (*aux_claim_explicit_out_ptr)(aux_ctrl_out_t *aux_ctrl);
 
-static bool aux_ctrl_claim_out_port (xbar_t *properties, uint8_t port, void *data)
-{
-    if(((aux_ctrl_out_t *)data)->port == (void *)EXPANDER_PORT) {
-        if(((aux_ctrl_out_t *)data)->pin == properties->pin && properties->set_value)
-            ((aux_ctrl_out_t *)data)->aux_port = port;
-    } else if(ioport_claim(Port_Digital, Port_Output, &port, xbar_fn_to_pinname(((aux_ctrl_out_t *)data)->function)))
-        ((aux_ctrl_out_t *)data)->aux_port = port;
+// Default functions for aux_ctrl_claim_out_ports()
 
-    return ((aux_ctrl_out_t *)data)->aux_port != 0xFF;
+static bool __claim_out_port (xbar_t *properties, uint8_t port, void *data)
+{
+    if(((aux_ctrl_out_t *)data)->gpio.port == (void *)EXPANDER_PORT) {
+        if(((aux_ctrl_out_t *)data)->gpio.pin == properties->pin && properties->set_value)
+            ((aux_ctrl_out_t *)data)->port = port;
+    } else if(ioport_claim(Port_Digital, Port_Output, &port, xbar_fn_to_pinname(((aux_ctrl_out_t *)data)->function)))
+        ((aux_ctrl_out_t *)data)->port = port;
+
+    return ((aux_ctrl_out_t *)data)->port != IOPORT_UNASSIGNED;
 }
 
+static bool ___claim_out_port_explicit (aux_ctrl_out_t *aux_ctrl)
+{
+    xbar_t *pin;
+
+    if((pin = ioport_claim(Port_Digital, Port_Output, &aux_ctrl->port, NULL)))
+        ioport_set_function(pin, aux_ctrl->function, NULL);
+    else
+        aux_ctrl->port = IOPORT_UNASSIGNED;
+
+    return aux_ctrl->port != IOPORT_UNASSIGNED;
+}
+
+//
+
 static inline void aux_ctrl_claim_out_ports (aux_claim_explicit_out_ptr aux_claim_explicit, ioports_enumerate_callback_ptr aux_claim)
 {
     uint_fast8_t idx;
 
     if(aux_claim == NULL)
-        aux_claim = aux_ctrl_claim_out_port;
+        aux_claim = __claim_out_port;
+
+    if(aux_claim_explicit == NULL)
+        aux_claim_explicit = ___claim_out_port_explicit;
 
     for(idx = 0; idx < sizeof(aux_ctrl_out) / sizeof(aux_ctrl_out_t); idx++) {
-        if(aux_ctrl_out[idx].port == (void *)EXPANDER_PORT) {
+        if(aux_ctrl_out[idx].gpio.port == (void *)EXPANDER_PORT) {
             if(ioports_enumerate(Port_Digital, Port_Output, (pin_cap_t){ .external = On, .claimable = On }, aux_claim, &aux_ctrl_out[idx])) {
-                if((aux_ctrl_out[idx].output = ioport_claim(Port_Digital, Port_Output, &aux_ctrl_out[idx].aux_port, NULL /*xbar_fn_to_pinname(aux_ctrl_out[idx].function)*/))) {
+                if((aux_ctrl_out[idx].output = ioport_claim(Port_Digital, Port_Output, &aux_ctrl_out[idx].port, NULL /*xbar_fn_to_pinname(aux_ctrl_out[idx].function)*/))) {
                     ioport_set_function((xbar_t *)aux_ctrl_out[idx].output, aux_ctrl_out[idx].function, NULL);
                         // TODO: else set description?
                     aux_claim_explicit(&aux_ctrl_out[idx]);
                 }
             }
-        } else if(aux_ctrl_out[idx].pin == 0xFF) {
+        } else if(aux_ctrl_out[idx].gpio.pin == 0xFF) {
             if(ioports_enumerate(Port_Digital, Port_Output, (pin_cap_t){ .claimable = On }, aux_claim, &aux_ctrl_out[idx]))
                 aux_claim_explicit(&aux_ctrl_out[idx]);
-        } else if(aux_ctrl_out[idx].aux_port != 0xFF)
+        } else if(aux_ctrl_out[idx].port != IOPORT_UNASSIGNED)
             aux_claim_explicit(&aux_ctrl_out[idx]);
     }
 }
diff --git a/probe.c b/probe.c
new file mode 100644
index 0000000..5c8a9a1
--- /dev/null
+++ b/probe.c
@@ -0,0 +1,227 @@
+/*
+  probe.c - An embedded CNC Controller with rs274/ngc (g-code) support
+
+  Part of grblHAL
+
+  Copyright (c) 2025 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
+  the Free Software Foundation, either version 3 of the License, or
+  (at your option) any later version.
+
+  grblHAL is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+  GNU General Public License for more details.
+
+  You should have received a copy of the GNU General Public License
+  along with grblHAL. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include "hal.h"
+
+typedef struct {
+    probe_id_t probe_id;
+    probe_flags_t flags;
+    probeflags_t inverted_bit;
+    uint8_t port;
+    void *input;
+    get_probe_input_ptr get_input;
+} probe_t;
+
+static probe_state_t probe_state = { .connected = On };
+static probe_t probes[N_PROBE_MAX], *probe = NULL;
+
+static void probe_irq_handler (uint8_t port, bool state)
+{
+    if(probe_state.is_probing) {
+        probe_state.triggered = On;
+        if(!probe_state.was_probing) {
+            probe_state.was_probing = true;
+        }
+    } else {
+        control_signals_t signals = { .mask = hal.control.get_state().mask };
+        signals.probe_triggered = On;
+        hal.control.interrupt_callback(signals);
+    }
+}
+
+// Toggle probe connected status.
+static void probe_connected_toggle (void)
+{
+    if(!probe_state.is_probing) {
+        if((probe->flags.connected = probe_state.connected = !probe_state.connected)) {
+            if(probe->flags.watchable && settings.probe.enable_protection)
+                probe->flags.guarded = probe_state.irq_enabled = ioport_enable_irq(probe->port, IRQ_Mode_Change, probe_irq_handler);
+        } else if(probe->flags.guarded && ioport_enable_irq(probe->port, IRQ_Mode_None, probe_irq_handler))
+            probe->flags.guarded = probe_state.irq_enabled = Off;
+
+        if(settings.probe.enable_protection)
+            system_add_rt_report(Report_ProbeProtect);
+    }
+}
+
+// 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)
+{
+    bool invert = !!(settings.probe.value & probe->inverted_bit.value);
+
+    probe_state.inverted = is_probe_away ? !invert : invert;
+
+    if(probing) {
+        if(probe->flags.latchable) {
+            probe_state.is_probing = probe_state.was_probing = Off;
+            probe_state.triggered = hal.probe.get_state().triggered;
+            probe_state.irq_mode = probe_state.triggered ? IRQ_Mode_None : (probe_state.inverted ? IRQ_Mode_Falling : IRQ_Mode_Rising);
+        }
+    } else
+        probe_state.irq_mode = probe->flags.connected && probe->flags.watchable && settings.probe.enable_protection ? IRQ_Mode_Change : IRQ_Mode_None;
+
+    if(ioport_enable_irq(probe->port, probe_state.irq_mode, probe_irq_handler)) {
+        probe_state.irq_enabled = probe_state.irq_mode != IRQ_Mode_None;
+        probe->flags.guarded = !probing && probe_state.irq_enabled;
+    }
+
+    if(settings.probe.enable_protection)
+        system_add_rt_report(Report_ProbeProtect);
+
+    if(!probe_state.irq_enabled)
+        probe_state.triggered = Off;
+
+    probe_state.is_probing = probing;
+}
+
+static bool probe_select (probe_id_t probe_id)
+{
+    uint_fast8_t idx = 0;
+    probe_t *selected_probe = NULL;
+
+    do {
+        if(probes[idx].probe_id == probe_id && probes[idx].get_input)
+            selected_probe = &probes[idx];
+    } while(selected_probe == NULL && ++idx < N_PROBE_MAX);
+
+    if(!probe_state.is_probing && selected_probe && selected_probe != probe) {
+
+        if(probe_state.irq_mode != IRQ_Mode_None)
+            ioport_enable_irq(probe->port, IRQ_Mode_None, probe_irq_handler);
+
+        probe = selected_probe;
+        hal.probe.configure(false, false);
+    }
+
+    return probe == selected_probe;
+}
+
+static probe_flags_t probe_get_caps (probe_id_t probe_id)
+{
+    uint_fast8_t idx = 0;
+    probe_t *probe = NULL;
+
+    do {
+        if(probes[idx].probe_id == probe_id && probes[idx].get_input)
+            probe = &probes[idx];
+    } while(probe == NULL && ++idx < N_PROBE_MAX);
+
+    return probe ? probe->flags : (probe_flags_t){0};
+}
+
+static bool is_triggered (probe_id_t probe_id)
+{
+    uint_fast8_t idx = 0;
+    probe_t *probe = NULL;
+
+    do {
+        if(probes[idx].probe_id == probe_id && probes[idx].get_input)
+            probe = &probes[idx];
+    } while(probe == NULL && ++idx < N_PROBE_MAX);
+
+    return !!probe && probe->get_input(probe->input) ^ !!(settings.probe.value & probe->inverted_bit.value);
+}
+
+// Returns the probe connected and triggered pin states.
+static probe_state_t get_state (void)
+{
+    probe_state_t state = { .value = probe_state.value };
+
+    state.probe_id  = probe->probe_id;
+    state.connected = probe->flags.connected;
+
+    if(probe_state.is_probing && probe_state.irq_enabled)
+        state.triggered = probe_state.triggered;
+    else
+        state.triggered = probe->get_input(probe->input) ^ probe_state.inverted;
+
+    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)
+{
+    static uint_fast8_t n_probes = 0;
+
+    if(get_input == NULL || n_probes >= N_PROBE_MAX)
+        return false;
+
+    bool can_latch;
+
+    if(!(can_latch = (irq_mode & IRQ_Mode_RisingFalling) == IRQ_Mode_RisingFalling))
+        hal.signals_cap.probe_triggered = Off;
+    else if(n_probes == 0)
+        hal.signals_cap.probe_triggered = On;
+
+    probes[n_probes].probe_id = probe_id;
+    probes[n_probes].port = port;
+    probes[n_probes].flags.available = On;
+    probes[n_probes].flags.connected = probe_state.connected;
+    probes[n_probes].flags.latchable = can_latch;
+    probes[n_probes].flags.watchable = !!(irq_mode & IRQ_Mode_Change);
+    probes[n_probes].input = input;
+    probes[n_probes].get_input = get_input;
+
+    switch(probe_id) {
+
+        case Probe_Toolsetter:
+            probes[n_probes].inverted_bit.invert_toolsetter_input = On;
+            break;
+
+        case Probe_2:
+            probes[n_probes].inverted_bit.invert_probe2_input = On;
+            break;
+
+        default: // Probe_Default
+            probes[n_probes].inverted_bit.invert_probe_pin = On;
+            break;
+    }
+
+    hal.driver_cap.probe_pull_up = On;
+    hal.probe.configure = probe_configure;
+    hal.probe.connected_toggle = probe_connected_toggle;
+    hal.probe.get_caps = probe_get_caps;
+    hal.probe.get_state = get_state;
+    hal.probe.is_triggered = is_triggered;
+
+    if(probe == NULL || probe_id == Probe_Default)
+        probe = &probes[n_probes];
+
+    if(++n_probes == 2)
+        hal.probe.select = probe_select;
+
+    return true;
+}
+
+void probe_connected_event (void *data)
+{
+    if(hal.probe.connected_toggle) {
+        if((uint32_t)data == 2)
+            hal.probe.connected_toggle();
+        else {
+            probe_state_t state = hal.probe.get_state();
+
+            if(state.probe_id == Probe_Default && hal.driver_cap.probe && state.connected != !!data)
+                hal.probe.connected_toggle();
+        }
+    }
+}
diff --git a/probe.h b/probe.h
index 7358944..27040f2 100644
--- a/probe.h
+++ b/probe.h
@@ -22,8 +22,14 @@
 #ifndef _PROBE_H_
 #define _PROBE_H_
 
+#include "crossbar.h"
+
+#define N_PROBE_MAX 3
+
 // Values that define the probing state machine.
 
+typedef bool (*get_probe_input_ptr)(void *input);
+
 typedef enum {
     Probing_Off = 0, //!< 0
     Probing_Active   //!< 1
@@ -36,33 +42,33 @@ typedef enum {
 } probe_id_t;
 
 typedef union {
-    uint8_t value;
+    uint16_t value;
     struct {
-        uint8_t triggered     :1, //!< Set to true when probe or toolsetter is triggered.
-                connected     :1, //!< Set to true when probe is connected. Always set to true if the driver does not have a probe connected input.
-                inverted      :1, //!< For driver use
-                is_probing    :1, //!< For driver use
-                irq_enabled   :1, //!< For driver use
-                tls_triggered :1, //!< Set to true when toolsetter is triggered.
-                probe_id      :2; //!< Id of active probe (for now).
+        uint16_t triggered     :1, //!< Set to true when probe or toolsetter is triggered.
+                 connected     :1, //!< Set to true when probe is connected. Always set to true if the driver does not have a probe connected input.
+                 inverted      :1, //!< For driver use
+                 is_probing    :1, //!< For driver use
+                 was_probing   :1, //!< For driver use
+                 irq_enabled   :1, //!< For driver use
+                 irq_mode      :5, //!< pin_irq_mode_t - for driver use
+                 unused        :3,
+                 probe_id      :2; //!< Id of active probe (for now).
     };
 } probe_state_t;
 
 typedef union {
     uint8_t value;
     struct {
-        uint8_t connected     :1, //!< Set to true when probe is connected. Always set to true if the driver does not have a probe connected input.
+        uint8_t available     :1, //!< Set to true when probe input is available.
+                connected     :1, //!< Set to true when probe is connected. Always set to true if the driver does not have a probe connected input.
                 latchable     :1, //!< Set to true when probe input supports change rising/falling.
                 watchable     :1, //!< Set to true when probe input supports change interrupt.
-                unassigned    :5;
+                guarded       :1, //!< Set to true when probe protection is enabled.
+                unassigned    :3;
     };
 } probe_flags_t;
 
-typedef struct {
-    probe_id_t probe_id;
-    probe_flags_t flags;
-    uint8_t port;
-    void *input;
-} probe_t;
+void probe_connected_event (void *data);
+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);
 
-#endif
+#endif // _PROBE_H_
diff --git a/protocol.c b/protocol.c
index 2992a64..1dc4327 100644
--- a/protocol.c
+++ b/protocol.c
@@ -183,7 +183,7 @@ bool protocol_main_loop (void)
         spindle_all_off();
         hal.coolant.set_state((coolant_state_t){0});
         sys.cold_start = false;
-		system_set_exec_state_flag(EXEC_RT_COMMAND);  // execute any statup up tasks
+		system_set_exec_state_flag(EXEC_RT_COMMAND);  // execute any startup up tasks
     }
 
     // ---------------------------------------------------------------------------------
@@ -918,17 +918,17 @@ ISR_CODE bool ISR_FUNC(protocol_enqueue_realtime_command)(char c)
             break;
 
         case CMD_PROBE_CONNECTED_TOGGLE:
-            if(hal.probe.connected_toggle)
-                hal.probe.connected_toggle();
+            if((drop = !!hal.probe.connected_toggle))
+                task_add_immediate(probe_connected_event, (void *)2);
             break;
 
         case CMD_OPTIONAL_STOP_TOGGLE:
-            if((signals.stop_disable = !hal.signals_cap.stop_disable)) // Not available as realtime command if HAL supports physical switch
+            if((drop = (signals.stop_disable = !hal.signals_cap.stop_disable))) // Not available as realtime command if HAL supports physical switch
                 sys.flags.optional_stop_disable = !sys.flags.optional_stop_disable;
             break;
 
         case CMD_SINGLE_BLOCK_TOGGLE:
-            if((signals.single_block = !hal.signals_cap.single_block)) // Not available as realtime command if HAL supports physical switch
+            if((drop = (signals.single_block = !hal.signals_cap.single_block))) // Not available as realtime command if HAL supports physical switch
                 sys.flags.single_block = !sys.flags.single_block;
             break;
 
diff --git a/report.c b/report.c
index 7760798..7ff6390 100644
--- a/report.c
+++ b/report.c
@@ -1473,8 +1473,11 @@ void report_realtime_status (stream_write_ptr stream_write)
         if(report.tool)
             stream_write(appendbuf(2, "|T:", uitoa((uint32_t)gc_state.tool->tool_id)));
 
-        if(report.probe_id)
+        if(report.probe_id || report.probe_protect) {
             stream_write(appendbuf(2, "|P:", uitoa((uint32_t)probe_state.probe_id)));
+            if(report.probe_protect && !probe_state.is_probing && probe_state.irq_enabled)
+                stream_write(",P");
+        }
 
         if(report.tlo_reference)
             stream_write(appendbuf(2, "|TLR:", uitoa(sys.tlo_reference_set.mask != 0)));
diff --git a/settings.c b/settings.c
index ce95de3..3525ffa 100644
--- a/settings.c
+++ b/settings.c
@@ -426,8 +426,8 @@ static char homing_options[] = "Enable,Enable single axis commands,Homing on sta
 #else
 static char probe_signals[] = "Probe";
 #endif
-static char probing_options[] = "Allow feed override,Apply soft limits,N/A,Auto select toolsetter,Auto select probe 2";
-static char control_signals[] = "Reset,Feed hold,Cycle start,Safety door,Block delete,Optional stop,EStop,Probe connected,Motor fault,Motor warning,Limits override,Single step blocks";
+static char probing_options[] = "Allow feed override,Apply soft limits,N/A,Auto select toolsetter,Auto select probe 2,Probe protection";
+static char control_signals[] = "Reset,Feed hold,Cycle start,Safety door,Block delete,Optional stop,EStop,Probe disconnected,Motor fault,Motor warning,Limits override,Single step blocks,Toolsetter overtravel";
 static char spindle_signals[] = "Spindle enable,Spindle direction,PWM";
 static char coolant_signals[] = "Flood,Mist";
 static char ganged_axes[] = "X-Axis,Y-Axis,Z-Axis";
@@ -702,11 +702,18 @@ static status_code_t set_ngc_debug_out (setting_id_t id, uint_fast16_t int_value
 
 static status_code_t set_control_invert (setting_id_t id, uint_fast16_t int_value)
 {
-    settings.control_invert.mask = (int_value & hal.signals_cap.mask) | limits_override.mask;
+    int_value = (int_value & hal.signals_cap.mask) | limits_override.mask;
+
+    bool pd_changed = hal.probe.connected_toggle && settings.control_invert.probe_disconnected != (control_signals_t){ .value = int_value }.probe_disconnected;
+
+    settings.control_invert.mask = int_value;
 
     ioport_setting_changed(id);
     system_init_switches();
 
+    if(pd_changed)
+        hal.probe.connected_toggle();
+
     return Status_OK;
 }
 
@@ -1077,9 +1084,12 @@ static status_code_t set_probe_flags (setting_id_t id, uint_fast16_t int_value)
 {
     settings.probe.allow_feed_override = bit_istrue(int_value, bit(0));
     settings.probe.soft_limited = bit_istrue(int_value, bit(1));
-    settings.probe.enable_protection = bit_istrue(int_value, bit(2));
+//    settings.probe.? = bit_istrue(int_value, bit(2));
     settings.probe.toolsetter_auto_select = bit_istrue(int_value, bit(3)) && hal.driver_cap.toolsetter && hal.probe.select;
     settings.probe.probe2_auto_select = bit_istrue(int_value, bit(4)) && hal.driver_cap.probe2 && hal.probe.select;
+    settings.probe.enable_protection = bit_istrue(int_value, bit(5));
+
+    hal.probe.configure(false, false);
 
     return Status_OK;
 }
@@ -1601,9 +1611,10 @@ static uint32_t get_int (setting_id_t id)
         case Setting_ProbingFlags:
             value = settings.probe.allow_feed_override |
                     (settings.probe.soft_limited << 1) |
-                     (settings.probe.enable_protection << 2) |
+             //      (settings.probe.enable_protection << 2) |
                       (settings.probe.toolsetter_auto_select << 3) |
-                       (settings.probe.probe2_auto_select << 4);
+                       (settings.probe.probe2_auto_select << 4) |
+                        (settings.probe.enable_protection << 5);
             break;
 
         case Setting_ToolChangeMode:
@@ -3509,6 +3520,14 @@ void settings_init (void)
     setting_remove_elements(Setting_ProbePullUpDisable, mask);
 
     mask = 0b00011 | (hal.probe.select ? ((hal.driver_cap.toolsetter << 3) | (hal.driver_cap.probe2 << 4)) : 0);
+#if 0
+    if(hal.probe.get_caps) {
+        if(hal.probe.get_caps(Probe_Default).watchable)
+            mask |= (1 << 5);
+    }
+#else
+    settings.probe.enable_protection = Off;
+#endif
     setting_remove_elements(Setting_ProbingFlags, mask);
 
     if(!settings.flags.settings_downgrade && settings.version.build != (GRBL_BUILD - 20000000UL)) {
diff --git a/stream.c b/stream.c
index eb4dcb0..274330f 100644
--- a/stream.c
+++ b/stream.c
@@ -724,8 +724,8 @@ void debug_write (const char *s)
 {
     if(dbg_write) {
         dbg_write(s);
-//        while(hal.debug.get_tx_buffer_count()) // Wait until message is delivered
-//            grbl.on_execute_realtime(state_get());
+        while(hal.debug.get_tx_buffer_count()) // Wait until message is delivered
+            grbl.on_execute_realtime(state_get());
     }
 }
 
@@ -740,8 +740,8 @@ void debug_writeln (const char *s)
         dbg_write(s);
         dbg_write(ASCII_EOL);
 
-//        while(hal.debug.get_tx_buffer_count()) // Wait until message is delivered
-//            grbl.on_execute_realtime(state_get());
+        while(hal.debug.get_tx_buffer_count()) // Wait until message is delivered
+            grbl.on_execute_realtime(state_get());
 
         lock = false;
     }
diff --git a/system.c b/system.c
index 44b98aa..6944651 100644
--- a/system.c
+++ b/system.c
@@ -72,8 +72,13 @@ ISR_CODE void ISR_FUNC(control_interrupt_handler)(control_signals_t signals)
        .motor_fault = On
     };
 
-    if(signals.deasserted)
+    if(signals.deasserted) {
+        if(signals.probe_disconnected) {
+            signals.probe_disconnected = Off;
+            task_add_immediate(probe_connected_event, (void *)1);
+        }
         signals.bits &= onoff_signals.bits;
+    }
 
     if(signals.bits) {
 
@@ -116,7 +121,8 @@ ISR_CODE void ISR_FUNC(control_interrupt_handler)(control_signals_t signals)
                 if(sys.probing_state == Probing_Active && state_get() == STATE_CYCLE) {
                     system_set_exec_state_flag(EXEC_FEED_HOLD);
                     sys.alarm_pending = Alarm_ProbeProtect;
-                }
+                } else
+                    task_add_immediate(probe_connected_event, NULL);
             } else if(signals.feed_hold)
                 system_set_exec_state_flag(EXEC_FEED_HOLD);
             else if(signals.cycle_start)
diff --git a/system.h b/system.h
index 3e1ebd8..e46083c 100644
--- a/system.h
+++ b/system.h
@@ -158,31 +158,6 @@ typedef union {
     };
 } step_control_t;
 
-// NOTE: the pin_function_t enum must be kept in sync with any changes!
-typedef union {
-    uint16_t bits;
-    uint16_t mask;
-    uint16_t value;
-    struct {
-        uint16_t reset                :1,
-                 feed_hold            :1,
-                 cycle_start          :1,
-                 safety_door_ajar     :1,
-                 block_delete         :1,
-                 stop_disable         :1, //! M1
-                 e_stop               :1,
-                 probe_disconnected   :1,
-                 motor_fault          :1,
-                 motor_warning        :1,
-                 limits_override      :1,
-                 single_block         :1,
-                 tls_overtravel       :1, //! used for probe (toolsetter) protection
-                 probe_overtravel     :1, //! used for probe protection
-                 probe_triggered      :1, //! used for probe protection
-                 deasserted           :1; //! this flag is set if signals are deasserted.
-    };
-} control_signals_t;
-
 // Define spindle stop override control states.
 typedef union {
     uint8_t value;
@@ -229,9 +204,10 @@ typedef enum {
     Report_SpindleId = (1 << 17),
     Report_ProbeId = (1 << 18),
     Report_DistanceToGo = (1 << 19),
+    Report_ProbeProtect = (1 << 20),
     Report_ForceWCO = (1 << 29),
     Report_CycleStart = (1 << 30),
-    Report_All = 0x8003FFFF
+    Report_All = 0x801FFFFF
 } report_tracking_t;
 
 typedef union {
@@ -257,7 +233,8 @@ typedef union {
                  spindle_id     :1, //!< Spindle changed.
                  probe_id       :1, //!< Probe changed.
                  distance_to_go :1, //!< Distance to go.
-                 unassigned     :9, //
+                 probe_protect  :1, //!< Probe protection state changed.
+                 unassigned     :8, //
                  force_wco      :1, //!< Add work coordinates (due to WCO changed during motion).
                  cycle_start    :1, //!< Cycle start signal triggered. __NOTE:__ do __NOT__ add to Report_All enum above!
                  all            :1; //!< Set when CMD_STATUS_REPORT_ALL is requested, may be used by user code.