diff --git a/grblHAL_Teensy4/src/T41BB5X_Pro_map.h b/grblHAL_Teensy4/src/T41BB5X_Pro_map.h
index 4d7cb94..d63d257 100644
--- a/grblHAL_Teensy4/src/T41BB5X_Pro_map.h
+++ b/grblHAL_Teensy4/src/T41BB5X_Pro_map.h
@@ -1,3 +1,4 @@
+<<<<<<< .mine
 /*
   T41BB5X_Pro_map.h - driver code for IMXRT1062 processor (on Teensy 4.1 board)
 
@@ -162,3 +163,332 @@
 #define I2C_SCL4    (24u) // Not used, for info only
 #define I2C_SDA4    (25u) // Not used, for info only
 #endif
+||||||| .r6421
+/*
+  T41BB5X_Pro_map.h - driver code for IMXRT1062 processor (on Teensy 4.1 board)
+
+  Part of grblHAL
+
+  Board by Phil Barrett: https://github.com/phil-barrett/grblHAL-teensy-4.x
+
+  Copyright (c) 2021 Terje Io
+
+  Grbl is free software: you can redistribute it and/or modify
+  it under the terms of the GNU General Public License as published by
+  the Free Software Foundation, either version 3 of the License, or
+  (at your option) any later version.
+
+  Grbl is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+  GNU General Public License for more details.
+
+  You should have received a copy of the GNU General Public License
+  along with Grbl.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#define BOARD_NAME "T41BB5X Pro"
+#define HAS_BOARD_INIT
+
+#if N_AXIS > 5
+#error Max number of axes is 5 for T41U5XBB
+#endif
+
+#if QEI_ENABLE && SPINDLE_SYNC_ENABLE
+#error Quadrature encoder and spindle sync cannot be enabled at the same time
+#endif
+
+// Board has 2K FRAM
+#undef EEPROM_ENABLE
+#undef EEPROM_IS_FRAM
+#define EEPROM_ENABLE   1
+#define EEPROM_IS_FRAM  1
+
+// Default pin assignments allow only one axis to be ganged or auto squared.
+// B axis pin numbers are used for the ganged/auto squared axis.
+// If a second axis is to be ganged/auto squared pin assignments needs to be changed!
+// Set to 1 to enable, 0 to disable.
+#define X_GANGED        0
+#define X_AUTO_SQUARE   0
+#define Y_GANGED        0
+#define Y_AUTO_SQUARE   0
+#define Z_GANGED        0
+#define Z_AUTO_SQUARE   0
+//
+
+#define X_STEP_PIN      (2u)
+#define X_DIRECTION_PIN (3u)
+#define X_ENABLE_PIN    (10u)
+#define X_LIMIT_PIN     (20u)
+
+#if X_GANGED || X_AUTO_SQUARE
+#define X2_STEP_PIN      (26u)
+#define X2_DIRECTION_PIN (27u)
+#define X2_ENABLE_PIN    (37u)
+#if X_AUTO_SQUARE
+  #define X2_LIMIT_PIN   (28u)
+#endif
+#endif
+
+#define Y_STEP_PIN      (4u)
+#define Y_DIRECTION_PIN (5u)
+#define Y_ENABLE_PIN    (35u)
+#define Y_LIMIT_PIN     (21u)
+
+#if Y_GANGED || Y_AUTO_SQUARE
+#define Y2_STEP_PIN      (26u)
+#define Y2_DIRECTION_PIN (27u)
+#define Y2_ENABLE_PIN    (37u)
+#if Y_AUTO_SQUARE
+  #define Y2_LIMIT_PIN   (28u)
+#endif
+#endif
+
+#define Z_STEP_PIN      (6u)
+#define Z_DIRECTION_PIN (7u)
+#define Z_ENABLE_PIN    (39u)
+#define Z_LIMIT_PIN     (22u)
+
+#if Z_GANGED || Z_AUTO_SQUARE
+#define Z2_STEP_PIN      (26u)
+#define Z2_DIRECTION_PIN (27u)
+#define Z2_ENABLE_PIN    (37u)
+#if Z_AUTO_SQUARE
+  #define Z2_LIMIT_PIN   (28u)
+#endif
+#endif
+
+#if N_AXIS > 3
+#define A_STEP_PIN      (8u)
+#define A_DIRECTION_PIN (9u)
+#define A_ENABLE_PIN    (38u)
+#define A_LIMIT_PIN     (23u)
+#endif
+
+#if N_AXIS > 4
+#define B_STEP_PIN      (26u)
+#define B_DIRECTION_PIN (27u)
+#define B_ENABLE_PIN    (37u)
+#define B_LIMIT_PIN     (28u)
+#endif
+
+// Define spindle enable and spindle direction output pins.
+#define SPINDLE_ENABLE_PIN      (12u)
+#define SPINDLE_DIRECTION_PIN   (11u)
+#define SPINDLEPWMPIN           (13u) // NOTE: only pin 12 or pin 13 can be assigned!
+
+// Define flood and mist coolant enable output pins.
+#define COOLANT_FLOOD_PIN   (19u)
+#define COOLANT_MIST_PIN    (18u)
+
+// Define user-control CONTROLs (cycle start, reset, feed hold, door) input pins.
+#define RESET_PIN           (40u)
+#define FEED_HOLD_PIN       (16u)
+#define CYCLE_START_PIN     (17u)
+#define SAFETY_DOOR_PIN     (29u)
+
+// Define probe switch input pin.
+#define PROBE_PIN           (15u)
+
+#if QEI_ENABLE
+#define QEI_A_PIN           (36u)
+#define QEI_B_PIN           (30u)
+//#define QEI_INDEX_PIN       (36u)
+#define QEI_SELECT_PIN      (31u)
+#endif
+
+#if SPINDLE_SYNC_ENABLE
+#define SPINDLE_INDEX_PIN   (31u) // ST2
+#define SPINDLE_PULSE_PIN   (14u) // ST3
+#endif
+
+// Define auxillary input pins
+#define AUXINPUT0_PIN       (36u) // ST0
+#if !QEI_ENABLE
+#define AUXINPUT1_PIN       (30u) // ST1
+#if !SPINDLE_SYNC_ENABLE
+#define AUXINPUT2_PIN       (31u) // ST2
+#define AUXINPUT3_PIN       (14u) // ST3
+#endif
+#endif
+
+// Define auxillary output pins
+#define AUXOUTPUT0_PIN      (32U)
+#define AUXOUTPUT1_PIN      (33U)
+#define AUXOUTPUT2_PIN      (34U)
+#define AUX_N_OUT 3
+#define AUX_OUT_MASK 0b111
+
+#if KEYPAD_ENABLE
+#define KEYPAD_STROBE_PIN   (41U)
+#endif
+
+#if EEPROM_ENABLE || KEYPAD_ENABLE
+#define I2C_PORT    4
+#define I2C_SCL4    (24u) // Not used, for info only
+#define I2C_SDA4    (25u) // Not used, for info only
+#endif
+=======
+/*
+  T41BB5X_Pro_map.h - driver code for IMXRT1062 processor (on Teensy 4.1 board)
+
+  Part of grblHAL
+
+  Board by Phil Barrett: https://github.com/phil-barrett/grblHAL-teensy-4.x
+
+  Copyright (c) 2021 Terje Io
+
+  Grbl is free software: you can redistribute it and/or modify
+  it under the terms of the GNU General Public License as published by
+  the Free Software Foundation, either version 3 of the License, or
+  (at your option) any later version.
+
+  Grbl is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+  GNU General Public License for more details.
+
+  You should have received a copy of the GNU General Public License
+  along with Grbl.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#define BOARD_NAME "T41BB5X Pro"
+#define HAS_BOARD_INIT
+
+#if N_AXIS > 5
+#error Max number of axes is 5 for T41U5XBB
+#endif
+
+#if QEI_ENABLE && SPINDLE_SYNC_ENABLE
+#error Quadrature encoder and spindle sync cannot be enabled at the same time
+#endif
+
+// Board has 2K FRAM
+#undef EEPROM_ENABLE
+#undef EEPROM_IS_FRAM
+#define EEPROM_ENABLE   1
+#define EEPROM_IS_FRAM  1
+
+// Default pin assignments allow only one axis to be ganged or auto squared.
+// B axis pin numbers are used for the ganged/auto squared axis.
+// If a second axis is to be ganged/auto squared pin assignments needs to be changed!
+// Set to 1 to enable, 0 to disable.
+#define X_GANGED        1
+#define X_AUTO_SQUARE   1
+#define Y_GANGED        0
+#define Y_AUTO_SQUARE   0
+#define Z_GANGED        0
+#define Z_AUTO_SQUARE   0
+//
+
+#define X_STEP_PIN      (2u)
+#define X_DIRECTION_PIN (3u)
+#define X_ENABLE_PIN    (10u)
+#define X_LIMIT_PIN     (20u)
+
+#if X_GANGED || X_AUTO_SQUARE
+#define X2_STEP_PIN      (26u)
+#define X2_DIRECTION_PIN (27u)
+#define X2_ENABLE_PIN    (37u)
+#if X_AUTO_SQUARE
+  #define X2_LIMIT_PIN   (28u)
+#endif
+#endif
+
+#define Y_STEP_PIN      (4u)
+#define Y_DIRECTION_PIN (5u)
+#define Y_ENABLE_PIN    (35u)
+#define Y_LIMIT_PIN     (21u)
+
+#if Y_GANGED || Y_AUTO_SQUARE
+#define Y2_STEP_PIN      (26u)
+#define Y2_DIRECTION_PIN (27u)
+#define Y2_ENABLE_PIN    (37u)
+#if Y_AUTO_SQUARE
+  #define Y2_LIMIT_PIN   (28u)
+#endif
+#endif
+
+#define Z_STEP_PIN      (6u)
+#define Z_DIRECTION_PIN (7u)
+#define Z_ENABLE_PIN    (39u)
+#define Z_LIMIT_PIN     (22u)
+
+#if Z_GANGED || Z_AUTO_SQUARE
+#define Z2_STEP_PIN      (26u)
+#define Z2_DIRECTION_PIN (27u)
+#define Z2_ENABLE_PIN    (37u)
+#if Z_AUTO_SQUARE
+  #define Z2_LIMIT_PIN   (28u)
+#endif
+#endif
+
+#if N_AXIS > 3
+#define A_STEP_PIN      (8u)
+#define A_DIRECTION_PIN (9u)
+#define A_ENABLE_PIN    (38u)
+#define A_LIMIT_PIN     (23u)
+#endif
+
+#if N_AXIS > 4
+#define B_STEP_PIN      (26u)
+#define B_DIRECTION_PIN (27u)
+#define B_ENABLE_PIN    (37u)
+#define B_LIMIT_PIN     (28u)
+#endif
+
+// Define spindle enable and spindle direction output pins.
+#define SPINDLE_ENABLE_PIN      (12u)
+#define SPINDLE_DIRECTION_PIN   (11u)
+#define SPINDLEPWMPIN           (13u) // NOTE: only pin 12 or pin 13 can be assigned!
+
+// Define flood and mist coolant enable output pins.
+#define COOLANT_FLOOD_PIN   (19u)
+#define COOLANT_MIST_PIN    (18u)
+
+// Define user-control CONTROLs (cycle start, reset, feed hold, door) input pins.
+#define RESET_PIN           (40u)
+#define FEED_HOLD_PIN       (16u)
+#define CYCLE_START_PIN     (17u)
+#define SAFETY_DOOR_PIN     (29u)
+
+// Define probe switch input pin.
+#define PROBE_PIN           (15u)
+
+#if QEI_ENABLE
+#define QEI_A_PIN           (36u)
+#define QEI_B_PIN           (30u)
+//#define QEI_INDEX_PIN       (36u)
+#define QEI_SELECT_PIN      (31u)
+#endif
+
+#if SPINDLE_SYNC_ENABLE
+#define SPINDLE_INDEX_PIN   (31u) // ST2
+#define SPINDLE_PULSE_PIN   (14u) // ST3
+#endif
+
+// Define auxillary input pins
+#define AUXINPUT0_PIN       (36u) // ST0
+#if !QEI_ENABLE
+#define AUXINPUT1_PIN       (30u) // ST1
+#if !SPINDLE_SYNC_ENABLE
+#define AUXINPUT2_PIN       (31u) // ST2
+#define AUXINPUT3_PIN       (14u) // ST3
+#endif
+#endif
+
+// Define auxillary output pins
+#define AUXOUTPUT0_PIN      (34U)
+#define AUXOUTPUT1_PIN      (32U)
+#define AUXOUTPUT2_PIN      (33U)
+
+#if KEYPAD_ENABLE
+#define KEYPAD_STROBE_PIN   (41U)
+#endif
+
+#if EEPROM_ENABLE || KEYPAD_ENABLE
+#define I2C_PORT    4
+#define I2C_SCL4    (24u) // Not used, for info only
+#define I2C_SDA4    (25u) // Not used, for info only
+#endif
+>>>>>>> .r6468
diff --git a/grblHAL_Teensy4/src/T41BB5X_Pro_map.h.mine b/grblHAL_Teensy4/src/T41BB5X_Pro_map.h.mine
new file mode 100644
index 0000000..4d7cb94
--- /dev/null
+++ b/grblHAL_Teensy4/src/T41BB5X_Pro_map.h.mine
@@ -0,0 +1,164 @@
+/*
+  T41BB5X_Pro_map.h - driver code for IMXRT1062 processor (on Teensy 4.1 board)
+
+  Part of grblHAL
+
+  Board by Phil Barrett: https://github.com/phil-barrett/grblHAL-teensy-4.x
+
+  Copyright (c) 2021 Terje Io
+
+  Grbl is free software: you can redistribute it and/or modify
+  it under the terms of the GNU General Public License as published by
+  the Free Software Foundation, either version 3 of the License, or
+  (at your option) any later version.
+
+  Grbl is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+  GNU General Public License for more details.
+
+  You should have received a copy of the GNU General Public License
+  along with Grbl.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#define BOARD_NAME "T41BB5X Pro"
+#define HAS_BOARD_INIT
+
+#if N_AXIS > 5
+#error Max number of axes is 5 for T41U5XBB
+#endif
+
+#if QEI_ENABLE && SPINDLE_SYNC_ENABLE
+#error Quadrature encoder and spindle sync cannot be enabled at the same time
+#endif
+
+// Board has 2K FRAM
+#undef EEPROM_ENABLE
+#undef EEPROM_IS_FRAM
+#define EEPROM_ENABLE   1
+#define EEPROM_IS_FRAM  1
+
+// Default pin assignments allow only one axis to be ganged or auto squared.
+// B axis pin numbers are used for the ganged/auto squared axis.
+// If a second axis is to be ganged/auto squared pin assignments needs to be changed!
+// Set to 1 to enable, 0 to disable.
+#define X_GANGED        0
+#define X_AUTO_SQUARE   0
+#define Y_GANGED        0
+#define Y_AUTO_SQUARE   0
+#define Z_GANGED        0
+#define Z_AUTO_SQUARE   0
+//
+
+#define X_STEP_PIN      (2u)
+#define X_DIRECTION_PIN (3u)
+#define X_ENABLE_PIN    (10u)
+#define X_LIMIT_PIN     (20u)
+
+#if X_GANGED || X_AUTO_SQUARE
+#define X2_STEP_PIN      (26u)
+#define X2_DIRECTION_PIN (27u)
+#define X2_ENABLE_PIN    (37u)
+#if X_AUTO_SQUARE
+  #define X2_LIMIT_PIN   (28u)
+#endif
+#endif
+
+#define Y_STEP_PIN      (4u)
+#define Y_DIRECTION_PIN (5u)
+#define Y_ENABLE_PIN    (35u)
+#define Y_LIMIT_PIN     (21u)
+
+#if Y_GANGED || Y_AUTO_SQUARE
+#define Y2_STEP_PIN      (26u)
+#define Y2_DIRECTION_PIN (27u)
+#define Y2_ENABLE_PIN    (37u)
+#if Y_AUTO_SQUARE
+  #define Y2_LIMIT_PIN   (28u)
+#endif
+#endif
+
+#define Z_STEP_PIN      (6u)
+#define Z_DIRECTION_PIN (7u)
+#define Z_ENABLE_PIN    (39u)
+#define Z_LIMIT_PIN     (22u)
+
+#if Z_GANGED || Z_AUTO_SQUARE
+#define Z2_STEP_PIN      (26u)
+#define Z2_DIRECTION_PIN (27u)
+#define Z2_ENABLE_PIN    (37u)
+#if Z_AUTO_SQUARE
+  #define Z2_LIMIT_PIN   (28u)
+#endif
+#endif
+
+#if N_AXIS > 3
+#define A_STEP_PIN      (8u)
+#define A_DIRECTION_PIN (9u)
+#define A_ENABLE_PIN    (38u)
+#define A_LIMIT_PIN     (23u)
+#endif
+
+#if N_AXIS > 4
+#define B_STEP_PIN      (26u)
+#define B_DIRECTION_PIN (27u)
+#define B_ENABLE_PIN    (37u)
+#define B_LIMIT_PIN     (28u)
+#endif
+
+// Define spindle enable and spindle direction output pins.
+#define SPINDLE_ENABLE_PIN      (12u)
+#define SPINDLE_DIRECTION_PIN   (11u)
+#define SPINDLEPWMPIN           (13u) // NOTE: only pin 12 or pin 13 can be assigned!
+
+// Define flood and mist coolant enable output pins.
+#define COOLANT_FLOOD_PIN   (19u)
+#define COOLANT_MIST_PIN    (18u)
+
+// Define user-control CONTROLs (cycle start, reset, feed hold, door) input pins.
+#define RESET_PIN           (40u)
+#define FEED_HOLD_PIN       (16u)
+#define CYCLE_START_PIN     (17u)
+#define SAFETY_DOOR_PIN     (29u)
+
+// Define probe switch input pin.
+#define PROBE_PIN           (15u)
+
+#if QEI_ENABLE
+#define QEI_A_PIN           (36u)
+#define QEI_B_PIN           (30u)
+//#define QEI_INDEX_PIN       (36u)
+#define QEI_SELECT_PIN      (31u)
+#endif
+
+#if SPINDLE_SYNC_ENABLE
+#define SPINDLE_INDEX_PIN   (31u) // ST2
+#define SPINDLE_PULSE_PIN   (14u) // ST3
+#endif
+
+// Define auxillary input pins
+#define AUXINPUT0_PIN       (36u) // ST0
+#if !QEI_ENABLE
+#define AUXINPUT1_PIN       (30u) // ST1
+#if !SPINDLE_SYNC_ENABLE
+#define AUXINPUT2_PIN       (31u) // ST2
+#define AUXINPUT3_PIN       (14u) // ST3
+#endif
+#endif
+
+// Define auxillary output pins
+#define AUXOUTPUT0_PIN      (32U)
+#define AUXOUTPUT1_PIN      (33U)
+#define AUXOUTPUT2_PIN      (34U)
+#define AUX_N_OUT 3
+#define AUX_OUT_MASK 0b111
+
+#if KEYPAD_ENABLE
+#define KEYPAD_STROBE_PIN   (41U)
+#endif
+
+#if EEPROM_ENABLE || KEYPAD_ENABLE
+#define I2C_PORT    4
+#define I2C_SCL4    (24u) // Not used, for info only
+#define I2C_SDA4    (25u) // Not used, for info only
+#endif
diff --git a/grblHAL_Teensy4/src/T41BB5X_Pro_map.h.r6421 b/grblHAL_Teensy4/src/T41BB5X_Pro_map.h.r6421
new file mode 100644
index 0000000..4d7cb94
--- /dev/null
+++ b/grblHAL_Teensy4/src/T41BB5X_Pro_map.h.r6421
@@ -0,0 +1,164 @@
+/*
+  T41BB5X_Pro_map.h - driver code for IMXRT1062 processor (on Teensy 4.1 board)
+
+  Part of grblHAL
+
+  Board by Phil Barrett: https://github.com/phil-barrett/grblHAL-teensy-4.x
+
+  Copyright (c) 2021 Terje Io
+
+  Grbl is free software: you can redistribute it and/or modify
+  it under the terms of the GNU General Public License as published by
+  the Free Software Foundation, either version 3 of the License, or
+  (at your option) any later version.
+
+  Grbl is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+  GNU General Public License for more details.
+
+  You should have received a copy of the GNU General Public License
+  along with Grbl.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#define BOARD_NAME "T41BB5X Pro"
+#define HAS_BOARD_INIT
+
+#if N_AXIS > 5
+#error Max number of axes is 5 for T41U5XBB
+#endif
+
+#if QEI_ENABLE && SPINDLE_SYNC_ENABLE
+#error Quadrature encoder and spindle sync cannot be enabled at the same time
+#endif
+
+// Board has 2K FRAM
+#undef EEPROM_ENABLE
+#undef EEPROM_IS_FRAM
+#define EEPROM_ENABLE   1
+#define EEPROM_IS_FRAM  1
+
+// Default pin assignments allow only one axis to be ganged or auto squared.
+// B axis pin numbers are used for the ganged/auto squared axis.
+// If a second axis is to be ganged/auto squared pin assignments needs to be changed!
+// Set to 1 to enable, 0 to disable.
+#define X_GANGED        0
+#define X_AUTO_SQUARE   0
+#define Y_GANGED        0
+#define Y_AUTO_SQUARE   0
+#define Z_GANGED        0
+#define Z_AUTO_SQUARE   0
+//
+
+#define X_STEP_PIN      (2u)
+#define X_DIRECTION_PIN (3u)
+#define X_ENABLE_PIN    (10u)
+#define X_LIMIT_PIN     (20u)
+
+#if X_GANGED || X_AUTO_SQUARE
+#define X2_STEP_PIN      (26u)
+#define X2_DIRECTION_PIN (27u)
+#define X2_ENABLE_PIN    (37u)
+#if X_AUTO_SQUARE
+  #define X2_LIMIT_PIN   (28u)
+#endif
+#endif
+
+#define Y_STEP_PIN      (4u)
+#define Y_DIRECTION_PIN (5u)
+#define Y_ENABLE_PIN    (35u)
+#define Y_LIMIT_PIN     (21u)
+
+#if Y_GANGED || Y_AUTO_SQUARE
+#define Y2_STEP_PIN      (26u)
+#define Y2_DIRECTION_PIN (27u)
+#define Y2_ENABLE_PIN    (37u)
+#if Y_AUTO_SQUARE
+  #define Y2_LIMIT_PIN   (28u)
+#endif
+#endif
+
+#define Z_STEP_PIN      (6u)
+#define Z_DIRECTION_PIN (7u)
+#define Z_ENABLE_PIN    (39u)
+#define Z_LIMIT_PIN     (22u)
+
+#if Z_GANGED || Z_AUTO_SQUARE
+#define Z2_STEP_PIN      (26u)
+#define Z2_DIRECTION_PIN (27u)
+#define Z2_ENABLE_PIN    (37u)
+#if Z_AUTO_SQUARE
+  #define Z2_LIMIT_PIN   (28u)
+#endif
+#endif
+
+#if N_AXIS > 3
+#define A_STEP_PIN      (8u)
+#define A_DIRECTION_PIN (9u)
+#define A_ENABLE_PIN    (38u)
+#define A_LIMIT_PIN     (23u)
+#endif
+
+#if N_AXIS > 4
+#define B_STEP_PIN      (26u)
+#define B_DIRECTION_PIN (27u)
+#define B_ENABLE_PIN    (37u)
+#define B_LIMIT_PIN     (28u)
+#endif
+
+// Define spindle enable and spindle direction output pins.
+#define SPINDLE_ENABLE_PIN      (12u)
+#define SPINDLE_DIRECTION_PIN   (11u)
+#define SPINDLEPWMPIN           (13u) // NOTE: only pin 12 or pin 13 can be assigned!
+
+// Define flood and mist coolant enable output pins.
+#define COOLANT_FLOOD_PIN   (19u)
+#define COOLANT_MIST_PIN    (18u)
+
+// Define user-control CONTROLs (cycle start, reset, feed hold, door) input pins.
+#define RESET_PIN           (40u)
+#define FEED_HOLD_PIN       (16u)
+#define CYCLE_START_PIN     (17u)
+#define SAFETY_DOOR_PIN     (29u)
+
+// Define probe switch input pin.
+#define PROBE_PIN           (15u)
+
+#if QEI_ENABLE
+#define QEI_A_PIN           (36u)
+#define QEI_B_PIN           (30u)
+//#define QEI_INDEX_PIN       (36u)
+#define QEI_SELECT_PIN      (31u)
+#endif
+
+#if SPINDLE_SYNC_ENABLE
+#define SPINDLE_INDEX_PIN   (31u) // ST2
+#define SPINDLE_PULSE_PIN   (14u) // ST3
+#endif
+
+// Define auxillary input pins
+#define AUXINPUT0_PIN       (36u) // ST0
+#if !QEI_ENABLE
+#define AUXINPUT1_PIN       (30u) // ST1
+#if !SPINDLE_SYNC_ENABLE
+#define AUXINPUT2_PIN       (31u) // ST2
+#define AUXINPUT3_PIN       (14u) // ST3
+#endif
+#endif
+
+// Define auxillary output pins
+#define AUXOUTPUT0_PIN      (32U)
+#define AUXOUTPUT1_PIN      (33U)
+#define AUXOUTPUT2_PIN      (34U)
+#define AUX_N_OUT 3
+#define AUX_OUT_MASK 0b111
+
+#if KEYPAD_ENABLE
+#define KEYPAD_STROBE_PIN   (41U)
+#endif
+
+#if EEPROM_ENABLE || KEYPAD_ENABLE
+#define I2C_PORT    4
+#define I2C_SCL4    (24u) // Not used, for info only
+#define I2C_SDA4    (25u) // Not used, for info only
+#endif
diff --git a/grblHAL_Teensy4/src/T41BB5X_Pro_map.h.r6468 b/grblHAL_Teensy4/src/T41BB5X_Pro_map.h.r6468
new file mode 100644
index 0000000..b528197
--- /dev/null
+++ b/grblHAL_Teensy4/src/T41BB5X_Pro_map.h.r6468
@@ -0,0 +1,162 @@
+/*
+  T41BB5X_Pro_map.h - driver code for IMXRT1062 processor (on Teensy 4.1 board)
+
+  Part of grblHAL
+
+  Board by Phil Barrett: https://github.com/phil-barrett/grblHAL-teensy-4.x
+
+  Copyright (c) 2021 Terje Io
+
+  Grbl is free software: you can redistribute it and/or modify
+  it under the terms of the GNU General Public License as published by
+  the Free Software Foundation, either version 3 of the License, or
+  (at your option) any later version.
+
+  Grbl is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+  GNU General Public License for more details.
+
+  You should have received a copy of the GNU General Public License
+  along with Grbl.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#define BOARD_NAME "T41BB5X Pro"
+#define HAS_BOARD_INIT
+
+#if N_AXIS > 5
+#error Max number of axes is 5 for T41U5XBB
+#endif
+
+#if QEI_ENABLE && SPINDLE_SYNC_ENABLE
+#error Quadrature encoder and spindle sync cannot be enabled at the same time
+#endif
+
+// Board has 2K FRAM
+#undef EEPROM_ENABLE
+#undef EEPROM_IS_FRAM
+#define EEPROM_ENABLE   1
+#define EEPROM_IS_FRAM  1
+
+// Default pin assignments allow only one axis to be ganged or auto squared.
+// B axis pin numbers are used for the ganged/auto squared axis.
+// If a second axis is to be ganged/auto squared pin assignments needs to be changed!
+// Set to 1 to enable, 0 to disable.
+#define X_GANGED        1
+#define X_AUTO_SQUARE   1
+#define Y_GANGED        0
+#define Y_AUTO_SQUARE   0
+#define Z_GANGED        0
+#define Z_AUTO_SQUARE   0
+//
+
+#define X_STEP_PIN      (2u)
+#define X_DIRECTION_PIN (3u)
+#define X_ENABLE_PIN    (10u)
+#define X_LIMIT_PIN     (20u)
+
+#if X_GANGED || X_AUTO_SQUARE
+#define X2_STEP_PIN      (26u)
+#define X2_DIRECTION_PIN (27u)
+#define X2_ENABLE_PIN    (37u)
+#if X_AUTO_SQUARE
+  #define X2_LIMIT_PIN   (28u)
+#endif
+#endif
+
+#define Y_STEP_PIN      (4u)
+#define Y_DIRECTION_PIN (5u)
+#define Y_ENABLE_PIN    (35u)
+#define Y_LIMIT_PIN     (21u)
+
+#if Y_GANGED || Y_AUTO_SQUARE
+#define Y2_STEP_PIN      (26u)
+#define Y2_DIRECTION_PIN (27u)
+#define Y2_ENABLE_PIN    (37u)
+#if Y_AUTO_SQUARE
+  #define Y2_LIMIT_PIN   (28u)
+#endif
+#endif
+
+#define Z_STEP_PIN      (6u)
+#define Z_DIRECTION_PIN (7u)
+#define Z_ENABLE_PIN    (39u)
+#define Z_LIMIT_PIN     (22u)
+
+#if Z_GANGED || Z_AUTO_SQUARE
+#define Z2_STEP_PIN      (26u)
+#define Z2_DIRECTION_PIN (27u)
+#define Z2_ENABLE_PIN    (37u)
+#if Z_AUTO_SQUARE
+  #define Z2_LIMIT_PIN   (28u)
+#endif
+#endif
+
+#if N_AXIS > 3
+#define A_STEP_PIN      (8u)
+#define A_DIRECTION_PIN (9u)
+#define A_ENABLE_PIN    (38u)
+#define A_LIMIT_PIN     (23u)
+#endif
+
+#if N_AXIS > 4
+#define B_STEP_PIN      (26u)
+#define B_DIRECTION_PIN (27u)
+#define B_ENABLE_PIN    (37u)
+#define B_LIMIT_PIN     (28u)
+#endif
+
+// Define spindle enable and spindle direction output pins.
+#define SPINDLE_ENABLE_PIN      (12u)
+#define SPINDLE_DIRECTION_PIN   (11u)
+#define SPINDLEPWMPIN           (13u) // NOTE: only pin 12 or pin 13 can be assigned!
+
+// Define flood and mist coolant enable output pins.
+#define COOLANT_FLOOD_PIN   (19u)
+#define COOLANT_MIST_PIN    (18u)
+
+// Define user-control CONTROLs (cycle start, reset, feed hold, door) input pins.
+#define RESET_PIN           (40u)
+#define FEED_HOLD_PIN       (16u)
+#define CYCLE_START_PIN     (17u)
+#define SAFETY_DOOR_PIN     (29u)
+
+// Define probe switch input pin.
+#define PROBE_PIN           (15u)
+
+#if QEI_ENABLE
+#define QEI_A_PIN           (36u)
+#define QEI_B_PIN           (30u)
+//#define QEI_INDEX_PIN       (36u)
+#define QEI_SELECT_PIN      (31u)
+#endif
+
+#if SPINDLE_SYNC_ENABLE
+#define SPINDLE_INDEX_PIN   (31u) // ST2
+#define SPINDLE_PULSE_PIN   (14u) // ST3
+#endif
+
+// Define auxillary input pins
+#define AUXINPUT0_PIN       (36u) // ST0
+#if !QEI_ENABLE
+#define AUXINPUT1_PIN       (30u) // ST1
+#if !SPINDLE_SYNC_ENABLE
+#define AUXINPUT2_PIN       (31u) // ST2
+#define AUXINPUT3_PIN       (14u) // ST3
+#endif
+#endif
+
+// Define auxillary output pins
+#define AUXOUTPUT0_PIN      (34U)
+#define AUXOUTPUT1_PIN      (32U)
+#define AUXOUTPUT2_PIN      (33U)
+
+#if KEYPAD_ENABLE
+#define KEYPAD_STROBE_PIN   (41U)
+#endif
+
+#if EEPROM_ENABLE || KEYPAD_ENABLE
+#define I2C_PORT    4
+#define I2C_SCL4    (24u) // Not used, for info only
+#define I2C_SDA4    (25u) // Not used, for info only
+#endif
diff --git a/grblHAL_Teensy4/src/T41U5XBB.c b/grblHAL_Teensy4/src/T41U5XBB.c
index 0ce25b5..3a82951 100644
--- a/grblHAL_Teensy4/src/T41U5XBB.c
+++ b/grblHAL_Teensy4/src/T41U5XBB.c
@@ -31,13 +31,15 @@
 
 #include "grbl/protocol.h"
 
-static input_signal_t *stx;
-static gpio_t aux_out[AUX_N_OUT];
+static uint_fast8_t aux_n_in, aux_n_out;
+static input_signal_t *aux_in;
+static output_signal_t *aux_out;
+static ioport_bus_t out = {0};
+static char input_ports[56] = "", output_ports[56] = "";
 
 static void aux_settings_load (void);
 static status_code_t aux_set_invert_out (setting_id_t id, uint_fast16_t int_value);
 static uint32_t aux_get_invert_out (setting_id_t setting);
-static char input_ports[30]; //
 
 static const setting_group_detail_t aux_groups[] = {
     { Group_Root, Group_AuxPorts, "Aux ports"}
@@ -45,7 +47,7 @@ static const setting_group_detail_t aux_groups[] = {
 
 static const setting_detail_t aux_settings[] = {
     { Settings_IoPort_InvertIn, Group_AuxPorts, "Invert I/O Port inputs", NULL, Format_Bitfield, input_ports, NULL, NULL, Setting_NonCore, &settings.ioport.invert_in.mask },
-    { Settings_IoPort_InvertOut, Group_AuxPorts, "Invert I/O Port outputs", NULL, Format_Bitfield, "Port 0,Port 1,Port 2", NULL, NULL, Setting_NonCoreFn, aux_set_invert_out, aux_get_invert_out },
+    { Settings_IoPort_InvertOut, Group_AuxPorts, "Invert I/O Port outputs", NULL, Format_Bitfield, output_ports, NULL, NULL, Setting_NonCoreFn, aux_set_invert_out, aux_get_invert_out },
 };
 
 static setting_details_t details = {
@@ -64,25 +66,25 @@ static setting_details_t *on_get_settings (void)
 
 static void aux_settings_load (void)
 {
-    uint_fast8_t idx = AUX_N_OUT;
+    uint_fast8_t idx = aux_n_out;
 
     do {
         idx--;
-        DIGITAL_OUT(aux_out[idx], (settings.ioport.invert_out.mask >> idx) & 0x01);
+        DIGITAL_OUT((*(aux_out[idx].port)), (settings.ioport.invert_out.mask >> idx) & 0x01);
     } while(idx);
 }
 
 static status_code_t aux_set_invert_out (setting_id_t id, uint_fast16_t value)
 {
     ioport_bus_t invert;
-    invert.mask = (uint8_t)value & AUX_OUT_MASK;
+    invert.mask = (uint8_t)value & out.mask;
 
     if(invert.mask != settings.ioport.invert_out.mask) {
-        uint_fast8_t idx = AUX_N_OUT;
+        uint_fast8_t idx = aux_n_out;
         do {
             idx--;
             if(((settings.ioport.invert_out.mask >> idx) & 0x01) != ((invert.mask >> idx) & 0x01))
-                DIGITAL_OUT(aux_out[idx], !DIGITAL_IN(aux_out[idx]));
+                DIGITAL_OUT((*(aux_out[idx].port)), !DIGITAL_IN((*(aux_out[idx].port))));
         } while(idx);
 
         settings.ioport.invert_out.mask = invert.mask;
@@ -98,8 +100,8 @@ static uint32_t aux_get_invert_out (setting_id_t setting)
 
 static void digital_out (uint8_t port, bool on)
 {
-    if(port < AUX_N_OUT)
-        DIGITAL_OUT(aux_out[port], ((settings.ioport.invert_out.mask >> port) & 0x01) ? !on : on);
+    if(port < aux_n_out)
+        DIGITAL_OUT((*(aux_out[port].port)), ((settings.ioport.invert_out.mask >> port) & 0x01) ? !on : on);
 }
 
 inline static __attribute__((always_inline)) int32_t get_input (gpio_t *gpio, bool invert, wait_mode_t wait_mode, float timeout)
@@ -130,8 +132,8 @@ static int32_t wait_on_input (bool digital, uint8_t port, wait_mode_t wait_mode,
     int32_t value = -1;
 
     if(digital) {
-        if(port < hal.port.num_digital_in)
-            value = get_input(stx[port].port, (settings.ioport.invert_in.mask << port) & 0x01, wait_mode, timeout);
+        if(port < aux_n_in)
+            value = get_input(aux_in[port].port, (settings.ioport.invert_in.mask << port) & 0x01, wait_mode, timeout);
     }
 //    else if(port == 0)
 //        value = analogRead(41);
@@ -145,29 +147,31 @@ static int32_t wait_on_input (bool digital, uint8_t port, wait_mode_t wait_mode,
     return value;
 }
 
-void board_init (pin_group_pins_t *aux_inputs)
+void board_init (pin_group_pins_t *aux_inputs, pin_group_pins_t *aux_outputs)
 {
-    stx = aux_inputs->pins;
+    aux_in = aux_inputs->pins.inputs;
+    aux_out = aux_outputs->pins.outputs;
 
-    hal.port.digital_out = digital_out;
     hal.port.wait_on_input = wait_on_input;
-//    hal.port.num_analog_in  = 1;
-    hal.port.num_digital_in = aux_inputs->n_pins;
-    hal.port.num_digital_out = AUX_N_OUT;
+    hal.port.digital_out = digital_out;
+    hal.port.num_digital_in = aux_n_in = aux_inputs->n_pins;
+    hal.port.num_digital_out = aux_n_out = aux_outputs->n_pins;
 
     details.on_get_settings = grbl.on_get_settings;
     grbl.on_get_settings = on_get_settings;
 
-    uint32_t i;
+    uint_fast8_t i;
 
-    for(i = 0; i < hal.port.num_digital_in; i++) {
+    for(i = 0; i < min(hal.port.num_digital_in, 8); i++) {
         strcat(input_ports, i == 0 ? "Port " : ",Port ");
         strcat(input_ports, uitoa(i));
     }
 
-    pinModeOutput(&aux_out[0], AUXOUTPUT0_PIN);
-    pinModeOutput(&aux_out[1], AUXOUTPUT1_PIN);
-    pinModeOutput(&aux_out[2], AUXOUTPUT2_PIN);
+    for(i = 0; i < min(hal.port.num_digital_out, 8) ; i++) {
+        out.mask = (out.mask << 1) | 1;
+        strcat(output_ports, i == 0 ? "Port " : ",Port ");
+        strcat(output_ports, uitoa(i));
+    }
 
 //    analog_init();
 }
diff --git a/grblHAL_Teensy4/src/T41U5XBB_map.h b/grblHAL_Teensy4/src/T41U5XBB_map.h
index ffd91d0..f16a2c2 100644
--- a/grblHAL_Teensy4/src/T41U5XBB_map.h
+++ b/grblHAL_Teensy4/src/T41U5XBB_map.h
@@ -136,8 +136,6 @@
 #define AUXOUTPUT0_PIN      (31u) // AUX0
 #define AUXOUTPUT1_PIN      (32u) // AUX1
 #define AUXOUTPUT2_PIN      (33u) // AUX2
-#define AUX_N_OUT 3
-#define AUX_OUT_MASK 0b111
 
 #if KEYPAD_ENABLE
 #define KEYPAD_STROBE_PIN   (41u) // I2C ST
diff --git a/grblHAL_Teensy4/src/driver.c b/grblHAL_Teensy4/src/driver.c
index 90f31a3..b893a79 100644
--- a/grblHAL_Teensy4/src/driver.c
+++ b/grblHAL_Teensy4/src/driver.c
@@ -267,6 +267,16 @@ static gpio_t QEI_A, QEI_B;
   static gpio_t AuxIn3;
 #endif
 
+#ifdef AUXOUTPUT0_PIN
+  static gpio_t AuxOut0;
+#endif
+#ifdef AUXOUTPUT1_PIN
+  static gpio_t AuxOut1;
+#endif
+#ifdef AUXOUTPUT2_PIN
+  static gpio_t AuxOut2;
+#endif
+
 input_signal_t inputpin[] = {
 #if ESTOP_ENABLE
     { .id = Input_EStop,          .port = &Reset,          .pin = RESET_PIN,           .group = PinGroup_Control },
@@ -339,7 +349,103 @@ input_signal_t inputpin[] = {
 #endif
 };
 
-static pin_group_pins_t aux_inputs = {0}, limit_inputs = {0};
+static output_signal_t outputpin[] = {
+    { .id = Output_StepX,           .port = &stepX,         .pin = X_STEP_PIN,              .group = PinGroup_StepperStep },
+    { .id = Output_StepY,           .port = &stepY,         .pin = Y_STEP_PIN,              .group = PinGroup_StepperStep },
+    { .id = Output_StepZ,           .port = &stepZ,         .pin = Z_STEP_PIN,              .group = PinGroup_StepperStep },
+#ifdef A_AXIS
+    { .id = Output_StepA,           .port = &stepA,         .pin = A_STEP_PIN,              .group = PinGroup_StepperStep },
+#endif
+#ifdef B_AXIS
+    { .id = Output_StepB,           .port = &stepB,         .pin = B_STEP_PIN,              .group = PinGroup_StepperStep },
+#endif
+#ifdef C_AXIS
+    { .id = Output_StepC,           .port = &stepC,         .pin = C_STEP_PIN,              .group = PinGroup_StepperStep },
+#endif
+#ifdef X2_STEP_PIN
+    { .id = Output_StepX,           .port = &stepX2,        .pin = X2_STEP_PIN,             .group = PinGroup_StepperStep },
+#endif
+#ifdef Y2_STEP_PIN
+    { .id = Output_StepY,           .port = &stepY2,        .pin = Y2_STEP_PIN,             .group = PinGroup_StepperStep },
+#endif
+#ifdef Z2_STEP_PIN
+    { .id = Output_StepZ,           .port = &stepZ2,        .pin = Z2_STEP_PIN,             .group = PinGroup_StepperStep },
+#endif
+    { .id = Output_DirX,            .port = &dirX,          .pin = X_DIRECTION_PIN,         .group = PinGroup_StepperDir },
+    { .id = Output_DirY,            .port = &dirY,          .pin = Y_DIRECTION_PIN,         .group = PinGroup_StepperDir },
+    { .id = Output_DirZ,            .port = &dirZ,          .pin = Z_DIRECTION_PIN,         .group = PinGroup_StepperDir },
+#ifdef A_AXIS
+    { .id = Output_DirA,            .port = &dirA,          .pin = A_DIRECTION_PIN,         .group = PinGroup_StepperDir },
+#endif
+#ifdef B_AXIS
+    { .id = Output_DirB,            .port = &dirB,          .pin = B_DIRECTION_PIN,         .group = PinGroup_StepperDir },
+#endif
+#ifdef C_AXIS
+    { .id = Output_DirC,            .port = &dirC,          .pin = C_DIRECTION_PIN,         .group = PinGroup_StepperDir },
+#endif
+#ifdef X2_DIRECTION_PIN
+    { .id = Output_DirX,            .port = &dirX2,         .pin = X2_DIRECTION_PIN,        .group = PinGroup_StepperDir },
+#endif
+#ifdef Y2_DIRECTION_PIN
+    { .id = Output_DirY,            .port = &dirY2,         .pin = Y2_DIRECTION_PIN,        .group = PinGroup_StepperDir },
+#endif
+#ifdef Z2_DIRECTION_PIN
+    { .id = Output_DirZ,            .port = &dirZ2,         .pin = Z2_DIRECTION_PIN,        .group = PinGroup_StepperDir },
+#endif
+#if !TRINAMIC_ENABLE
+#ifdef STEPPERS_ENABLE_PIN
+    { .id = Output_StepperEnable,   .port = &steppersEnable, .pin = STEPPERS_ENABLE_PIN,    .group = PinGroup_StepperEnable },
+#endif
+#ifdef X_ENABLE_PIN
+    { .id = Output_StepperEnableX,  .port = &enableX,       .pin = X_ENABLE_PIN,            .group = PinGroup_StepperEnable },
+#endif
+#ifdef Y_ENABLE_PIN
+    { .id = Output_StepperEnableY,  .port = &enableY,       .pin = Y_ENABLE_PIN,            .group = PinGroup_StepperEnable },
+#endif
+#ifdef Z_ENABLE_PIN
+    { .id = Output_StepperEnableZ,  .port = &enableZ,       .pin = Z_ENABLE_PIN,            .group = PinGroup_StepperEnable },
+#endif
+#ifdef A_ENABLE_PIN
+    { .id = Output_StepperEnableA,  .port = &enableA,       .pin = A_ENABLE_PIN,            .group = PinGroup_StepperEnable },
+#endif
+#ifdef B_ENABLE_PIN
+    { .id = Output_StepperEnableB,  .port = &enableB,       .pin = B_ENABLE_PIN,            .group = PinGroup_StepperEnable },
+#endif
+#ifdef C_ENABLE_PIN
+    { .id = Output_StepperEnableC,  .port = &enableC,       .pin = C_ENABLE_PIN,            .group = PinGroup_StepperEnable },
+#endif
+#ifdef X2_ENABLE_PIN
+    { .id = Output_StepperEnableX,  .port = &enableX2,      .pin = X2_ENABLE_PIN,           .group = PinGroup_StepperEnable },
+#endif
+#ifdef Y2_ENABLE_PIN
+    { .id = Output_StepperEnableY,  .port = &enableY2,      .pin = Y2_ENABLE_PIN,           .group = PinGroup_StepperEnable },
+#endif
+#ifdef Z2_ENABLE_PIN
+    { .id = Output_StepperEnableZ,  .port = &enableZ2,      .pin = Z2_ENABLE_PIN,           .group = PinGroup_StepperEnable },
+#endif
+#endif
+#if !VFD_SPINDLE
+    { .id = Output_SpindleOn,       .port = &spindleEnable, .pin = SPINDLE_ENABLE_PIN,      .group = PinGroup_SpindleControl },
+#ifdef SPINDLE_DIRECTION_PIN
+    { .id = Output_SpindleDir,      .port = &spindleDir,    .pin = SPINDLE_DIRECTION_PIN,   .group = PinGroup_SpindleControl },
+#endif
+#endif
+    { .id = Output_CoolantMist,     .port = &Mist,          .pin = COOLANT_FLOOD_PIN,       .group = PinGroup_Coolant },
+#ifdef COOLANT_MIST_PIN
+    { .id = Output_CoolantFlood,    .port = &Flood,         .pin = COOLANT_MIST_PIN,        .group = PinGroup_Coolant },
+#endif
+#ifdef AUXOUTPUT0_PIN
+    { .id = Output_Aux0,            .port = &AuxOut0,       .pin = AUXOUTPUT0_PIN,          .group = PinGroup_AuxOutput },
+#endif
+#ifdef AUXOUTPUT1_PIN
+    { .id = Output_Aux1,            .port = &AuxOut1,       .pin = AUXOUTPUT1_PIN,          .group = PinGroup_AuxOutput },
+#endif
+#ifdef AUXOUTPUT2_PIN
+    { .id = Output_Aux2,            .port = &AuxOut2,       .pin = AUXOUTPUT2_PIN,          .group = PinGroup_AuxOutput }
+#endif
+};
+
+static pin_group_pins_t limit_inputs = {0};
 
 #if USB_SERIAL_CDC || QEI_ENABLE
 #define ADD_MSEVENT 1
@@ -1010,11 +1116,11 @@ static void limitsEnable (bool on, bool homing)
 
     do {
         i--;
-        limit_inputs.pins[i].gpio.reg->ISR = limit_inputs.pins[i].gpio.bit;       // Clear interrupt.
+        limit_inputs.pins.inputs[i].gpio.reg->ISR = limit_inputs.pins.inputs[i].gpio.bit;       // Clear interrupt.
         if(on)
-            limit_inputs.pins[i].gpio.reg->IMR |= limit_inputs.pins[i].gpio.bit;  // Enable interrupt.
+            limit_inputs.pins.inputs[i].gpio.reg->IMR |= limit_inputs.pins.inputs[i].gpio.bit;  // Enable interrupt.
         else
-            limit_inputs.pins[i].gpio.reg->IMR &= ~limit_inputs.pins[i].gpio.bit; // Disable interrupt.
+            limit_inputs.pins.inputs[i].gpio.reg->IMR &= ~limit_inputs.pins.inputs[i].gpio.bit; // Disable interrupt.
     } while(i);
 }
 
@@ -1738,6 +1844,14 @@ static bool driver_setup (settings_t *settings)
     driver_settings.trinamic.driver_enable.mask = AXES_BITMASK;
 #endif
 
+    /*************************
+     *  Output signals init  *
+     *************************/
+
+    uint32_t i;
+    for(i = 0 ; i < sizeof(outputpin) / sizeof(output_signal_t); i++)
+        pinModeOutput(outputpin[i].port, outputpin[i].pin);
+
     /******************
      *  Stepper init  *
      ******************/
@@ -1773,79 +1887,6 @@ static bool driver_setup (settings_t *settings)
     TMR2_ENBL = 1;
 #endif
 
-    pinModeOutput(&stepX, X_STEP_PIN);
-    pinModeOutput(&dirX, X_DIRECTION_PIN);
-#ifdef X_ENABLE_PIN
-    pinModeOutput(&enableX, X_ENABLE_PIN);
-#endif
-#ifdef X2_STEP_PIN
-    pinModeOutput(&stepX2, X2_STEP_PIN);
-#endif
-#ifdef X2_DIRECTION_PIN
-    pinModeOutput(&dirX2, X2_DIRECTION_PIN);
-#endif
-#ifdef X2_ENABLE_PIN
-    pinModeOutput(&enableX2, X2_ENABLE_PIN);
-#endif
-
-    pinModeOutput(&stepY, Y_STEP_PIN);
-    pinModeOutput(&dirY, Y_DIRECTION_PIN);
-#ifdef Y_ENABLE_PIN
-    pinModeOutput(&enableY, Y_ENABLE_PIN);
-#endif
-#ifdef Y2_STEP_PIN
-    pinModeOutput(&stepY2, Y2_STEP_PIN);
-#endif
-#ifdef Y2_DIRECTION_PIN
-    pinModeOutput(&dirY2, Y2_DIRECTION_PIN);
-#endif
-#ifdef Y2_ENABLE_PIN
-    pinModeOutput(&enableY2, Y2_ENABLE_PIN);
-#endif
-
-    pinModeOutput(&stepZ, Z_STEP_PIN);
-    pinModeOutput(&dirZ, Z_DIRECTION_PIN);
-#ifdef Z_ENABLE_PIN
-    pinModeOutput(&enableZ, Z_ENABLE_PIN);
-#endif
-#ifdef Z2_STEP_PIN
-    pinModeOutput(&stepZ2, Z2_STEP_PIN);
-#endif
-#ifdef Z2_DIRECTION_PIN
-    pinModeOutput(&dirZ2, Z2_DIRECTION_PIN);
-#endif
-#ifdef Z2_ENABLE_PIN
-    pinModeOutput(&enableZ2, Z2_ENABLE_PIN);
-#endif
-
-#ifdef A_AXIS
-    pinModeOutput(&stepA, A_STEP_PIN);
-    pinModeOutput(&dirA, A_DIRECTION_PIN);
-  #ifdef A_ENABLE_PIN
-    pinModeOutput(&enableA, A_ENABLE_PIN);
-  #endif
-#endif
-
-#ifdef B_AXIS
-    pinModeOutput(&stepB, B_STEP_PIN);
-    pinModeOutput(&dirB, B_DIRECTION_PIN);
-  #ifdef B_ENABLE_PIN
-    pinModeOutput(&enableB, B_ENABLE_PIN);
-  #endif
-#endif
-
-#ifdef C_AXIS
-    pinModeOutput(&stepC, C_STEP_PIN);
-    pinModeOutput(&dirC, C_DIRECTION_PIN);
-  #ifdef C_ENABLE_PIN
-    pinModeOutput(&enableC, C_ENABLE_PIN);
-  #endif
-#endif
-
-#ifdef STEPPERS_ENABLE_PIN
-    pinModeOutput(&steppersEnable, STEPPERS_ENABLE_PIN);
-#endif
-
    /****************************
     *  Software debounce init  *
     ****************************/
@@ -2106,7 +2147,7 @@ bool driver_init (void)
         options[strlen(options) - 1] = '\0';
 
     hal.info = "iMXRT1062";
-    hal.driver_version = "210509";
+    hal.driver_version = "210526";
 #ifdef BOARD_NAME
     hal.board = BOARD_NAME;
 #endif
@@ -2256,25 +2297,36 @@ bool driver_init (void)
 
     uint32_t i;
     input_signal_t *signal;
+    static pin_group_pins_t aux_inputs = {0}, aux_outputs = {0};
 
     for(i = 0 ; i < sizeof(inputpin) / sizeof(input_signal_t); i++) {
         signal = &inputpin[i];
 
         if(signal->group == PinGroup_AuxInput) {
-            if(aux_inputs.pins == NULL)
-                aux_inputs.pins = signal;
+            if(aux_inputs.pins.inputs == NULL)
+                aux_inputs.pins.inputs = signal;
             aux_inputs.n_pins++;
         }
 
         if(signal->group == PinGroup_Limit) {
-            if(limit_inputs.pins == NULL)
-                limit_inputs.pins = signal;
+            if(limit_inputs.pins.inputs == NULL)
+                limit_inputs.pins.inputs = signal;
             limit_inputs.n_pins++;
         }
     }
 
+    output_signal_t *output;
+    for(i = 0 ; i < sizeof(outputpin) / sizeof(output_signal_t); i++) {
+        output = &outputpin[i];
+        if(output->group == PinGroup_AuxOutput) {
+            if(aux_outputs.pins.outputs == NULL)
+                aux_outputs.pins.outputs = output;
+            aux_outputs.n_pins++;
+        }
+    }
+
 #ifdef HAS_BOARD_INIT
-    board_init(&aux_inputs);
+    board_init(&aux_inputs, &aux_outputs);
 #endif
 
 #if ETHERNET_ENABLE
diff --git a/grblHAL_Teensy4/src/driver.h b/grblHAL_Teensy4/src/driver.h
index e45f7da..5ec91ce 100644
--- a/grblHAL_Teensy4/src/driver.h
+++ b/grblHAL_Teensy4/src/driver.h
@@ -283,9 +283,19 @@ typedef struct {
     volatile bool debounce;
 } input_signal_t;
 
+typedef struct {
+    pin_function_t id;
+    gpio_t *port;
+    uint8_t pin;
+    pin_group_t group;
+} output_signal_t;
+
 typedef struct {
     uint8_t n_pins;
-    input_signal_t *pins;
+    union {
+        input_signal_t *inputs;
+        output_signal_t *outputs;
+    } pins;
 } pin_group_pins_t;
 
 // The following struct is pulled from the Teensy Library core, Copyright (c) 2019 PJRC.COM, LLC.
@@ -304,7 +314,7 @@ void pinModeOutput (gpio_t *gpio, uint8_t pin);
 uint32_t xTaskGetTickCount();
 
 #ifdef HAS_BOARD_INIT
-void board_init(pin_group_pins_t *aux_inputs);
+void board_init(pin_group_pins_t *aux_inputs, pin_group_pins_t *aux_outputs);
 #endif
 
 #ifdef UART_DEBUG
diff --git a/grblHAL_Teensy4/src/enet.c b/grblHAL_Teensy4/src/enet.c
index 34deb85..0ce6b4b 100644
--- a/grblHAL_Teensy4/src/enet.c
+++ b/grblHAL_Teensy4/src/enet.c
@@ -36,6 +36,7 @@
 #include "grbl/report.h"
 #include "grbl/nvs_buffer.h"
 
+#include "networking/ftpd.h"
 #include "networking/TCPStream.h"
 #include "networking/WsStream.h"
 
@@ -50,9 +51,12 @@ static void report_options (bool newopt)
 {
     on_report_options(newopt);
 
-    if(newopt)
+    if(newopt) {
         hal.stream.write(",ETH");
-    else {
+#if FTP_ENABLE
+        hal.stream.write(",FTP");
+#endif
+    } else {
         hal.stream.write("[IP:");
         hal.stream.write(IPAddress);
         hal.stream.write("]" ASCII_EOL);
@@ -81,6 +85,13 @@ static void netif_status_callback (struct netif *netif)
     }
 #endif
 
+#if FTP_ENABLE
+    if(network.services.ftp && !services.ftp) {
+        ftpd_init();
+        services.ftp = On;
+    }
+#endif
+
 #if WEBSOCKET_ENABLE
     if(network.services.websocket && !services.websocket) {
         WsStreamInit();
@@ -105,7 +116,11 @@ void grbl_enet_poll (void)
         if(services.telnet)
           TCPStreamPoll();
 #endif
-    #if WEBSOCKET_ENABLE
+#if FTP_ENABLE
+        if(services.ftp)
+            ftpd_poll();
+#endif
+#if WEBSOCKET_ENABLE
         if(services.websocket)
           WsStreamPoll();
 #endif
@@ -159,16 +174,28 @@ static const setting_group_detail_t ethernet_groups [] = {
     { Group_Root, Group_Networking, "Networking" }
 };
 
+#if TELNET_ENABLE && WEBSOCKET_ENABLE && FTP_ENABLE
+static const char netservices[] = "Telnet,Websocket,FTP";
+static const char servicemap[] = "11";
+#endif
 #if TELNET_ENABLE && WEBSOCKET_ENABLE && HTTP_ENABLE
 static const char netservices[] = "Telnet,Websocket,HTTP";
+static const char servicemap[] = "7";
 #endif
-#if TELNET_ENABLE && WEBSOCKET_ENABLE && !HTTP_ENABLE
+#if TELNET_ENABLE && WEBSOCKET_ENABLE && !FTP_ENABLE && !HTTP_ENABLE
 static const char netservices[] = "Telnet,Websocket";
+static const char servicemap[] = "2";
 #endif
 #if TELNET_ENABLE && !WEBSOCKET_ENABLE && !HTTP_ENABLE
 static const char netservices[] = "Telnet";
+static const char servicemap[] = "1";
 #endif
 
+static const network_services_t aserv = {
+    .telnet = 1,
+    .ftp = 1
+};
+
 PROGMEM static const setting_detail_t ethernet_settings[] = {
     { Setting_NetworkServices, Group_Networking, "Network Services", NULL, Format_Bitfield, netservices, NULL, NULL, Setting_NonCore, &ethernet.services.mask, NULL, NULL },
     { Setting_Hostname, Group_Networking, "Hostname", NULL, Format_String, "x(64)", NULL, "64", Setting_NonCore, ethernet.hostname, NULL, NULL },
@@ -291,6 +318,10 @@ void ethernet_settings_restore (void)
     ethernet.services.telnet = On;
 #endif
 
+#if FTP_ENABLE
+    ethernet.services.ftp = On;
+#endif
+
 #if HTTP_ENABLE
     ethernet.services.http = On;
 #endif
diff --git a/grblHAL_Teensy4/src/grbl b/grblHAL_Teensy4/src/grbl
index cad8d28..37a2ea9 160000
--- a/grblHAL_Teensy4/src/grbl
+++ b/grblHAL_Teensy4/src/grbl
@@ -1 +1 @@
-Subproject commit cad8d28ca8fbfd9c665d097948a5a250dc865d47
+Subproject commit 37a2ea92913961e969f5a7259fa1246c6cee30ed
diff --git a/grblHAL_Teensy4/src/my_machine.h b/grblHAL_Teensy4/src/my_machine.h
index 16c5861..ceb78e5 100644
--- a/grblHAL_Teensy4/src/my_machine.h
+++ b/grblHAL_Teensy4/src/my_machine.h
@@ -69,6 +69,7 @@ N_AXIS has a default value of 3, edit grbl\config.h to increase.
 
 #if ETHERNET_ENABLE > 0
 #define TELNET_ENABLE           1 // Telnet daemon - requires Ethernet streaming enabled.
+#define FTP_ENABLE              0 // Ftp daemon - requires SD card enabled. !!DO NOT ENABLE - there is a bug in the FatFS library that has to be fixed first!!
 #define WEBSOCKET_ENABLE        1 // Websocket daemon - requires Ethernet streaming enabled.
 #define NETWORK_HOSTNAME        "GRBL"
 #define NETWORK_IPMODE          1 // 0 = static, 1 = DHCP, 2 = AutoIP
diff --git a/grblHAL_Teensy4/src/networking b/grblHAL_Teensy4/src/networking
index fbface3..a70f4f9 160000
--- a/grblHAL_Teensy4/src/networking
+++ b/grblHAL_Teensy4/src/networking
@@ -1 +1 @@
-Subproject commit fbface3a869613b3589a136db0c4ec1f75c471aa
+Subproject commit a70f4f9386354c9656180f2d45cfe228601b9885
diff --git a/grblHAL_Teensy4/src/odometer b/grblHAL_Teensy4/src/odometer
index f179fe3..955c579 160000
--- a/grblHAL_Teensy4/src/odometer
+++ b/grblHAL_Teensy4/src/odometer
@@ -1 +1 @@
-Subproject commit f179fe355198e7b64052783d5c442880ea94a965
+Subproject commit 955c579e8e84bd2ac097ea359140604696ca906c
diff --git a/grblHAL_Teensy4/src/sdcard b/grblHAL_Teensy4/src/sdcard
index 0327163..b965526 160000
--- a/grblHAL_Teensy4/src/sdcard
+++ b/grblHAL_Teensy4/src/sdcard
@@ -1 +1 @@
-Subproject commit 03271638aab8d381d44945df38ae7fd5344667e7
+Subproject commit b96552616a2cab815151ca255fc0b98c870f0f3c