Tweak probe preheat to wait for cooldown, bypass block on unload prime

Merge branch 'bugfix-2.0.x' into CR-6Devel
Use -g3 to include macros in debug symbols (#21052 )
2021-02-14 14:13:41 -05:00 · 2021-02-14 12:06:14 -05:00 · 2021-02-12 08:33:27 -06:00 · 2021-02-12 07:49:07 -06:00 · 2021-02-12 05:00:41 -06:00 · 2021-02-12 04:55:06 -06:00
502 changed files with 16718 additions and 6492 deletions
@@ -19,9 +19,9 @@
 # along with this program.  If not, see <https://www.gnu.org/licenses/>.
 #

-# Our automatic versioning scheme generates the following file
-# NEVER put it in the repository
+# Generated files
 _Version.h
+bdf2u8g

 #
 # OS
@@ -70,8 +70,8 @@
 // @section info

 // Author info of this build printed to the host during boot and M115
-#define STRING_CONFIG_H_AUTHOR "(none, default config)" // Who made the changes.
-//#define CUSTOM_VERSION_FILE Version.h // Path from the root directory (no quotes)
+#define STRING_CONFIG_H_AUTHOR "Tinymachines" // Who made the changes.
+#define CUSTOM_VERSION_FILE Version.h // Path from the root directory (no quotes)

 /**
 * *** VENDORS PLEASE READ ***
@@ -103,14 +103,14 @@
 *
 * :[-1, 0, 1, 2, 3, 4, 5, 6, 7]
 */
-#define SERIAL_PORT 0
+#define SERIAL_PORT 1

 /**
 * Select a secondary serial port on the board to use for communication with the host.
 * Currently Ethernet (-2) is only supported on Teensy 4.1 boards.
 * :[-2, -1, 0, 1, 2, 3, 4, 5, 6, 7]
 */
-//#define SERIAL_PORT_2 -1
+//#define SERIAL_PORT_2 3

 /**
 * This setting determines the communication speed of the printer.
@@ -121,14 +121,14 @@
 *
 * :[2400, 9600, 19200, 38400, 57600, 115200, 250000, 500000, 1000000]
 */
-#define BAUDRATE 250000
+#define BAUDRATE 115200

 // Enable the Bluetooth serial interface on AT90USB devices
 //#define BLUETOOTH

 // Choose the name from boards.h that matches your setup
 #ifndef MOTHERBOARD
-  #define MOTHERBOARD BOARD_GMARSH_EBAB
+  #define MOTHERBOARD BOARD_CREALITY_V453
 #endif

 // Name displayed in the LCD "Ready" message and Info menu
@@ -321,9 +321,6 @@
  //#define PSU_DEFAULT_OFF         // Keep power off until enabled directly with M80
  //#define PSU_POWERUP_DELAY 250   // (ms) Delay for the PSU to warm up to full power

-  //#define PSU_POWERUP_GCODE  "M355 S1"  // G-code to run after power-on (e.g., case light on)
-  //#define PSU_POWEROFF_GCODE "M355 S0"  // G-code to run before power-off (e.g., case light off)
-
  //#define AUTO_POWER_CONTROL      // Enable automatic control of the PS_ON pin
  #if ENABLED(AUTO_POWER_CONTROL)
    #define AUTO_POWER_FANS         // Turn on PSU if fans need power
@@ -416,12 +413,12 @@
 #define TEMP_SENSOR_5 0
 #define TEMP_SENSOR_6 0
 #define TEMP_SENSOR_7 0
-#define TEMP_SENSOR_BED 0
+#define TEMP_SENSOR_BED 1
 #define TEMP_SENSOR_PROBE 0
 #define TEMP_SENSOR_CHAMBER 0

 // Dummy thermistor constant temperature readings, for use with 998 and 999
-#define DUMMY_THERMISTOR_998_VALUE 25
+#define DUMMY_THERMISTOR_998_VALUE  25
 #define DUMMY_THERMISTOR_999_VALUE 100

 // Resistor values when using MAX31865 sensors (-5) on TEMP_SENSOR_0 / 1
@@ -445,7 +442,7 @@

 // Below this temperature the heater will be switched off
 // because it probably indicates a broken thermistor wire.
-#define HEATER_0_MINTEMP   5
+#define HEATER_0_MINTEMP   0
 #define HEATER_1_MINTEMP   5
 #define HEATER_2_MINTEMP   5
 #define HEATER_3_MINTEMP   5
@@ -453,12 +450,12 @@
 #define HEATER_5_MINTEMP   5
 #define HEATER_6_MINTEMP   5
 #define HEATER_7_MINTEMP   5
-#define BED_MINTEMP        5
+#define BED_MINTEMP        0

 // Above this temperature the heater will be switched off.
 // This can protect components from overheating, but NOT from shorts and failures.
 // (Use MINTEMP for thermistor short/failure protection.)
-#define HEATER_0_MAXTEMP 275
+#define HEATER_0_MAXTEMP 300
 #define HEATER_1_MAXTEMP 275
 #define HEATER_2_MAXTEMP 275
 #define HEATER_3_MAXTEMP 275
@@ -466,7 +463,7 @@
 #define HEATER_5_MAXTEMP 275
 #define HEATER_6_MAXTEMP 275
 #define HEATER_7_MAXTEMP 275
-#define BED_MAXTEMP      150
+#define BED_MAXTEMP      120

 //===========================================================================
 //============================= PID Settings ================================
@@ -492,9 +489,9 @@
    #define DEFAULT_Ki_LIST {   1.08,   1.08 }
    #define DEFAULT_Kd_LIST { 114.00, 114.00 }
  #else
-    #define DEFAULT_Kp  22.20
-    #define DEFAULT_Ki   1.08
-    #define DEFAULT_Kd 114.00
+    #define DEFAULT_Kp  14.32
+    #define DEFAULT_Ki   0.81
+    #define DEFAULT_Kd 63.12
  #endif
 #endif // PIDTEMP

@@ -515,7 +512,7 @@
 * heater. If your configuration is significantly different than this and you don't understand
 * the issues involved, don't use bed PID until someone else verifies that your hardware works.
 */
-//#define PIDTEMPBED
+#define PIDTEMPBED

 //#define BED_LIMIT_SWITCHING

@@ -531,11 +528,11 @@
  //#define MIN_BED_POWER 0
  //#define PID_BED_DEBUG // Sends debug data to the serial port.

-  // 120V 250W silicone heater into 4mm borosilicate (MendelMax 1.5+)
-  // from FOPDT model - kp=.39 Tp=405 Tdead=66, Tc set to 79.2, aggressive factor of .15 (vs .1, 1, 10)
-  #define DEFAULT_bedKp 10.00
-  #define DEFAULT_bedKi .023
-  #define DEFAULT_bedKd 305.4
+  //120V 250W silicone heater into 4mm borosilicate (MendelMax 1.5+)
+  //from FOPDT model - kp=.39 Tp=405 Tdead=66, Tc set to 79.2, aggressive factor of .15 (vs .1, 1, 10)
+  #define DEFAULT_bedKp 79.49
+  #define DEFAULT_bedKi 1.17
+  #define DEFAULT_bedKd 1349.52

  // FIND YOUR OWN: "M303 E-1 C8 S90" to run autotune on the bed at 90 degreesC for 8 cycles.
 #endif // PIDTEMPBED
@@ -565,7 +562,7 @@
 * Note: For Bowden Extruders make this large enough to allow load/unload.
 */
 #define PREVENT_LENGTHY_EXTRUDE
-#define EXTRUDE_MAXLENGTH 200
+#define EXTRUDE_MAXLENGTH 1000

 //===========================================================================
 //======================== Thermal Runaway Protection =======================
@@ -586,7 +583,7 @@

 #define THERMAL_PROTECTION_HOTENDS // Enable thermal protection for all extruders
 #define THERMAL_PROTECTION_BED     // Enable thermal protection for the heated bed
-#define THERMAL_PROTECTION_CHAMBER // Enable thermal protection for the heated chamber
+//#define THERMAL_PROTECTION_CHAMBER // Enable thermal protection for the heated chamber

 //===========================================================================
 //============================= Mechanical Settings =========================
@@ -663,8 +660,6 @@
 *
 * A4988 is assumed for unspecified drivers.
 *
- * Use TMC2208/TMC2208_STANDALONE for TMC2225 drivers and TMC2209/TMC2209_STANDALONE for TMC2226 drivers.
- *
 * Options: A4988, A5984, DRV8825, LV8729, L6470, L6474, POWERSTEP01,
 *          TB6560, TB6600, TMC2100,
 *          TMC2130, TMC2130_STANDALONE, TMC2160, TMC2160_STANDALONE,
@@ -673,15 +668,15 @@
 *          TMC5130, TMC5130_STANDALONE, TMC5160, TMC5160_STANDALONE
 * :['A4988', 'A5984', 'DRV8825', 'LV8729', 'L6470', 'L6474', 'POWERSTEP01', 'TB6560', 'TB6600', 'TMC2100', 'TMC2130', 'TMC2130_STANDALONE', 'TMC2160', 'TMC2160_STANDALONE', 'TMC2208', 'TMC2208_STANDALONE', 'TMC2209', 'TMC2209_STANDALONE', 'TMC26X', 'TMC26X_STANDALONE', 'TMC2660', 'TMC2660_STANDALONE', 'TMC5130', 'TMC5130_STANDALONE', 'TMC5160', 'TMC5160_STANDALONE']
 */
-#define X_DRIVER_TYPE  A4988
-#define Y_DRIVER_TYPE  A4988
-#define Z_DRIVER_TYPE  A4988
+#define X_DRIVER_TYPE  TMC2209_STANDALONE
+#define Y_DRIVER_TYPE  TMC2209_STANDALONE
+#define Z_DRIVER_TYPE  TMC2209_STANDALONE
 //#define X2_DRIVER_TYPE A4988
 //#define Y2_DRIVER_TYPE A4988
 //#define Z2_DRIVER_TYPE A4988
 //#define Z3_DRIVER_TYPE A4988
 //#define Z4_DRIVER_TYPE A4988
-#define E0_DRIVER_TYPE A4988
+#define E0_DRIVER_TYPE TMC2209_STANDALONE
 //#define E1_DRIVER_TYPE A4988
 //#define E2_DRIVER_TYPE A4988
 //#define E3_DRIVER_TYPE A4988
@@ -736,14 +731,14 @@
 * Override with M92
 *                                      X, Y, Z, E0 [, E1[, E2...]]
 */
-#define DEFAULT_AXIS_STEPS_PER_UNIT   { 80, 80, 400, 500 }
+#define DEFAULT_AXIS_STEPS_PER_UNIT   { 80, 80, 400, 93 }

 /**
 * Default Max Feed Rate (mm/s)
 * Override with M203
 *                                      X, Y, Z, E0 [, E1[, E2...]]
 */
-#define DEFAULT_MAX_FEEDRATE          { 300, 300, 5, 25 }
+#define DEFAULT_MAX_FEEDRATE          { 500, 500, 5, 25 }

 //#define LIMITED_MAX_FR_EDITING        // Limit edit via M203 or LCD to DEFAULT_MAX_FEEDRATE * 2
 #if ENABLED(LIMITED_MAX_FR_EDITING)
@@ -756,7 +751,7 @@
 * Override with M201
 *                                      X, Y, Z, E0 [, E1[, E2...]]
 */
-#define DEFAULT_MAX_ACCELERATION      { 3000, 3000, 100, 10000 }
+#define DEFAULT_MAX_ACCELERATION      { 500, 500, 100, 1000 }

 //#define LIMITED_MAX_ACCEL_EDITING     // Limit edit via M201 or LCD to DEFAULT_MAX_ACCELERATION * 2
 #if ENABLED(LIMITED_MAX_ACCEL_EDITING)
@@ -771,9 +766,9 @@
 *   M204 R    Retract Acceleration
 *   M204 T    Travel Acceleration
 */
-#define DEFAULT_ACCELERATION          3000    // X, Y, Z and E acceleration for printing moves
-#define DEFAULT_RETRACT_ACCELERATION  3000    // E acceleration for retracts
-#define DEFAULT_TRAVEL_ACCELERATION   3000    // X, Y, Z acceleration for travel (non printing) moves
+#define DEFAULT_ACCELERATION          500    // X, Y, Z and E acceleration for printing moves
+#define DEFAULT_RETRACT_ACCELERATION  1000    // E acceleration for retracts
+#define DEFAULT_TRAVEL_ACCELERATION   1000    // X, Y, Z acceleration for travel (non printing) moves

 /**
 * Default Jerk limits (mm/s)
@@ -783,7 +778,7 @@
 * When changing speed and direction, if the difference is less than the
 * value set here, it may happen instantaneously.
 */
-//#define CLASSIC_JERK
+#define CLASSIC_JERK
 #if ENABLED(CLASSIC_JERK)
  #define DEFAULT_XJERK 10.0
  #define DEFAULT_YJERK 10.0
@@ -807,7 +802,7 @@
 *   https://blog.kyneticcnc.com/2018/10/computing-junction-deviation-for-marlin.html
 */
 #if DISABLED(CLASSIC_JERK)
-  #define JUNCTION_DEVIATION_MM 0.013 // (mm) Distance from real junction edge
+  #define JUNCTION_DEVIATION_MM 0.05 // (mm) Distance from real junction edge
  #define JD_HANDLE_SMALL_SEGMENTS    // Use curvature estimation instead of just the junction angle
                                      // for small segments (< 1mm) with large junction angles (> 135°).
 #endif
@@ -820,7 +815,7 @@
 *
 * See https://github.com/synthetos/TinyG/wiki/Jerk-Controlled-Motion-Explained
 */
-//#define S_CURVE_ACCELERATION
+#define S_CURVE_ACCELERATION

 //===========================================================================
 //============================= Z Probe Options =============================
@@ -839,7 +834,7 @@
 #define Z_MIN_PROBE_USES_Z_MIN_ENDSTOP_PIN

 // Force the use of the probe for Z-axis homing
-//#define USE_PROBE_FOR_Z_HOMING
+#define USE_PROBE_FOR_Z_HOMING

 /**
 * Z_MIN_PROBE_PIN
@@ -883,7 +878,7 @@
 * Use the nozzle as the probe, as with a conductive
 * nozzle system or a piezo-electric smart effector.
 */
-//#define NOZZLE_AS_PROBE
+#define NOZZLE_AS_PROBE

 /**
 * Z Servo Probe, such as an endstop switch on a rotating arm.
@@ -986,21 +981,30 @@
 *     |    [-]    |
 *     O-- FRONT --+
 */
-#define NOZZLE_TO_PROBE_OFFSET { 10, 10, 0 }
+#define NOZZLE_TO_PROBE_OFFSET { 0, 0, 0.2 }

 // Most probes should stay away from the edges of the bed, but
 // with NOZZLE_AS_PROBE this can be negative for a wider probing area.
-#define PROBING_MARGIN 10
+#define PROBING_MARGIN 5

 // X and Y axis travel speed (mm/min) between probes
 #define XY_PROBE_SPEED (133*60)

 // Feedrate (mm/min) for the first approach when double-probing (MULTIPLE_PROBING == 2)
-#define Z_PROBE_SPEED_FAST (4*60)
+#define Z_PROBE_SPEED_FAST ((4 * 60) / 2)

 // Feedrate (mm/min) for the "accurate" probe of each point
 #define Z_PROBE_SPEED_SLOW (Z_PROBE_SPEED_FAST / 2)

+// Fail to probe if the probe does not indicate itself as active.
+// This may be a switch indicating proper deployment, or an optical switch to report the carriage is near the bed.
+#define PROBE_ACTIVE_INPUT
+#if ENABLED(PROBE_ACTIVE_INPUT)
+  #define PROBE_ACTIVE_INPUT_STATE LOW // State indicating probe is active
+  //#define PROBE_ENABLE_PIN PC6 // Override default pin
+#endif
+
+
 /**
 * Probe Activation Switch
 * A switch indicating proper deployment, or an optical
@@ -1012,19 +1016,18 @@
  //#define PROBE_ACTIVATION_SWITCH_PIN PC6 // Override default pin
 #endif

-/**
- * Tare Probe (determine zero-point) prior to each probe.
- * Useful for a strain gauge or piezo sensor that needs to factor out
- * elements such as cables pulling on the carriage.
- */
-//#define PROBE_TARE
+// Probe should be tared prior to each probe
+// Useful for strain or piezo sensors which must exclude strain such
+// as that from cables or bowden cables pulling on the carriage.
+#define PROBE_TARE
 #if ENABLED(PROBE_TARE)
-  #define PROBE_TARE_TIME  200    // (ms) Time to hold tare pin
-  #define PROBE_TARE_DELAY 200    // (ms) Delay after tare before
+  #define PROBE_TARE_TIME  200    // Time to hold tare pin (milliseconds)
+  #define PROBE_TARE_DELAY 200    // Delay after tare before (milliseconds)
  #define PROBE_TARE_STATE HIGH   // State to write pin for tare
  //#define PROBE_TARE_PIN PA5    // Override default pin
-  #if ENABLED(PROBE_ACTIVATION_SWITCH)
-    //#define PROBE_TARE_ONLY_WHILE_INACTIVE  // Fail to tare/probe if PROBE_ACTIVATION_SWITCH is active
+  #if ENABLED(PROBE_ACTIVE_INPUT)
+    // Fail to tare/probe if PROBE_ACTIVE_INPUT reports the probe to be active
+    //#define PROBE_TARE_ONLY_WHILE_INACTIVE
  #endif
 #endif

@@ -1037,7 +1040,7 @@
 * A total of 2 does fast/slow probes with a weighted average.
 * A total of 3 or more adds more slow probes, taking the average.
 */
-//#define MULTIPLE_PROBING 2
+#define MULTIPLE_PROBING 2
 //#define EXTRA_PROBING    1

 /**
@@ -1055,18 +1058,18 @@
 *     But: `M851 Z+1` with a CLEARANCE of 2  =>  2mm from bed to nozzle.
 */
 #define Z_CLEARANCE_DEPLOY_PROBE   10 // Z Clearance for Deploy/Stow
-#define Z_CLEARANCE_BETWEEN_PROBES  5 // Z Clearance between probe points
-#define Z_CLEARANCE_MULTI_PROBE     5 // Z Clearance between multiple probes
+#define Z_CLEARANCE_BETWEEN_PROBES  3 // Z Clearance between probe points
+#define Z_CLEARANCE_MULTI_PROBE     3 // Z Clearance between multiple probes
 //#define Z_AFTER_PROBING           5 // Z position after probing is done

-#define Z_PROBE_LOW_POINT          -2 // Farthest distance below the trigger-point to go before stopping
+#define Z_PROBE_LOW_POINT          -3 // Farthest distance below the trigger-point to go before stopping

 // For M851 give a range for adjusting the Z probe offset
-#define Z_PROBE_OFFSET_RANGE_MIN -20
-#define Z_PROBE_OFFSET_RANGE_MAX 20
+#define Z_PROBE_OFFSET_RANGE_MIN -10
+#define Z_PROBE_OFFSET_RANGE_MAX 10

 // Enable the M48 repeatability test to test probe accuracy
-//#define Z_MIN_PROBE_REPEATABILITY_TEST
+#define Z_MIN_PROBE_REPEATABILITY_TEST

 // Before deploy/stow pause for user confirmation
 //#define PAUSE_BEFORE_DEPLOY_STOW
@@ -1081,18 +1084,18 @@
 * These options are most useful for the BLTouch probe, but may also improve
 * readings with inductive probes and piezo sensors.
 */
-//#define PROBING_HEATERS_OFF       // Turn heaters off when probing
+#define PROBING_HEATERS_OFF       // Turn heaters off when probing
 #if ENABLED(PROBING_HEATERS_OFF)
-  //#define WAIT_FOR_BED_HEATER     // Wait for bed to heat back up between probes (to improve accuracy)
+  #define WAIT_FOR_BED_HEATER     // Wait for bed to heat back up between probes (to improve accuracy)
 #endif
 //#define PROBING_FANS_OFF          // Turn fans off when probing
-//#define PROBING_STEPPERS_OFF      // Turn steppers off (unless needed to hold position) when probing
+#define PROBING_STEPPERS_OFF      // Turn steppers off (unless needed to hold position) when probing
 //#define DELAY_BEFORE_PROBING 200  // (ms) To prevent vibrations from triggering piezo sensors

 // Require minimum nozzle and/or bed temperature for probing
-//#define PREHEAT_BEFORE_PROBING
+#define PREHEAT_BEFORE_PROBING
 #if ENABLED(PREHEAT_BEFORE_PROBING)
-  #define PROBING_NOZZLE_TEMP 120   // (°C) Only applies to E0 at this time
+  #define PROBING_NOZZLE_TEMP 170   // (°C) Only applies to E0 at this time
  #define PROBING_BED_TEMP     50
 #endif

@@ -1120,14 +1123,14 @@
 // @section machine

 // Invert the stepper direction. Change (or reverse the motor connector) if an axis goes the wrong way.
-#define INVERT_X_DIR false
-#define INVERT_Y_DIR true
-#define INVERT_Z_DIR false
+#define INVERT_X_DIR true
+#define INVERT_Y_DIR false
+#define INVERT_Z_DIR true

 // @section extruder

 // For direct drive extruder v9 set to true, for geared extruder set to false.
-#define INVERT_E0_DIR false
+#define INVERT_E0_DIR true
 #define INVERT_E1_DIR false
 #define INVERT_E2_DIR false
 #define INVERT_E3_DIR false
@@ -1138,14 +1141,14 @@

 // @section homing

-//#define NO_MOTION_BEFORE_HOMING // Inhibit movement until all axes have been homed. Also enable HOME_AFTER_DEACTIVATE for extra safety.
-//#define HOME_AFTER_DEACTIVATE   // Require rehoming after steppers are deactivated. Also enable NO_MOTION_BEFORE_HOMING for extra safety.
+//#define NO_MOTION_BEFORE_HOMING // Inhibit movement until all axes have been homed
+
 //#define UNKNOWN_Z_NO_RAISE      // Don't raise Z (lower the bed) if Z is "unknown." For beds that fall when Z is powered off.

-//#define Z_HOMING_HEIGHT  4      // (mm) Minimal Z height before homing (G28) for Z clearance above the bed, clamps, ...
+#define Z_HOMING_HEIGHT  15      // (mm) Minimal Z height before homing (G28) for Z clearance above the bed, clamps, ...
                                  // Be sure to have this much clearance over your Z_MAX_POS to prevent grinding.

-//#define Z_AFTER_HOMING  10      // (mm) Height to move to after homing Z
+#define Z_AFTER_HOMING  5      // (mm) Height to move to after homing Z

 // Direction of endstops when homing; 1=MAX, -1=MIN
 // :[-1,1]
@@ -1156,16 +1159,16 @@
 // @section machine

 // The size of the print bed
-#define X_BED_SIZE 200
-#define Y_BED_SIZE 200
+#define X_BED_SIZE 235
+#define Y_BED_SIZE 235

 // Travel limits (mm) after homing, corresponding to endstop positions.
-#define X_MIN_POS 0
-#define Y_MIN_POS 0
+#define X_MIN_POS -5
+#define Y_MIN_POS -2
 #define Z_MIN_POS 0
 #define X_MAX_POS X_BED_SIZE
 #define Y_MAX_POS Y_BED_SIZE
-#define Z_MAX_POS 200
+#define Z_MAX_POS 250

 /**
 * Software Endstops
@@ -1200,56 +1203,16 @@
 * Filament Runout Sensors
 * Mechanical or opto endstops are used to check for the presence of filament.
 *
- * IMPORTANT: Runout will only trigger if Marlin is aware that a print job is running.
- * Marlin knows a print job is running when:
- *  1. Running a print job from media started with M24.
- *  2. The Print Job Timer has been started with M75.
- *  3. The heaters were turned on and PRINTJOB_TIMER_AUTOSTART is enabled.
- *
 * RAMPS-based boards use SERVO3_PIN for the first runout sensor.
 * For other boards you may need to define FIL_RUNOUT_PIN, FIL_RUNOUT2_PIN, etc.
 */
-//#define FILAMENT_RUNOUT_SENSOR
+#define FILAMENT_RUNOUT_SENSOR
 #if ENABLED(FILAMENT_RUNOUT_SENSOR)
  #define FIL_RUNOUT_ENABLED_DEFAULT true // Enable the sensor on startup. Override with M412 followed by M500.
  #define NUM_RUNOUT_SENSORS   1          // Number of sensors, up to one per extruder. Define a FIL_RUNOUT#_PIN for each.
-
-  #define FIL_RUNOUT_STATE     LOW        // Pin state indicating that filament is NOT present.
-  #define FIL_RUNOUT_PULLUP               // Use internal pullup for filament runout pins.
-  //#define FIL_RUNOUT_PULLDOWN           // Use internal pulldown for filament runout pins.
-
-  // Override individually if the runout sensors vary
-  //#define FIL_RUNOUT1_STATE LOW
-  //#define FIL_RUNOUT1_PULLUP
-  //#define FIL_RUNOUT1_PULLDOWN
-
-  //#define FIL_RUNOUT2_STATE LOW
-  //#define FIL_RUNOUT2_PULLUP
-  //#define FIL_RUNOUT2_PULLDOWN
-
-  //#define FIL_RUNOUT3_STATE LOW
-  //#define FIL_RUNOUT3_PULLUP
-  //#define FIL_RUNOUT3_PULLDOWN
-
-  //#define FIL_RUNOUT4_STATE LOW
-  //#define FIL_RUNOUT4_PULLUP
-  //#define FIL_RUNOUT4_PULLDOWN
-
-  //#define FIL_RUNOUT5_STATE LOW
-  //#define FIL_RUNOUT5_PULLUP
-  //#define FIL_RUNOUT5_PULLDOWN
-
-  //#define FIL_RUNOUT6_STATE LOW
-  //#define FIL_RUNOUT6_PULLUP
-  //#define FIL_RUNOUT6_PULLDOWN
-
-  //#define FIL_RUNOUT7_STATE LOW
-  //#define FIL_RUNOUT7_PULLUP
-  //#define FIL_RUNOUT7_PULLDOWN
-
-  //#define FIL_RUNOUT8_STATE LOW
-  //#define FIL_RUNOUT8_PULLUP
-  //#define FIL_RUNOUT8_PULLDOWN
+  #define FIL_RUNOUT_STATE     HIGH       // Pin state indicating that filament is NOT present.
+  #define FIL_RUNOUT_PULLUP             // Use internal pullup for filament runout pins.
+  //#define FIL_RUNOUT_PULLDOWN             // Use internal pulldown for filament runout pins.

  // Set one or more commands to execute on filament runout.
  // (After 'M412 H' Marlin will ask the host to handle the process.)
@@ -1258,7 +1221,7 @@
  // After a runout is detected, continue printing this length of filament
  // before executing the runout script. Useful for a sensor at the end of
  // a feed tube. Requires 4 bytes SRAM per sensor, plus 4 bytes overhead.
-  //#define FILAMENT_RUNOUT_DISTANCE_MM 25
+  #define FILAMENT_RUNOUT_DISTANCE_MM 5

  #ifdef FILAMENT_RUNOUT_DISTANCE_MM
    // Enable this option to use an encoder disc that toggles the runout pin
@@ -1308,7 +1271,7 @@
 */
 //#define AUTO_BED_LEVELING_3POINT
 //#define AUTO_BED_LEVELING_LINEAR
-//#define AUTO_BED_LEVELING_BILINEAR
+#define AUTO_BED_LEVELING_BILINEAR
 //#define AUTO_BED_LEVELING_UBL
 //#define MESH_BED_LEVELING

@@ -1317,7 +1280,7 @@
 * these options to restore the prior leveling state or to always enable
 * leveling immediately after G28.
 */
-//#define RESTORE_LEVELING_AFTER_G28
+#define RESTORE_LEVELING_AFTER_G28
 //#define ENABLE_LEVELING_AFTER_G28

 /**
@@ -1334,16 +1297,13 @@
 * Turn on with the command 'M111 S32'.
 * NOTE: Requires a lot of PROGMEM!
 */
-//#define DEBUG_LEVELING_FEATURE
+#define DEBUG_LEVELING_FEATURE

 #if ANY(MESH_BED_LEVELING, AUTO_BED_LEVELING_BILINEAR, AUTO_BED_LEVELING_UBL)
  // Gradually reduce leveling correction until a set height is reached,
  // at which point movement will be level to the machine's XY plane.
  // The height can be set with M420 Z<height>
  #define ENABLE_LEVELING_FADE_HEIGHT
-  #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
-    #define DEFAULT_LEVELING_FADE_HEIGHT 10.0 // (mm) Default fade height.
-  #endif

  // For Cartesian machines, instead of dividing moves on mesh boundaries,
  // split up moves into short segments like a Delta. This follows the
@@ -1354,13 +1314,14 @@
  /**
   * Enable the G26 Mesh Validation Pattern tool.
   */
-  //#define G26_MESH_VALIDATION
+  #define G26_MESH_VALIDATION
  #if ENABLED(G26_MESH_VALIDATION)
    #define MESH_TEST_NOZZLE_SIZE    0.4  // (mm) Diameter of primary nozzle.
-    #define MESH_TEST_LAYER_HEIGHT   0.2  // (mm) Default layer height for the G26 Mesh Validation Tool.
-    #define MESH_TEST_HOTEND_TEMP  205    // (°C) Default nozzle temperature for the G26 Mesh Validation Tool.
-    #define MESH_TEST_BED_TEMP      60    // (°C) Default bed temperature for the G26 Mesh Validation Tool.
-    #define G26_XY_FEEDRATE         20    // (mm/s) Feedrate for XY Moves for the G26 Mesh Validation Tool.
+    #define MESH_TEST_LAYER_HEIGHT   0.2  // (mm) Default layer height for G26.
+    #define MESH_TEST_HOTEND_TEMP  205    // (°C) Default nozzle temperature for G26.
+    #define MESH_TEST_BED_TEMP      60    // (°C) Default bed temperature for G26.
+    #define G26_XY_FEEDRATE         20    // (mm/s) Feedrate for G26 XY moves.
+    #define G26_XY_FEEDRATE_TRAVEL 100    // (mm/s) Feedrate for G26 XY travel moves.
    #define G26_RETRACT_MULTIPLIER   1.0  // G26 Q (retraction) used by default between mesh test elements.
  #endif

@@ -1369,17 +1330,17 @@
 #if EITHER(AUTO_BED_LEVELING_LINEAR, AUTO_BED_LEVELING_BILINEAR)

  // Set the number of grid points per dimension.
-  #define GRID_MAX_POINTS_X 3
+  #define GRID_MAX_POINTS_X 5
  #define GRID_MAX_POINTS_Y GRID_MAX_POINTS_X

  // Probe along the Y axis, advancing X after each column
-  //#define PROBE_Y_FIRST
+  #define PROBE_Y_FIRST

  #if ENABLED(AUTO_BED_LEVELING_BILINEAR)

    // Beyond the probed grid, continue the implied tilt?
    // Default is to maintain the height of the nearest edge.
-    //#define EXTRAPOLATE_BEYOND_GRID
+    #define EXTRAPOLATE_BEYOND_GRID

    //
    // Experimental Subdivision of the grid by Catmull-Rom method.
@@ -1445,12 +1406,6 @@
  #define LEVEL_CORNERS_HEIGHT      0.0   // (mm) Z height of nozzle at leveling points
  #define LEVEL_CORNERS_Z_HOP       4.0   // (mm) Z height of nozzle between leveling points
  //#define LEVEL_CENTER_TOO              // Move to the center after the last corner
-  //#define LEVEL_CORNERS_USE_PROBE
-  #if ENABLED(LEVEL_CORNERS_USE_PROBE)
-    #define LEVEL_CORNERS_PROBE_TOLERANCE 0.1
-    #define LEVEL_CORNERS_VERIFY_RAISED   // After adjustment triggers the probe, re-probe to verify
-    //#define LEVEL_CORNERS_AUDIO_FEEDBACK
-  #endif

  /**
   * Corner Leveling Order
@@ -1498,7 +1453,7 @@
 // - Move the Z probe (or nozzle) to a defined XY point before Z Homing.
 // - Prevent Z homing when the Z probe is outside bed area.
 //
-//#define Z_SAFE_HOMING
+#define Z_SAFE_HOMING

 #if ENABLED(Z_SAFE_HOMING)
  #define Z_SAFE_HOMING_X_POINT X_CENTER  // X point for Z homing
@@ -1506,7 +1461,7 @@
 #endif

 // Homing speeds (mm/min)
-#define HOMING_FEEDRATE_MM_M { (50*60), (50*60), (4*60) }
+#define HOMING_FEEDRATE_MM_M { (50*60), (50*60), (10*60) }

 // Validate that endstops are triggered on homing moves
 #define VALIDATE_HOMING_ENDSTOPS
@@ -1583,12 +1538,12 @@
 *   M501 - Read settings from EEPROM. (i.e., Throw away unsaved changes)
 *   M502 - Revert settings to "factory" defaults. (Follow with M500 to init the EEPROM.)
 */
-//#define EEPROM_SETTINGS     // Persistent storage with M500 and M501
+#define EEPROM_SETTINGS     // Persistent storage with M500 and M501
 //#define DISABLE_M503        // Saves ~2700 bytes of PROGMEM. Disable for release!
-#define EEPROM_CHITCHAT       // Give feedback on EEPROM commands. Disable to save PROGMEM.
+//#define EEPROM_CHITCHAT       // Give feedback on EEPROM commands. Disable to save PROGMEM.
 #define EEPROM_BOOT_SILENT    // Keep M503 quiet and only give errors during first load
 #if ENABLED(EEPROM_SETTINGS)
-  //#define EEPROM_AUTO_INIT  // Init EEPROM automatically on any errors.
+  #define EEPROM_AUTO_INIT  // Init EEPROM automatically on any errors.
 #endif

 //
@@ -1613,16 +1568,18 @@

 // @section temperature

-// Preheat Constants
+//
+// Preheat Constants - Up to 5 are supported without changes
+//
 #define PREHEAT_1_LABEL       "PLA"
-#define PREHEAT_1_TEMP_HOTEND 180
-#define PREHEAT_1_TEMP_BED     70
-#define PREHEAT_1_FAN_SPEED     0 // Value from 0 to 255
+#define PREHEAT_1_TEMP_HOTEND 200
+#define PREHEAT_1_TEMP_BED     60
+#define PREHEAT_1_FAN_SPEED     255 // Value from 0 to 255

 #define PREHEAT_2_LABEL       "ABS"
 #define PREHEAT_2_TEMP_HOTEND 240
-#define PREHEAT_2_TEMP_BED    110
-#define PREHEAT_2_FAN_SPEED     0 // Value from 0 to 255
+#define PREHEAT_2_TEMP_BED    70
+#define PREHEAT_2_FAN_SPEED     255 // Value from 0 to 255

 /**
 * Nozzle Park
@@ -1635,12 +1592,12 @@
 *    P1  Raise the nozzle always to Z-park height.
 *    P2  Raise the nozzle by Z-park amount, limited to Z_MAX_POS.
 */
-//#define NOZZLE_PARK_FEATURE
+#define NOZZLE_PARK_FEATURE

 #if ENABLED(NOZZLE_PARK_FEATURE)
  // Specify a park position as { X, Y, Z_raise }
  #define NOZZLE_PARK_POINT { (X_MIN_POS + 10), (Y_MAX_POS - 10), 20 }
-  //#define NOZZLE_PARK_X_ONLY          // X move only is required to park
+  #define NOZZLE_PARK_X_ONLY          // X move only is required to park
  //#define NOZZLE_PARK_Y_ONLY          // Y move only is required to park
  #define NOZZLE_PARK_Z_RAISE_MIN   2   // (mm) Always raise Z by at least this distance
  #define NOZZLE_PARK_XY_FEEDRATE 100   // (mm/s) X and Y axes feedrate (also used for delta Z axis)
@@ -1752,7 +1709,10 @@
 *
 * View the current statistics with M78.
 */
-//#define PRINTCOUNTER
+#define PRINTCOUNTER
+#if ENABLED(PRINTCOUNTER)
+  #define PRINTCOUNTER_SAVE_INTERVAL 60 // (minutes) EEPROM save interval during print
+#endif

 /**
 * Password
@@ -1797,9 +1757,9 @@
 * Select the language to display on the LCD. These languages are available:
 *
 *   en, an, bg, ca, cz, da, de, el, el_gr, es, eu, fi, fr, gl, hr, hu, it,
- *   jp_kana, ko_KR, nl, pl, pt, pt_br, ro, ru, sk, tr, uk, vi, zh_CN, zh_TW, test
+ *   jp_kana, ko_KR, nl, pl, pt, pt_br, ro, ru, sk, sv, tr, uk, vi, zh_CN, zh_TW
 *
- * :{ 'en':'English', 'an':'Aragonese', 'bg':'Bulgarian', 'ca':'Catalan', 'cz':'Czech', 'da':'Danish', 'de':'German', 'el':'Greek', 'el_gr':'Greek (Greece)', 'es':'Spanish', 'eu':'Basque-Euskera', 'fi':'Finnish', 'fr':'French', 'gl':'Galician', 'hr':'Croatian', 'hu':'Hungarian', 'it':'Italian', 'jp_kana':'Japanese', 'ko_KR':'Korean (South Korea)', 'nl':'Dutch', 'pl':'Polish', 'pt':'Portuguese', 'pt_br':'Portuguese (Brazilian)', 'ro':'Romanian', 'ru':'Russian', 'sk':'Slovak', 'tr':'Turkish', 'uk':'Ukrainian', 'vi':'Vietnamese', 'zh_CN':'Chinese (Simplified)', 'zh_TW':'Chinese (Traditional)', 'test':'TEST' }
+ * :{ 'en':'English', 'an':'Aragonese', 'bg':'Bulgarian', 'ca':'Catalan', 'cz':'Czech', 'da':'Danish', 'de':'German', 'el':'Greek', 'el_gr':'Greek (Greece)', 'es':'Spanish', 'eu':'Basque-Euskera', 'fi':'Finnish', 'fr':'French', 'gl':'Galician', 'hr':'Croatian', 'hu':'Hungarian', 'it':'Italian', 'jp_kana':'Japanese', 'ko_KR':'Korean (South Korea)', 'nl':'Dutch', 'pl':'Polish', 'pt':'Portuguese', 'pt_br':'Portuguese (Brazilian)', 'ro':'Romanian', 'ru':'Russian', 'sk':'Slovak', 'sv':'Swedish', 'tr':'Turkish', 'uk':'Ukrainian', 'vi':'Vietnamese', 'zh_CN':'Chinese (Simplified)', 'zh_TW':'Chinese (Traditional)' }
 */
 #define LCD_LANGUAGE en

@@ -1864,13 +1824,13 @@
 // This option overrides the default number of encoder pulses needed to
 // produce one step. Should be increased for high-resolution encoders.
 //
-//#define ENCODER_PULSES_PER_STEP 4
+//#define ENCODER_PULSES_PER_STEP 4  // SD: I don't think this is needed, Ender 3 V2 used probably same codebase

 //
 // Use this option to override the number of step signals required to
 // move between next/prev menu items.
 //
-//#define ENCODER_STEPS_PER_MENU_ITEM 1
+//#define ENCODER_STEPS_PER_MENU_ITEM 1 // SD: I don't think this is needed, Ender 3 V2 used probably same codebase

 /**
 * Encoder Direction Options
@@ -1918,7 +1878,7 @@
 // If you have a speaker that can produce tones, enable it here.
 // By default Marlin assumes you have a buzzer with a fixed frequency.
 //
-//#define SPEAKER
+#define SPEAKER

 //
 // The duration and frequency for the UI feedback sound.
@@ -1927,8 +1887,8 @@
 // Note: Test audio output with the G-Code:
 //  M300 S<frequency Hz> P<duration ms>
 //
-//#define LCD_FEEDBACK_FREQUENCY_DURATION_MS 2
-//#define LCD_FEEDBACK_FREQUENCY_HZ 5000
+#define LCD_FEEDBACK_FREQUENCY_DURATION_MS 2
+#define LCD_FEEDBACK_FREQUENCY_HZ 5000

 //=============================================================================
 //======================== LCD / Controller Selection =========================
@@ -2192,7 +2152,6 @@
 // different pins/wiring (see pins_ANET_10.h). Enable one of these.
 //
 //#define ANET_FULL_GRAPHICS_LCD
-//#define ANET_FULL_GRAPHICS_LCD_ALT_WIRING

 //
 // AZSMZ 12864 LCD with SD
@@ -2273,6 +2232,12 @@
 //#define DGUS_LCD_UI_FYSETC
 //#define DGUS_LCD_UI_HIPRECY

+//
+// CR-6 OEM touch screen. A DWIN display with touch.
+//
+
+#define DGUS_LCD_UI_CREALITY_TOUCH
+
 //
 // Touch-screen LCD for Malyan M200/M300 printers
 //
@@ -2386,19 +2351,6 @@
 // Generic TFT with detailed options
 //
 //#define TFT_GENERIC
-#if ENABLED(TFT_GENERIC)
-  // :[ 'AUTO', 'ST7735', 'ST7789', 'ST7796', 'R61505', 'ILI9328', 'ILI9341', 'ILI9488' ]
-  #define TFT_DRIVER AUTO
-
-  // Interface. Enable one of the following options:
-  //#define TFT_INTERFACE_FSMC
-  //#define TFT_INTERFACE_SPI
-
-  // TFT Resolution. Enable one of the following options:
-  //#define TFT_RES_320x240
-  //#define TFT_RES_480x272
-  //#define TFT_RES_480x320
-#endif

 /**
 * TFT UI - User Interface Selection. Enable one of the following options:
@@ -2453,6 +2405,10 @@
  //#define TOUCH_OFFSET_Y        257
  //#define TOUCH_ORIENTATION TOUCH_LANDSCAPE

+  #if BOTH(TOUCH_SCREEN_CALIBRATION, EEPROM_SETTINGS)
+    #define TOUCH_CALIBRATION_AUTO_SAVE // Auto save successful calibration values to EEPROM
+  #endif
+
  #if ENABLED(TFT_COLOR_UI)
    //#define SINGLE_TOUCH_NAVIGATION
  #endif
@@ -113,6 +113,12 @@
  #define CHAMBER_BETA                 3950    // Beta value
 #endif

+#if TEMP_SENSOR_PROBE == 1000
+  #define PROBE_PULLUP_RESISTOR_OHMS   4700    // Pullup resistor
+  #define PROBE_RESISTANCE_25C_OHMS    100000  // Resistance at 25C
+  #define PROBE_BETA                   3950    // Beta value
+#endif
+
 //
 // Hephestos 2 24V heated bed upgrade kit.
 // https://store.bq.com/en/heated-bed-kit-hephestos2
@@ -163,7 +169,7 @@
  #if ENABLED(CHAMBER_VENT)
    #define CHAMBER_VENT_SERVO_NR  1  // Index of the vent servo
    #define HIGH_EXCESS_HEAT_LIMIT 5  // How much above target temp to consider there is excess heat in the chamber
-    #define LOW_EXCESS_HEAT_LIMIT 3
+    #define LOW_EXCESS_HEAT_LIMIT  3
    #define MIN_COOLING_SLOPE_TIME_CHAMBER_VENT 20
    #define MIN_COOLING_SLOPE_DEG_CHAMBER_VENT 1.5
  #endif
@@ -186,12 +192,12 @@
 * THERMAL_PROTECTION_HYSTERESIS and/or THERMAL_PROTECTION_PERIOD
 */
 #if ENABLED(THERMAL_PROTECTION_HOTENDS)
-  #define THERMAL_PROTECTION_PERIOD 40        // Seconds
-  #define THERMAL_PROTECTION_HYSTERESIS 4     // Degrees Celsius
+  #define THERMAL_PROTECTION_PERIOD 60        // Seconds
+  #define THERMAL_PROTECTION_HYSTERESIS 12     // Degrees Celsius

-  //#define ADAPTIVE_FAN_SLOWING              // Slow part cooling fan if temperature drops
+  #define ADAPTIVE_FAN_SLOWING              // Slow part cooling fan if temperature drops
  #if BOTH(ADAPTIVE_FAN_SLOWING, PIDTEMP)
-    //#define NO_FAN_SLOWING_IN_PID_TUNING    // Don't slow fan speed during M303
+    #define NO_FAN_SLOWING_IN_PID_TUNING    // Don't slow fan speed during M303
  #endif

  /**
@@ -206,7 +212,7 @@
   * and/or decrease WATCH_TEMP_INCREASE. WATCH_TEMP_INCREASE should not be set
   * below 2.
   */
-  #define WATCH_TEMP_PERIOD 20                // Seconds
+  #define WATCH_TEMP_PERIOD 60                // Seconds
  #define WATCH_TEMP_INCREASE 2               // Degrees Celsius
 #endif

@@ -214,13 +220,13 @@
 * Thermal Protection parameters for the bed are just as above for hotends.
 */
 #if ENABLED(THERMAL_PROTECTION_BED)
-  #define THERMAL_PROTECTION_BED_PERIOD        20 // Seconds
+  #define THERMAL_PROTECTION_BED_PERIOD        180 // Seconds
  #define THERMAL_PROTECTION_BED_HYSTERESIS     2 // Degrees Celsius

  /**
   * As described above, except for the bed (M140/M190/M303).
   */
-  #define WATCH_BED_TEMP_PERIOD                60 // Seconds
+  #define WATCH_BED_TEMP_PERIOD                180 // Seconds
  #define WATCH_BED_TEMP_INCREASE               2 // Degrees Celsius
 #endif

@@ -284,8 +290,8 @@
      // DEFAULT_Kf and PID_FAN_SCALING_LIN_FACTOR are calculated accordingly.

      #define PID_FAN_SCALING_AT_FULL_SPEED 13.0        //=PID_FAN_SCALING_LIN_FACTOR*255+DEFAULT_Kf
-      #define PID_FAN_SCALING_AT_MIN_SPEED 6.0          //=PID_FAN_SCALING_LIN_FACTOR*PID_FAN_SCALING_MIN_SPEED+DEFAULT_Kf
-      #define PID_FAN_SCALING_MIN_SPEED 10.0            // Minimum fan speed at which to enable PID_FAN_SCALING
+      #define PID_FAN_SCALING_AT_MIN_SPEED   6.0        //=PID_FAN_SCALING_LIN_FACTOR*PID_FAN_SCALING_MIN_SPEED+DEFAULT_Kf
+      #define PID_FAN_SCALING_MIN_SPEED     10.0        // Minimum fan speed at which to enable PID_FAN_SCALING

      #define DEFAULT_Kf (255.0*PID_FAN_SCALING_AT_MIN_SPEED-PID_FAN_SCALING_AT_FULL_SPEED*PID_FAN_SCALING_MIN_SPEED)/(255.0-PID_FAN_SCALING_MIN_SPEED)
      #define PID_FAN_SCALING_LIN_FACTOR (PID_FAN_SCALING_AT_FULL_SPEED-DEFAULT_Kf)/255.0
@@ -331,7 +337,7 @@
 * High Temperature Thermistor Support
 *
 * Thermistors able to support high temperature tend to have a hard time getting
- * good readings at room and lower temperatures. This means HEATER_X_RAW_LO_TEMP
+ * good readings at room and lower temperatures. This means TEMP_SENSOR_X_RAW_LO_TEMP
 * will probably be caught when the heating element first turns on during the
 * preheating process, which will trigger a min_temp_error as a safety measure
 * and force stop everything.
@@ -371,9 +377,9 @@
 * Hotend Idle Timeout
 * Prevent filament in the nozzle from charring and causing a critical jam.
 */
-//#define HOTEND_IDLE_TIMEOUT
+#define HOTEND_IDLE_TIMEOUT
 #if ENABLED(HOTEND_IDLE_TIMEOUT)
-  #define HOTEND_IDLE_TIMEOUT_SEC (5*60)    // (seconds) Time without extruder movement to trigger protection
+  #define HOTEND_IDLE_TIMEOUT_SEC (15*60)    // (seconds) Time without extruder movement to trigger protection
  #define HOTEND_IDLE_MIN_TRIGGER   180     // (°C) Minimum temperature to enable hotend protection
  #define HOTEND_IDLE_NOZZLE_TARGET   0     // (°C) Safe temperature for the nozzle after timeout
  #define HOTEND_IDLE_BED_TARGET      0     // (°C) Safe temperature for the bed after timeout
@@ -430,7 +436,7 @@
 *
 * Define one or both of these to override the default 0-255 range.
 */
-//#define FAN_MIN_PWM 50
+#define FAN_MIN_PWM 50
 //#define FAN_MAX_PWM 128

 /**
@@ -503,7 +509,7 @@
 /**
 * M355 Case Light on-off / brightness
 */
-//#define CASE_LIGHT_ENABLE
+#define CASE_LIGHT_ENABLE
 #if ENABLED(CASE_LIGHT_ENABLE)
  //#define CASE_LIGHT_PIN 4                  // Override the default pin if needed
  #define INVERT_CASE_LIGHT false             // Set true if Case Light is ON when pin is LOW
@@ -666,7 +672,7 @@

 //#define HOMING_BACKOFF_POST_MM { 2, 2, 2 }  // (mm) Backoff from endstops after homing

-//#define QUICK_HOME                          // If G28 contains XY do a diagonal move first
+#define QUICK_HOME                          // If G28 contains XY do a diagonal move first
 //#define HOME_Y_BEFORE_X                     // If G28 contains XY home Y before X
 //#define HOME_Z_FIRST                        // Home Z first. Requires a Z-MIN endstop (not a probe).
 //#define CODEPENDENT_XY_HOMING               // If X/Y can't home without homing Y/X first
@@ -870,7 +876,7 @@
 // Increase the slowdown divisor for larger buffer sizes.
 #define SLOWDOWN
 #if ENABLED(SLOWDOWN)
-  #define SLOWDOWN_DIVISOR 2
+  #define SLOWDOWN_DIVISOR 8
 #endif

 /**
@@ -893,19 +899,19 @@
 // Backlash Compensation
 // Adds extra movement to axes on direction-changes to account for backlash.
 //
-//#define BACKLASH_COMPENSATION
+#define BACKLASH_COMPENSATION
 #if ENABLED(BACKLASH_COMPENSATION)
  // Define values for backlash distance and correction.
  // If BACKLASH_GCODE is enabled these values are the defaults.
  #define BACKLASH_DISTANCE_MM { 0, 0, 0 } // (mm)
-  #define BACKLASH_CORRECTION    0.0       // 0.0 = no correction; 1.0 = full correction
+  #define BACKLASH_CORRECTION    0.1       // 0.0 = no correction; 1.0 = full correction

  // Set BACKLASH_SMOOTHING_MM to spread backlash correction over multiple segments
  // to reduce print artifacts. (Enabling this is costly in memory and computation!)
  //#define BACKLASH_SMOOTHING_MM 3 // (mm)

  // Add runtime configuration and tuning of backlash values (M425)
-  //#define BACKLASH_GCODE
+  #define BACKLASH_GCODE

  #if ENABLED(BACKLASH_GCODE)
    // Measure the Z backlash when probing (G29) and set with "M425 Z"
@@ -1157,7 +1163,7 @@
  //#define LCD_DECIMAL_SMALL_XY

  // Add an 'M73' G-code to set the current percentage
-  //#define LCD_SET_PROGRESS_MANUALLY
+  #define LCD_SET_PROGRESS_MANUALLY

  // Show the E position (filament used) during printing
  //#define LCD_SHOW_E_TOTAL
@@ -1221,9 +1227,7 @@
  //#define NO_SD_AUTOSTART                 // Remove auto#.g file support completely to save some Flash, SRAM
  //#define MENU_ADDAUTOSTART               // Add a menu option to run auto#.g files

-  //#define BROWSE_MEDIA_ON_INSERT          // Open the file browser when media is inserted
-
-  #define EVENT_GCODE_SD_ABORT "G28XY"      // G-code to run on SD Abort Print (e.g., "G28XY" or "G27")
+  #define EVENT_GCODE_SD_ABORT "G27\nM84"      // G-code to run on SD Abort Print (e.g., "G28XY" or "G27")

  #if ENABLED(PRINTER_EVENT_LEDS)
    #define PE_LEDS_COMPLETED_TIME  (30*60) // (seconds) Time to keep the LED "done" color before restoring normal illumination
@@ -1237,7 +1241,7 @@
   * an option on the LCD screen to continue the print from the last-known
   * point in the file.
   */
-  //#define POWER_LOSS_RECOVERY
+  #define POWER_LOSS_RECOVERY
  #if ENABLED(POWER_LOSS_RECOVERY)
    #define PLR_ENABLED_DEFAULT   false // Power Loss Recovery enabled by default. (Set with 'M413 Sn' & M500)
    //#define BACKUP_POWER_SUPPLY       // Backup power / UPS to move the steppers on power loss
@@ -1245,8 +1249,7 @@
    //#define POWER_LOSS_ZRAISE       2 // (mm) Z axis raise on resume (on power loss with UPS)
    //#define POWER_LOSS_PIN         44 // Pin to detect power loss. Set to -1 to disable default pin on boards without module.
    //#define POWER_LOSS_STATE     HIGH // State of pin indicating power loss
-    //#define POWER_LOSS_PULLUP         // Set pullup / pulldown as appropriate for your sensor
-    //#define POWER_LOSS_PULLDOWN
+    //#define POWER_LOSS_PULL           // Set pullup / pulldown as appropriate
    //#define POWER_LOSS_PURGE_LEN   20 // (mm) Length of filament to purge on resume
    //#define POWER_LOSS_RETRACT_LEN 10 // (mm) Length of filament to retract on fail. Requires backup power.

@@ -1278,7 +1281,7 @@
   *  - SDSORT_CACHE_NAMES will retain the sorted file listing in RAM. (Expensive!)
   *  - SDSORT_DYNAMIC_RAM only uses RAM when the SD menu is visible. (Use with caution!)
   */
-  //#define SDCARD_SORT_ALPHA
+  #define SDCARD_SORT_ALPHA

  // SD Card Sorting options
  #if ENABLED(SDCARD_SORT_ALPHA)
@@ -1298,7 +1301,7 @@
  //#define UTF_FILENAME_SUPPORT

  // This allows hosts to request long names for files and folders with M33
-  //#define LONG_FILENAME_HOST_SUPPORT
+  #define LONG_FILENAME_HOST_SUPPORT

  // Enable this option to scroll long filenames in the SD card menu
  //#define SCROLL_LONG_FILENAMES
@@ -1323,7 +1326,7 @@
  /**
   * Auto-report SdCard status with M27 S<seconds>
   */
-  //#define AUTO_REPORT_SD_STATUS
+  #define AUTO_REPORT_SD_STATUS

  /**
   * Support for USB thumb drives using an Arduino USB Host Shield or
@@ -1492,7 +1495,7 @@
 //
 // Additional options for DGUS / DWIN displays
 //
-#if HAS_DGUS_LCD
+#if HAS_DGUS_LCD || ENABLED(DGUS_LCD_UI_CREALITY_TOUCH)
  #define LCD_SERIAL_PORT 3
  #define LCD_BAUDRATE 115200

@@ -1526,6 +1529,19 @@
  #endif
 #endif // HAS_DGUS_LCD

+//
+// Specify additional languages for the UI. Default specified by LCD_LANGUAGE.
+//
+#if ANY(DOGLCD, TFT_COLOR_UI, TOUCH_UI_FTDI_EVE)
+  //#define LCD_LANGUAGE_2 fr
+  //#define LCD_LANGUAGE_3 de
+  //#define LCD_LANGUAGE_4 es
+  //#define LCD_LANGUAGE_5 it
+  #ifdef LCD_LANGUAGE_2
+    //#define LCD_LANGUAGE_AUTO_SAVE // Automatically save language to EEPROM on change
+  #endif
+#endif
+
 //
 // Touch UI for the FTDI Embedded Video Engine (EVE)
 //
@@ -1601,13 +1617,6 @@
  // Use a smaller font when labels don't fit buttons
  #define TOUCH_UI_FIT_TEXT

-  // Allow language selection from menu at run-time (otherwise use LCD_LANGUAGE)
-  //#define LCD_LANGUAGE_1 en
-  //#define LCD_LANGUAGE_2 fr
-  //#define LCD_LANGUAGE_3 de
-  //#define LCD_LANGUAGE_4 es
-  //#define LCD_LANGUAGE_5 it
-
  // Use a numeric passcode for "Screen lock" keypad.
  // (recommended for smaller displays)
  //#define TOUCH_UI_PASSCODE
@@ -1649,7 +1658,7 @@
 * NOTE: This method is less reliable as it can only catch hangups while
 * interrupts are enabled.
 */
-#define USE_WATCHDOG
+//#define USE_WATCHDOG
 #if ENABLED(USE_WATCHDOG)
  //#define WATCHDOG_RESET_MANUAL
 #endif
@@ -1663,15 +1672,15 @@
 *
 * Warning: Does not respect endstops!
 */
-//#define BABYSTEPPING
+#define BABYSTEPPING
 #if ENABLED(BABYSTEPPING)
-  //#define INTEGRATED_BABYSTEPPING         // EXPERIMENTAL integration of babystepping into the Stepper ISR
+  #define INTEGRATED_BABYSTEPPING         // EXPERIMENTAL integration of babystepping into the Stepper ISR
  //#define BABYSTEP_WITHOUT_HOMING
-  //#define BABYSTEP_ALWAYS_AVAILABLE       // Allow babystepping at all times (not just during movement).
+  #define BABYSTEP_ALWAYS_AVAILABLE       // Allow babystepping at all times (not just during movement).
  //#define BABYSTEP_XY                     // Also enable X/Y Babystepping. Not supported on DELTA!
  #define BABYSTEP_INVERT_Z false           // Change if Z babysteps should go the other way
  //#define BABYSTEP_MILLIMETER_UNITS       // Specify BABYSTEP_MULTIPLICATOR_(XY|Z) in mm instead of micro-steps
-  #define BABYSTEP_MULTIPLICATOR_Z  1       // (steps or mm) Steps or millimeter distance for each Z babystep
+  #define BABYSTEP_MULTIPLICATOR_Z  40      // (steps or mm) Steps or millimeter distance for each Z babystep
  #define BABYSTEP_MULTIPLICATOR_XY 1       // (steps or mm) Steps or millimeter distance for each XY babystep

  //#define DOUBLECLICK_FOR_Z_BABYSTEPPING  // Double-click on the Status Screen for Z Babystepping.
@@ -1686,7 +1695,7 @@

  //#define BABYSTEP_DISPLAY_TOTAL          // Display total babysteps since last G28

-  //#define BABYSTEP_ZPROBE_OFFSET          // Combine M851 Z and Babystepping
+  #define BABYSTEP_ZPROBE_OFFSET            // Combine M851 Z and Babystepping
  #if ENABLED(BABYSTEP_ZPROBE_OFFSET)
    //#define BABYSTEP_HOTEND_Z_OFFSET      // For multiple hotends, babystep relative Z offsets
    //#define BABYSTEP_ZPROBE_GFX_OVERLAY   // Enable graphical overlay on Z-offset editor
@@ -1713,9 +1722,9 @@
 //#define LIN_ADVANCE
 #if ENABLED(LIN_ADVANCE)
  //#define EXTRA_LIN_ADVANCE_K // Enable for second linear advance constants
-  #define LIN_ADVANCE_K 0.22    // Unit: mm compression per 1mm/s extruder speed
+  #define LIN_ADVANCE_K 0    // Unit: mm compression per 1mm/s extruder speed
  //#define LA_DEBUG            // If enabled, this will generate debug information output over USB.
-  //#define EXPERIMENTAL_SCURVE // Enable this option to permit S-Curve Acceleration
+  #define EXPERIMENTAL_SCURVE // Enable this option to permit S-Curve Acceleration
 #endif

 // @section leveling
@@ -1955,7 +1964,7 @@
 #if BOTH(SDSUPPORT, DIRECT_STEPPING)
  #define BLOCK_BUFFER_SIZE  8
 #elif ENABLED(SDSUPPORT)
-  #define BLOCK_BUFFER_SIZE 16
+  #define BLOCK_BUFFER_SIZE 64
 #else
  #define BLOCK_BUFFER_SIZE 16
 #endif
@@ -1964,7 +1973,7 @@

 // The ASCII buffer for serial input
 #define MAX_CMD_SIZE 96
-#define BUFSIZE 4
+#define BUFSIZE 32

 // Transmission to Host Buffer Size
 // To save 386 bytes of PROGMEM (and TX_BUFFER_SIZE+3 bytes of RAM) set to 0.
@@ -1973,7 +1982,7 @@
 // For debug-echo: 128 bytes for the optimal speed.
 // Other output doesn't need to be that speedy.
 // :[0, 2, 4, 8, 16, 32, 64, 128, 256]
-#define TX_BUFFER_SIZE 0
+#define TX_BUFFER_SIZE 128

 // Host Receive Buffer Size
 // Without XON/XOFF flow control (see SERIAL_XON_XOFF below) 32 bytes should be enough.
@@ -2008,16 +2017,16 @@
 * Currently handles M108, M112, M410, M876
 * NOTE: Not yet implemented for all platforms.
 */
-//#define EMERGENCY_PARSER
+#define EMERGENCY_PARSER

 // Bad Serial-connections can miss a received command by sending an 'ok'
 // Therefore some clients abort after 30 seconds in a timeout.
 // Some other clients start sending commands while receiving a 'wait'.
 // This "wait" is only sent when the buffer is empty. 1 second is a good value here.
-//#define NO_TIMEOUTS 1000 // Milliseconds
+#define NO_TIMEOUTS 1000 // Milliseconds

 // Some clients will have this feature soon. This could make the NO_TIMEOUTS unnecessary.
-//#define ADVANCED_OK
+#define ADVANCED_OK

 // Printrun may have trouble receiving long strings all at once.
 // This option inserts short delays between lines of serial output.
@@ -2051,23 +2060,23 @@
 *
 * Note that M207 / M208 / M209 settings are saved to EEPROM.
 */
-//#define FWRETRACT
+#define FWRETRACT
 #if ENABLED(FWRETRACT)
-  #define FWRETRACT_AUTORETRACT           // Override slicer retractions
+  #define FWRETRACT_AUTORETRACT             // Override slicer retractions
  #if ENABLED(FWRETRACT_AUTORETRACT)
-    #define MIN_AUTORETRACT 0.1           // (mm) Don't convert E moves under this length
-    #define MAX_AUTORETRACT 10.0          // (mm) Don't convert E moves over this length
+    #define MIN_AUTORETRACT             0.1 // (mm) Don't convert E moves under this length
+    #define MAX_AUTORETRACT            10.0 // (mm) Don't convert E moves over this length
  #endif
-  #define RETRACT_LENGTH 3                // (mm) Default retract length (positive value)
-  #define RETRACT_LENGTH_SWAP 13          // (mm) Default swap retract length (positive value)
-  #define RETRACT_FEEDRATE 45             // (mm/s) Default feedrate for retracting
-  #define RETRACT_ZRAISE 0                // (mm) Default retract Z-raise
-  #define RETRACT_RECOVER_LENGTH 0        // (mm) Default additional recover length (added to retract length on recover)
-  #define RETRACT_RECOVER_LENGTH_SWAP 0   // (mm) Default additional swap recover length (added to retract length on recover from toolchange)
-  #define RETRACT_RECOVER_FEEDRATE 8      // (mm/s) Default feedrate for recovering from retraction
-  #define RETRACT_RECOVER_FEEDRATE_SWAP 8 // (mm/s) Default feedrate for recovering from swap retraction
+  #define RETRACT_LENGTH                3   // (mm) Default retract length (positive value)
+  #define RETRACT_LENGTH_SWAP          13   // (mm) Default swap retract length (positive value)
+  #define RETRACT_FEEDRATE             45   // (mm/s) Default feedrate for retracting
+  #define RETRACT_ZRAISE                0   // (mm) Default retract Z-raise
+  #define RETRACT_RECOVER_LENGTH        0   // (mm) Default additional recover length (added to retract length on recover)
+  #define RETRACT_RECOVER_LENGTH_SWAP   0   // (mm) Default additional swap recover length (added to retract length on recover from toolchange)
+  #define RETRACT_RECOVER_FEEDRATE      8   // (mm/s) Default feedrate for recovering from retraction
+  #define RETRACT_RECOVER_FEEDRATE_SWAP 8   // (mm/s) Default feedrate for recovering from swap retraction
  #if ENABLED(MIXING_EXTRUDER)
-    //#define RETRACT_SYNC_MIXING         // Retract and restore all mixing steppers simultaneously
+    //#define RETRACT_SYNC_MIXING           // Retract and restore all mixing steppers simultaneously
  #endif
 #endif

@@ -2151,7 +2160,7 @@
 * Requires NOZZLE_PARK_FEATURE.
 * This feature is required for the default FILAMENT_RUNOUT_SCRIPT.
 */
-//#define ADVANCED_PAUSE_FEATURE
+#define ADVANCED_PAUSE_FEATURE
 #if ENABLED(ADVANCED_PAUSE_FEATURE)
  #define PAUSE_PARK_RETRACT_FEEDRATE         60  // (mm/s) Initial retract feedrate.
  #define PAUSE_PARK_RETRACT_LENGTH            2  // (mm) Initial retract.
@@ -2172,7 +2181,7 @@
                                                  //   For direct drive, the full length of the nozzle.
  //#define ADVANCED_PAUSE_CONTINUOUS_PURGE       // Purge continuously up to the purge length until interrupted.
  #define ADVANCED_PAUSE_PURGE_FEEDRATE        3  // (mm/s) Extrude feedrate (after loading). Should be slower than load feedrate.
-  #define ADVANCED_PAUSE_PURGE_LENGTH         50  // (mm) Length to extrude after loading.
+  #define ADVANCED_PAUSE_PURGE_LENGTH         2  // (mm) Length to extrude after loading.
                                                  //   Set to 0 for manual extrusion.
                                                  //   Filament can be extruded repeatedly from the Filament Change menu
                                                  //   until extrusion is consistent, and to purge old filament.
@@ -2180,19 +2189,19 @@
  //#define ADVANCED_PAUSE_FANS_PAUSE             // Turn off print-cooling fans while the machine is paused.

                                                  // Filament Unload does a Retract, Delay, and Purge first:
-  #define FILAMENT_UNLOAD_PURGE_RETRACT       13  // (mm) Unload initial retract length.
+  #define FILAMENT_UNLOAD_PURGE_RETRACT       3  // (mm) Unload initial retract length.
  #define FILAMENT_UNLOAD_PURGE_DELAY       5000  // (ms) Delay for the filament to cool after retract.
  #define FILAMENT_UNLOAD_PURGE_LENGTH         8  // (mm) An unretract is done, then this length is purged.
  #define FILAMENT_UNLOAD_PURGE_FEEDRATE      25  // (mm/s) feedrate to purge before unload

-  #define PAUSE_PARK_NOZZLE_TIMEOUT           45  // (seconds) Time limit before the nozzle is turned off for safety.
-  #define FILAMENT_CHANGE_ALERT_BEEPS         10  // Number of alert beeps to play when a response is needed.
+  #define PAUSE_PARK_NOZZLE_TIMEOUT           90  // (seconds) Time limit before the nozzle is turned off for safety.
+  #define FILAMENT_CHANGE_ALERT_BEEPS         2  // Number of alert beeps to play when a response is needed.
  #define PAUSE_PARK_NO_STEPPER_TIMEOUT           // Enable for XYZ steppers to stay powered on during filament change.

-  //#define PARK_HEAD_ON_PAUSE                    // Park the nozzle during pause and filament change.
+  #define PARK_HEAD_ON_PAUSE                    // Park the nozzle during pause and filament change.
  //#define HOME_BEFORE_FILAMENT_CHANGE           // If needed, home before parking for filament change

-  //#define FILAMENT_LOAD_UNLOAD_GCODES           // Add M701/M702 Load/Unload G-codes, plus Load/Unload in the LCD Prepare menu.
+  #define FILAMENT_LOAD_UNLOAD_GCODES           // Add M701/M702 Load/Unload G-codes, plus Load/Unload in the LCD Prepare menu.
  //#define FILAMENT_UNLOAD_ALL_EXTRUDERS         // Allow M702 to unload all extruders above a minimum target temp (as set by M302)
 #endif

@@ -3216,7 +3225,7 @@
 /**
 * Auto-report temperatures with M155 S<seconds>
 */
-#define AUTO_REPORT_TEMPERATURES
+//#define AUTO_REPORT_TEMPERATURES

 /**
 * Include capabilities in M115 output
@@ -3295,6 +3304,8 @@
  //#define GCODE_QUOTED_STRINGS  // Support for quoted string parameters
 #endif

+//#define MEATPACK                // Support for MeatPack G-code compression (https://github.com/scottmudge/OctoPrint-MeatPack)
+
 //#define GCODE_CASE_INSENSITIVE  // Accept G-code sent to the firmware in lowercase

 //#define REPETIER_GCODE_M360     // Add commands originally from Repetier FW
@@ -3334,7 +3345,8 @@
 #endif

 /**
- * User-defined menu items that execute custom GCode
+ * User-defined menu items to run custom G-code.
+ * Up to 25 may be defined, but the actual number is LCD-dependent.
 */
 //#define CUSTOM_USER_MENUS
 #if ENABLED(CUSTOM_USER_MENUS)
@@ -3344,7 +3356,7 @@
  //#define USER_SCRIPT_RETURN  // Return to status screen after a script

  #define USER_DESC_1 "Home & UBL Info"
-  #define USER_GCODE_1 "G29NW"
+  #define USER_GCODE_1 "G28\nG29W"

  #define USER_DESC_2 "Preheat for " PREHEAT_1_LABEL
  #define USER_GCODE_2 "M140 S" STRINGIFY(PREHEAT_1_TEMP_BED) "\nM104 S" STRINGIFY(PREHEAT_1_TEMP_HOTEND)
@@ -3353,7 +3365,7 @@
  #define USER_GCODE_3 "M140 S" STRINGIFY(PREHEAT_2_TEMP_BED) "\nM104 S" STRINGIFY(PREHEAT_2_TEMP_HOTEND)

  #define USER_DESC_4 "Heat Bed/Home/Level"
-  #define USER_GCODE_4 "M140 S" STRINGIFY(PREHEAT_2_TEMP_BED) "\nG29N"
+  #define USER_GCODE_4 "M140 S" STRINGIFY(PREHEAT_2_TEMP_BED) "\nG28\nG29"

  #define USER_DESC_5 "Home & Info"
  #define USER_GCODE_5 "G28\nM503"
@@ -3373,9 +3385,9 @@
 * Host Prompt Support enables Marlin to use the host for user prompts so
 * filament runout and other processes can be managed from the host side.
 */
-//#define HOST_ACTION_COMMANDS
+#define HOST_ACTION_COMMANDS
 #if ENABLED(HOST_ACTION_COMMANDS)
-  //#define HOST_PROMPT_SUPPORT
+  #define HOST_PROMPT_SUPPORT
  //#define HOST_START_MENU_ITEM  // Add a menu item that tells the host to start
 #endif

@@ -3384,7 +3396,7 @@
 *
 * Implement M486 to allow Marlin to skip objects
 */
-//#define CANCEL_OBJECTS
+#define CANCEL_OBJECTS

 /**
 * I2C position encoders for closed loop control.
@@ -3708,12 +3720,12 @@
 //
 // M42 - Set pin states
 //
-//#define DIRECT_PIN_CONTROL
+#define DIRECT_PIN_CONTROL

 //
 // M43 - display pin status, toggle pins, watch pins, watch endstops & toggle LED, test servo probe
 //
-//#define PINS_DEBUGGING
+#define PINS_DEBUGGING

 // Enable Marlin dev mode which adds some special commands
 //#define MARLIN_DEV_MODE
@@ -28,25 +28,25 @@
 /**
 * Marlin release version identifier
 */
-//#define SHORT_BUILD_VERSION "bugfix-2.0.x"
+#define SHORT_BUILD_VERSION "TM3D2.0.7.2-CR6-C"

 /**
 * Verbose version identifier which should contain a reference to the location
 * from where the binary was downloaded or the source code was compiled.
 */
-//#define DETAILED_BUILD_VERSION SHORT_BUILD_VERSION
+#define DETAILED_BUILD_VERSION SHORT_BUILD_VERSION

 /**
 * The STRING_DISTRIBUTION_DATE represents when the binary file was built,
 * here we define this default string as the date where the latest release
 * version was tagged.
 */
-//#define STRING_DISTRIBUTION_DATE "2019-07-10"
+#define STRING_DISTRIBUTION_DATE "2020-12-28"

 /**
 * Defines a generic printer name to be output to the LCD after booting Marlin.
 */
-//#define MACHINE_NAME "3D Printer"
+#define MACHINE_NAME "TM3D CR-6"

 /**
 * The SOURCE_CODE_URL is the location where users will find the Marlin Source
@@ -54,7 +54,7 @@
 * has a distinct Github fork— the Source Code URL should just be the main
 * Marlin repository.
 */
-//#define SOURCE_CODE_URL "github.com/MarlinFirmware/Marlin"
+#define SOURCE_CODE_URL "https://github.com/InsanityAutomation/Marlin/tree/CR-6Devel"

 /**
 * Default generic printer UUID.
@@ -65,7 +65,7 @@
 * The WEBSITE_URL is the location where users can get more information such as
 * documentation about a specific Marlin release.
 */
-//#define WEBSITE_URL "marlinfw.org"
+#define WEBSITE_URL "tinymachines3d.com"

 /**
 * Set the vendor info the serial USB interface, if changable
@@ -24,6 +24,13 @@
 #include "../../inc/MarlinConfig.h"
 #include "HAL.h"

+#ifdef USBCON
+  DefaultSerial MSerial(false, Serial);
+  #ifdef BLUETOOTH
+    BTSerial btSerial(false, bluetoothSerial);
+  #endif
+#endif
+
 // ------------------------
 // Public Variables
 // ------------------------
@@ -82,7 +82,15 @@ typedef int8_t pin_t;

 // Serial ports
 #ifdef USBCON
-  #define MYSERIAL0 TERN(BLUETOOTH, bluetoothSerial, Serial)
+  #include "../../core/serial_hook.h"
+  typedef ForwardSerial0Type< decltype(Serial) > DefaultSerial;
+  extern DefaultSerial MSerial;
+  #ifdef BLUETOOTH
+    typedef ForwardSerial0Type< decltype(bluetoothSerial) > BTSerial;
+    extern BTSerial btSerial;
+  #endif
+
+  #define MYSERIAL0 TERN(BLUETOOTH, btSerial, MSerial)
 #else
  #if !WITHIN(SERIAL_PORT, -1, 3)
    #error "SERIAL_PORT must be from -1 to 3. Please update your configuration."
@@ -109,7 +117,7 @@ typedef int8_t pin_t;
    #error "LCD_SERIAL_PORT must be from -1 to 3. Please update your configuration."
  #endif
  #define LCD_SERIAL lcdSerial
-  #if HAS_DGUS_LCD
+  #if HAS_DGUS_LCD || ENABLED(DGUS_LCD_UI_CREALITY_TOUCH)
    #define SERIAL_GET_TX_BUFFER_FREE() LCD_SERIAL.get_tx_buffer_free()
  #endif
 #endif
@@ -454,7 +454,7 @@ void MarlinSerial<Cfg>::flush() {
 }

 template<typename Cfg>
-void MarlinSerial<Cfg>::write(const uint8_t c) {
+size_t MarlinSerial<Cfg>::write(const uint8_t c) {
  if (Cfg::TX_SIZE == 0) {

    _written = true;
@@ -480,7 +480,7 @@ void MarlinSerial<Cfg>::write(const uint8_t c) {
      // location". This makes sure flush() won't return until the bytes
      // actually got written
      B_TXC = 1;
-      return;
+      return 1;
    }

    const uint8_t i = (tx_buffer.head + 1) & (Cfg::TX_SIZE - 1);
@@ -510,6 +510,7 @@ void MarlinSerial<Cfg>::write(const uint8_t c) {
    // Enable TX ISR - Non atomic, but it will eventually enable TX ISR
    B_UDRIE = 1;
  }
+  return 1;
 }

 template<typename Cfg>
@@ -556,161 +557,6 @@ void MarlinSerial<Cfg>::flushTX() {
  }
 }

-/**
- * Imports from print.h
- */
-
-template<typename Cfg>
-void MarlinSerial<Cfg>::print(char c, int base) {
-  print((long)c, base);
-}
-
-template<typename Cfg>
-void MarlinSerial<Cfg>::print(unsigned char b, int base) {
-  print((unsigned long)b, base);
-}
-
-template<typename Cfg>
-void MarlinSerial<Cfg>::print(int n, int base) {
-  print((long)n, base);
-}
-
-template<typename Cfg>
-void MarlinSerial<Cfg>::print(unsigned int n, int base) {
-  print((unsigned long)n, base);
-}
-
-template<typename Cfg>
-void MarlinSerial<Cfg>::print(long n, int base) {
-  if (base == 0) write(n);
-  else if (base == 10) {
-    if (n < 0) { print('-'); n = -n; }
-    printNumber(n, 10);
-  }
-  else
-    printNumber(n, base);
-}
-
-template<typename Cfg>
-void MarlinSerial<Cfg>::print(unsigned long n, int base) {
-  if (base == 0) write(n);
-  else printNumber(n, base);
-}
-
-template<typename Cfg>
-void MarlinSerial<Cfg>::print(double n, int digits) {
-  printFloat(n, digits);
-}
-
-template<typename Cfg>
-void MarlinSerial<Cfg>::println() {
-  print('\r');
-  print('\n');
-}
-
-template<typename Cfg>
-void MarlinSerial<Cfg>::println(const String& s) {
-  print(s);
-  println();
-}
-
-template<typename Cfg>
-void MarlinSerial<Cfg>::println(const char c[]) {
-  print(c);
-  println();
-}
-
-template<typename Cfg>
-void MarlinSerial<Cfg>::println(char c, int base) {
-  print(c, base);
-  println();
-}
-
-template<typename Cfg>
-void MarlinSerial<Cfg>::println(unsigned char b, int base) {
-  print(b, base);
-  println();
-}
-
-template<typename Cfg>
-void MarlinSerial<Cfg>::println(int n, int base) {
-  print(n, base);
-  println();
-}
-
-template<typename Cfg>
-void MarlinSerial<Cfg>::println(unsigned int n, int base) {
-  print(n, base);
-  println();
-}
-
-template<typename Cfg>
-void MarlinSerial<Cfg>::println(long n, int base) {
-  print(n, base);
-  println();
-}
-
-template<typename Cfg>
-void MarlinSerial<Cfg>::println(unsigned long n, int base) {
-  print(n, base);
-  println();
-}
-
-template<typename Cfg>
-void MarlinSerial<Cfg>::println(double n, int digits) {
-  print(n, digits);
-  println();
-}
-
-// Private Methods
-
-template<typename Cfg>
-void MarlinSerial<Cfg>::printNumber(unsigned long n, uint8_t base) {
-  if (n) {
-    unsigned char buf[8 * sizeof(long)]; // Enough space for base 2
-    int8_t i = 0;
-    while (n) {
-      buf[i++] = n % base;
-      n /= base;
-    }
-    while (i--)
-      print((char)(buf[i] + (buf[i] < 10 ? '0' : 'A' - 10)));
-  }
-  else
-    print('0');
-}
-
-template<typename Cfg>
-void MarlinSerial<Cfg>::printFloat(double number, uint8_t digits) {
-  // Handle negative numbers
-  if (number < 0.0) {
-    print('-');
-    number = -number;
-  }
-
-  // Round correctly so that print(1.999, 2) prints as "2.00"
-  double rounding = 0.5;
-  LOOP_L_N(i, digits) rounding *= 0.1;
-  number += rounding;
-
-  // Extract the integer part of the number and print it
-  unsigned long int_part = (unsigned long)number;
-  double remainder = number - (double)int_part;
-  print(int_part);
-
-  // Print the decimal point, but only if there are digits beyond
-  if (digits) {
-    print('.');
-    // Extract digits from the remainder one at a time
-    while (digits--) {
-      remainder *= 10.0;
-      int toPrint = int(remainder);
-      print(toPrint);
-      remainder -= toPrint;
-    }
-  }
-}
-
 // Hookup ISR handlers
 ISR(SERIAL_REGNAME(USART, SERIAL_PORT, _RX_vect)) {
  MarlinSerial<MarlinSerialCfg<SERIAL_PORT>>::store_rxd_char();
@@ -720,11 +566,9 @@ ISR(SERIAL_REGNAME(USART, SERIAL_PORT, _UDRE_vect)) {
  MarlinSerial<MarlinSerialCfg<SERIAL_PORT>>::_tx_udr_empty_irq();
 }

-// Preinstantiate
-template class MarlinSerial<MarlinSerialCfg<SERIAL_PORT>>;
-
-// Instantiate
-MarlinSerial<MarlinSerialCfg<SERIAL_PORT>> customizedSerial1;
+// Because of the template definition above, it's required to instantiate the template to have all method generated
+template class MarlinSerial< MarlinSerialCfg<SERIAL_PORT> >;
+MSerialT customizedSerial1(MSerialT::HasEmergencyParser);

 #ifdef SERIAL_PORT_2

@@ -737,12 +581,8 @@ MarlinSerial<MarlinSerialCfg<SERIAL_PORT>> customizedSerial1;
    MarlinSerial<MarlinSerialCfg<SERIAL_PORT_2>>::_tx_udr_empty_irq();
  }

-  // Preinstantiate
-  template class MarlinSerial<MarlinSerialCfg<SERIAL_PORT_2>>;
-
-  // Instantiate
-  MarlinSerial<MarlinSerialCfg<SERIAL_PORT_2>> customizedSerial2;
-
+  template class MarlinSerial< MarlinSerialCfg<SERIAL_PORT_2> >;
+  MSerialT2 customizedSerial2(MSerialT2::HasEmergencyParser);
 #endif

 #ifdef MMU2_SERIAL_PORT
@@ -755,12 +595,8 @@ MarlinSerial<MarlinSerialCfg<SERIAL_PORT>> customizedSerial1;
    MarlinSerial<MMU2SerialCfg<MMU2_SERIAL_PORT>>::_tx_udr_empty_irq();
  }

-  // Preinstantiate
-  template class MarlinSerial<MMU2SerialCfg<MMU2_SERIAL_PORT>>;
-
-  // Instantiate
-  MarlinSerial<MMU2SerialCfg<MMU2_SERIAL_PORT>> mmuSerial;
-
+  template class MarlinSerial< MMU2SerialCfg<MMU2_SERIAL_PORT> >;
+  MSerialT3 mmuSerial(MSerialT3::HasEmergencyParser);
 #endif

 #ifdef LCD_SERIAL_PORT
@@ -773,13 +609,10 @@ MarlinSerial<MarlinSerialCfg<SERIAL_PORT>> customizedSerial1;
    MarlinSerial<LCDSerialCfg<LCD_SERIAL_PORT>>::_tx_udr_empty_irq();
  }

-  // Preinstantiate
-  template class MarlinSerial<LCDSerialCfg<LCD_SERIAL_PORT>>;
+  template class MarlinSerial< LCDSerialCfg<LCD_SERIAL_PORT> >;
+  MSerialT4 lcdSerial(MSerialT4::HasEmergencyParser);

-  // Instantiate
-  MarlinSerial<LCDSerialCfg<LCD_SERIAL_PORT>> lcdSerial;
-
-  #if HAS_DGUS_LCD
+  #if HAS_DGUS_LCD || ENABLED(DGUS_LCD_UI_CREALITY_TOUCH)
    template<typename Cfg>
    typename MarlinSerial<Cfg>::ring_buffer_pos_t MarlinSerial<Cfg>::get_tx_buffer_free() {
      const ring_buffer_pos_t t = tx_buffer.tail,  // next byte to send.
@@ -796,7 +629,7 @@ MarlinSerial<MarlinSerialCfg<SERIAL_PORT>> customizedSerial1;

 // For AT90USB targets use the UART for BT interfacing
 #if defined(USBCON) && ENABLED(BLUETOOTH)
-  HardwareSerial bluetoothSerial;
+  MSerialT5 bluetoothSerial(false);
 #endif

 #endif // __AVR__
@@ -34,6 +34,7 @@
 #include <WString.h>

 #include "../../inc/MarlinConfigPre.h"
+#include "../../core/serial_hook.h"

 #ifndef SERIAL_PORT
  #define SERIAL_PORT 0
@@ -135,10 +136,6 @@
    UART_DECL(3);
  #endif

-  #define DEC 10
-  #define HEX 16
-  #define OCT 8
-  #define BIN 2
  #define BYTE 0

  // Templated type selector
@@ -202,60 +199,30 @@
    static FORCE_INLINE void atomic_set_rx_tail(ring_buffer_pos_t value);
    static FORCE_INLINE ring_buffer_pos_t atomic_read_rx_tail();

-    public:
-
+  public:
    FORCE_INLINE static void store_rxd_char();
    FORCE_INLINE static void _tx_udr_empty_irq();

-    public:
-      MarlinSerial() {};
-      static void begin(const long);
-      static void end();
-      static int peek();
-      static int read();
-      static void flush();
-      static ring_buffer_pos_t available();
-      static void write(const uint8_t c);
-      static void flushTX();
-      #if HAS_DGUS_LCD
-        static ring_buffer_pos_t get_tx_buffer_free();
-      #endif
+  public:
+    static void begin(const long);
+    static void end();
+    static int peek();
+    static int read();
+    static void flush();
+    static ring_buffer_pos_t available();
+    static size_t write(const uint8_t c);
+    static void flushTX();
+    #if HAS_DGUS_LCD || ENABLED(DGUS_LCD_UI_CREALITY_TOUCH)
+      static ring_buffer_pos_t get_tx_buffer_free();
+    #endif

-      static inline bool emergency_parser_enabled() { return Cfg::EMERGENCYPARSER; }
+    enum { HasEmergencyParser = Cfg::EMERGENCYPARSER };
+    static inline bool emergency_parser_enabled() { return Cfg::EMERGENCYPARSER; }

-      FORCE_INLINE static uint8_t dropped() { return Cfg::DROPPED_RX ? rx_dropped_bytes : 0; }
-      FORCE_INLINE static uint8_t buffer_overruns() { return Cfg::RX_OVERRUNS ? rx_buffer_overruns : 0; }
-      FORCE_INLINE static uint8_t framing_errors() { return Cfg::RX_FRAMING_ERRORS ? rx_framing_errors : 0; }
-      FORCE_INLINE static ring_buffer_pos_t rxMaxEnqueued() { return Cfg::MAX_RX_QUEUED ? rx_max_enqueued : 0; }
-
-      FORCE_INLINE static void write(const char* str) { while (*str) write(*str++); }
-      FORCE_INLINE static void write(const uint8_t* buffer, size_t size) { while (size--) write(*buffer++); }
-      FORCE_INLINE static void print(const String& s) { for (int i = 0; i < (int)s.length(); i++) write(s[i]); }
-      FORCE_INLINE static void print(const char* str) { write(str); }
-
-      static void print(char, int = BYTE);
-      static void print(unsigned char, int = BYTE);
-      static void print(int, int = DEC);
-      static void print(unsigned int, int = DEC);
-      static void print(long, int = DEC);
-      static void print(unsigned long, int = DEC);
-      static void print(double, int = 2);
-
-      static void println(const String& s);
-      static void println(const char[]);
-      static void println(char, int = BYTE);
-      static void println(unsigned char, int = BYTE);
-      static void println(int, int = DEC);
-      static void println(unsigned int, int = DEC);
-      static void println(long, int = DEC);
-      static void println(unsigned long, int = DEC);
-      static void println(double, int = 2);
-      static void println();
-      operator bool() { return true; }
-
-    private:
-      static void printNumber(unsigned long, const uint8_t);
-      static void printFloat(double, uint8_t);
+    FORCE_INLINE static uint8_t dropped() { return Cfg::DROPPED_RX ? rx_dropped_bytes : 0; }
+    FORCE_INLINE static uint8_t buffer_overruns() { return Cfg::RX_OVERRUNS ? rx_buffer_overruns : 0; }
+    FORCE_INLINE static uint8_t framing_errors() { return Cfg::RX_FRAMING_ERRORS ? rx_framing_errors : 0; }
+    FORCE_INLINE static ring_buffer_pos_t rxMaxEnqueued() { return Cfg::MAX_RX_QUEUED ? rx_max_enqueued : 0; }
  };

  template <uint8_t serial>
@@ -270,12 +237,13 @@
    static constexpr bool RX_FRAMING_ERRORS = ENABLED(SERIAL_STATS_RX_FRAMING_ERRORS);
    static constexpr bool MAX_RX_QUEUED     = ENABLED(SERIAL_STATS_MAX_RX_QUEUED);
  };
-  extern MarlinSerial<MarlinSerialCfg<SERIAL_PORT>> customizedSerial1;
+
+  typedef Serial0Type< MarlinSerial< MarlinSerialCfg<SERIAL_PORT> > > MSerialT;
+  extern MSerialT customizedSerial1;

  #ifdef SERIAL_PORT_2
-
-    extern MarlinSerial<MarlinSerialCfg<SERIAL_PORT_2>> customizedSerial2;
-
+    typedef Serial0Type< MarlinSerial< MarlinSerialCfg<SERIAL_PORT_2> > > MSerialT2;
+    extern MSerialT2 customizedSerial2;
  #endif

 #endif // !USBCON
@@ -294,7 +262,8 @@
    static constexpr bool RX_OVERRUNS       = false;
  };

-  extern MarlinSerial<MMU2SerialCfg<MMU2_SERIAL_PORT>> mmuSerial;
+  typedef Serial0Type< MarlinSerial< MMU2SerialCfg<MMU2_SERIAL_PORT> > > MSerialT3;
+  extern MSerialT3 mmuSerial;
 #endif

 #ifdef LCD_SERIAL_PORT
@@ -307,7 +276,7 @@
    static constexpr bool DROPPED_RX          = false;
    static constexpr bool RX_FRAMING_ERRORS   = false;
    static constexpr bool MAX_RX_QUEUED       = false;
-    #if HAS_DGUS_LCD
+    #if HAS_DGUS_LCD || ENABLED(DGUS_LCD_UI_CREALITY_TOUCH)
      static constexpr unsigned int RX_SIZE   = DGUS_RX_BUFFER_SIZE;
      static constexpr unsigned int TX_SIZE   = DGUS_TX_BUFFER_SIZE;
      static constexpr bool RX_OVERRUNS       = ENABLED(SERIAL_STATS_RX_BUFFER_OVERRUNS);
@@ -322,11 +291,13 @@
    #endif
  };

-  extern MarlinSerial<LCDSerialCfg<LCD_SERIAL_PORT>> lcdSerial;

+  typedef Serial0Type< MarlinSerial< LCDSerialCfg<LCD_SERIAL_PORT> > > MSerialT4;
+  extern MSerialT4 lcdSerial;
 #endif

 // Use the UART for Bluetooth in AT90USB configurations
 #if defined(USBCON) && ENABLED(BLUETOOTH)
-  extern HardwareSerial bluetoothSerial;
+  typedef Serial0Type<HardwareSerial> MSerialT5;
+  extern MSerialT5 bluetoothSerial;
 #endif
@@ -235,8 +235,8 @@ static void print_is_also_tied() { SERIAL_ECHOPGM(" is also tied to this pin");

 inline void com_print(const uint8_t N, const uint8_t Z) {
  const uint8_t *TCCRA = (uint8_t*)TCCR_A(N);
-  SERIAL_ECHOPGM("    COM");
-  SERIAL_CHAR('0' + N, Z);
+  SERIAL_ECHOPAIR("    COM", AS_CHAR('0' + N));
+  SERIAL_CHAR(Z);
  SERIAL_ECHOPAIR(": ", int((*TCCRA >> (6 - Z * 2)) & 0x03));
 }

@@ -247,8 +247,8 @@ void timer_prefix(uint8_t T, char L, uint8_t N) {  // T - timer    L - pwm  N -
  uint8_t WGM = (((*TCCRB & _BV(WGM_2)) >> 1) | (*TCCRA & (_BV(WGM_0) | _BV(WGM_1))));
  if (N == 4) WGM |= ((*TCCRB & _BV(WGM_3)) >> 1);

-  SERIAL_ECHOPGM("    TIMER");
-  SERIAL_CHAR(T + '0', L);
+  SERIAL_ECHOPAIR("    TIMER", AS_CHAR(T + '0'));
+  SERIAL_CHAR(L);
  SERIAL_ECHO_SP(3);

  if (N == 3) {
@@ -262,19 +262,11 @@ void timer_prefix(uint8_t T, char L, uint8_t N) {  // T - timer    L - pwm  N -
  SERIAL_ECHOPAIR("    WGM: ", WGM);
  com_print(T,L);
  SERIAL_ECHOPAIR("    CS: ", (*TCCRB & (_BV(CS_0) | _BV(CS_1) | _BV(CS_2)) ));
-
-  SERIAL_ECHOPGM("    TCCR");
-  SERIAL_CHAR(T + '0');
-  SERIAL_ECHOPAIR("A: ", *TCCRA);
-
-  SERIAL_ECHOPGM("    TCCR");
-  SERIAL_CHAR(T + '0');
-  SERIAL_ECHOPAIR("B: ", *TCCRB);
+  SERIAL_ECHOPAIR("    TCCR", AS_CHAR(T + '0'), "A: ", *TCCRA);
+  SERIAL_ECHOPAIR("    TCCR", AS_CHAR(T + '0'), "B: ", *TCCRB);

  const uint8_t *TMSK = (uint8_t*)TIMSK(T);
-  SERIAL_ECHOPGM("    TIMSK");
-  SERIAL_CHAR(T + '0');
-  SERIAL_ECHOPAIR(": ", *TMSK);
+  SERIAL_ECHOPAIR("    TIMSK", AS_CHAR(T + '0'), ": ", *TMSK);

  const uint8_t OCIE = L - 'A' + 1;
  if (N == 3) { if (WGM == 0 || WGM == 2 || WGM ==  4 || WGM ==  6) err_is_counter(); }
@@ -102,4 +102,11 @@ uint16_t HAL_adc_get_result() {
  return HAL_adc_result;
 }

+// Forward the default serial port
+DefaultSerial MSerial(false, Serial);
+
+DefaultSerial1 MSerial1(false, Serial1);
+DefaultSerial2 MSerial2(false, Serial2);
+DefaultSerial3 MSerial3(false, Serial3);
+
 #endif // ARDUINO_ARCH_SAM
@@ -36,9 +36,20 @@

 #include <stdint.h>

-#define _MSERIAL(X) Serial##X
+#include "../../core/serial_hook.h"
+typedef ForwardSerial0Type< decltype(Serial) > DefaultSerial;
+extern DefaultSerial MSerial;
+
+typedef ForwardSerial0Type< decltype(Serial1) > DefaultSerial1;
+typedef ForwardSerial0Type< decltype(Serial2) > DefaultSerial2;
+typedef ForwardSerial0Type< decltype(Serial3) > DefaultSerial3;
+extern DefaultSerial1 MSerial1;
+extern DefaultSerial2 MSerial2;
+extern DefaultSerial3 MSerial3;
+
+#define _MSERIAL(X) MSerial##X
 #define MSERIAL(X) _MSERIAL(X)
-#define Serial0 Serial
+#define MSerial0 MSerial

 // Define MYSERIAL0/1 before MarlinSerial includes!
 #if SERIAL_PORT == -1 || ENABLED(EMERGENCY_PARSER)
@@ -61,7 +72,7 @@

 #ifdef MMU2_SERIAL_PORT
  #if WITHIN(MMU2_SERIAL_PORT, 0, 3)
-    #define MMU2_SERIAL MSERIAL(SERIAL_PORT)
+    #define MMU2_SERIAL MSERIAL(MMU2_SERIAL_PORT)
  #else
    #error "MMU2_SERIAL_PORT must be from 0 to 3. Please update your configuration."
  #endif
@@ -83,16 +94,6 @@
 // On AVR this is in math.h?
 #define square(x) ((x)*(x))

-#ifndef strncpy_P
-  #define strncpy_P(dest, src, num) strncpy((dest), (src), (num))
-#endif
-
-// Fix bug in pgm_read_ptr
-#undef pgm_read_ptr
-#define pgm_read_ptr(addr) (*((void**)(addr)))
-#undef pgm_read_word
-#define pgm_read_word(addr) (*((uint16_t*)(addr)))
-
 typedef int8_t pin_t;

 #define SHARED_SERVOS HAS_SERVOS
@@ -240,7 +240,7 @@
  }

  // all the others
-  static uint32_t spiDelayCyclesX4 = (F_CPU) / 1000000; // 4µs => 125khz
+  static uint32_t spiDelayCyclesX4 = 4 * (F_CPU) / 1000000; // 4µs => 125khz

  static uint8_t spiTransferX(uint8_t b) { // using Mode 0
    int bits = 8;
@@ -249,12 +249,12 @@
      b <<= 1; // little setup time

      WRITE(SD_SCK_PIN, HIGH);
-      __delay_4cycles(spiDelayCyclesX4);
+      DELAY_CYCLES(spiDelayCyclesX4);

      b |= (READ(SD_MISO_PIN) != 0);

      WRITE(SD_SCK_PIN, LOW);
-      __delay_4cycles(spiDelayCyclesX4);
+      DELAY_CYCLES(spiDelayCyclesX4);
    } while (--bits);
    return b;
  }
@@ -510,7 +510,7 @@
        spiRxBlock = (pfnSpiRxBlock)spiRxBlockX;
        break;
      default:
-        spiDelayCyclesX4 = ((F_CPU) / 1000000) >> (6 - spiRate);
+        spiDelayCyclesX4 = ((F_CPU) / 1000000) >> (6 - spiRate) << 2; // spiRate of 2 gives the maximum error with current CPU
        spiTransferTx = (pfnSpiTransfer)spiTransferX;
        spiTransferRx = (pfnSpiTransfer)spiTransferX;
        spiTxBlock = (pfnSpiTxBlock)spiTxBlockX;
@@ -382,7 +382,7 @@ void MarlinSerial<Cfg>::flush() {
 }

 template<typename Cfg>
-void MarlinSerial<Cfg>::write(const uint8_t c) {
+size_t MarlinSerial<Cfg>::write(const uint8_t c) {
  _written = true;

  if (Cfg::TX_SIZE == 0) {
@@ -400,7 +400,7 @@ void MarlinSerial<Cfg>::write(const uint8_t c) {
    // XOFF char at the RX isr, but it is properly handled there
    if (!(HWUART->UART_IMR & UART_IMR_TXRDY) && (HWUART->UART_SR & UART_SR_TXRDY)) {
      HWUART->UART_THR = c;
-      return;
+      return 1;
    }

    const uint8_t i = (tx_buffer.head + 1) & (Cfg::TX_SIZE - 1);
@@ -428,6 +428,7 @@ void MarlinSerial<Cfg>::write(const uint8_t c) {
    // Enable TX isr - Non atomic, but it will eventually enable TX isr
    HWUART->UART_IER = UART_IER_TXRDY;
  }
+  return 1;
 }

 template<typename Cfg>
@@ -473,169 +474,16 @@ void MarlinSerial<Cfg>::flushTX() {
  }
 }

-/**
- * Imports from print.h
- */
-
-template<typename Cfg>
-void MarlinSerial<Cfg>::print(char c, int base) {
-  print((long)c, base);
-}
-
-template<typename Cfg>
-void MarlinSerial<Cfg>::print(unsigned char b, int base) {
-  print((unsigned long)b, base);
-}
-
-template<typename Cfg>
-void MarlinSerial<Cfg>::print(int n, int base) {
-  print((long)n, base);
-}
-
-template<typename Cfg>
-void MarlinSerial<Cfg>::print(unsigned int n, int base) {
-  print((unsigned long)n, base);
-}
-
-template<typename Cfg>
-void MarlinSerial<Cfg>::print(long n, int base) {
-  if (base == 0) write(n);
-  else if (base == 10) {
-    if (n < 0) { print('-'); n = -n; }
-    printNumber(n, 10);
-  }
-  else
-    printNumber(n, base);
-}
-
-template<typename Cfg>
-void MarlinSerial<Cfg>::print(unsigned long n, int base) {
-  if (base == 0) write(n);
-  else printNumber(n, base);
-}
-
-template<typename Cfg>
-void MarlinSerial<Cfg>::print(double n, int digits) {
-  printFloat(n, digits);
-}
-
-template<typename Cfg>
-void MarlinSerial<Cfg>::println() {
-  print('\r');
-  print('\n');
-}
-
-template<typename Cfg>
-void MarlinSerial<Cfg>::println(const String& s) {
-  print(s);
-  println();
-}
-
-template<typename Cfg>
-void MarlinSerial<Cfg>::println(const char c[]) {
-  print(c);
-  println();
-}
-
-template<typename Cfg>
-void MarlinSerial<Cfg>::println(char c, int base) {
-  print(c, base);
-  println();
-}
-
-template<typename Cfg>
-void MarlinSerial<Cfg>::println(unsigned char b, int base) {
-  print(b, base);
-  println();
-}
-
-template<typename Cfg>
-void MarlinSerial<Cfg>::println(int n, int base) {
-  print(n, base);
-  println();
-}
-
-template<typename Cfg>
-void MarlinSerial<Cfg>::println(unsigned int n, int base) {
-  print(n, base);
-  println();
-}
-
-template<typename Cfg>
-void MarlinSerial<Cfg>::println(long n, int base) {
-  print(n, base);
-  println();
-}
-
-template<typename Cfg>
-void MarlinSerial<Cfg>::println(unsigned long n, int base) {
-  print(n, base);
-  println();
-}
-
-template<typename Cfg>
-void MarlinSerial<Cfg>::println(double n, int digits) {
-  print(n, digits);
-  println();
-}
-
-// Private Methods
-template<typename Cfg>
-void MarlinSerial<Cfg>::printNumber(unsigned long n, uint8_t base) {
-  if (n) {
-    unsigned char buf[8 * sizeof(long)]; // Enough space for base 2
-    int8_t i = 0;
-    while (n) {
-      buf[i++] = n % base;
-      n /= base;
-    }
-    while (i--)
-      print((char)(buf[i] + (buf[i] < 10 ? '0' : 'A' - 10)));
-  }
-  else
-    print('0');
-}
-
-template<typename Cfg>
-void MarlinSerial<Cfg>::printFloat(double number, uint8_t digits) {
-  // Handle negative numbers
-  if (number < 0.0) {
-    print('-');
-    number = -number;
-  }
-
-  // Round correctly so that print(1.999, 2) prints as "2.00"
-  double rounding = 0.5;
-  LOOP_L_N(i, digits) rounding *= 0.1;
-  number += rounding;
-
-  // Extract the integer part of the number and print it
-  unsigned long int_part = (unsigned long)number;
-  double remainder = number - (double)int_part;
-  print(int_part);
-
-  // Print the decimal point, but only if there are digits beyond
-  if (digits) {
-    print('.');
-    // Extract digits from the remainder one at a time
-    while (digits--) {
-      remainder *= 10.0;
-      int toPrint = int(remainder);
-      print(toPrint);
-      remainder -= toPrint;
-    }
-  }
-}

 // If not using the USB port as serial port
 #if SERIAL_PORT >= 0
-  template class MarlinSerial<MarlinSerialCfg<SERIAL_PORT>>;      // Define
-  MarlinSerial<MarlinSerialCfg<SERIAL_PORT>> customizedSerial1;   // Instantiate
+  template class MarlinSerial< MarlinSerialCfg<SERIAL_PORT> >;
+  MSerialT customizedSerial1(MarlinSerialCfg<SERIAL_PORT>::EMERGENCYPARSER);
 #endif

 #if defined(SERIAL_PORT_2) && SERIAL_PORT_2 >= 0
-  template class MarlinSerial<MarlinSerialCfg<SERIAL_PORT_2>>;    // Define
-  MarlinSerial<MarlinSerialCfg<SERIAL_PORT_2>> customizedSerial2; // Instantiate
+  template class MarlinSerial< MarlinSerialCfg<SERIAL_PORT_2> >;
+  MSerialT2 customizedSerial2(MarlinSerialCfg<SERIAL_PORT_2>::EMERGENCYPARSER);
 #endif

 #endif // ARDUINO_ARCH_SAM
@@ -30,11 +30,7 @@
 #include <WString.h>

 #include "../../inc/MarlinConfigPre.h"
-
-#define DEC 10
-#define HEX 16
-#define OCT 8
-#define BIN 2
+#include "../../core/serial_hook.h"

 // Define constants and variables for buffering incoming serial data.  We're
 // using a ring buffer (I think), in which rx_buffer_head is the index of the
@@ -119,7 +115,7 @@ public:
  static int read();
  static void flush();
  static ring_buffer_pos_t available();
-  static void write(const uint8_t c);
+  static size_t write(const uint8_t c);
  static void flushTX();

  static inline bool emergency_parser_enabled() { return Cfg::EMERGENCYPARSER; }
@@ -128,35 +124,6 @@ public:
  FORCE_INLINE static uint8_t buffer_overruns() { return Cfg::RX_OVERRUNS ? rx_buffer_overruns : 0; }
  FORCE_INLINE static uint8_t framing_errors() { return Cfg::RX_FRAMING_ERRORS ? rx_framing_errors : 0; }
  FORCE_INLINE static ring_buffer_pos_t rxMaxEnqueued() { return Cfg::MAX_RX_QUEUED ? rx_max_enqueued : 0; }
-
-  FORCE_INLINE static void write(const char* str) { while (*str) write(*str++); }
-  FORCE_INLINE static void write(const uint8_t* buffer, size_t size) { while (size--) write(*buffer++); }
-  FORCE_INLINE static void print(const String& s) { for (int i = 0; i < (int)s.length(); i++) write(s[i]); }
-  FORCE_INLINE static void print(const char* str) { write(str); }
-
-  static void print(char, int = 0);
-  static void print(unsigned char, int = 0);
-  static void print(int, int = DEC);
-  static void print(unsigned int, int = DEC);
-  static void print(long, int = DEC);
-  static void print(unsigned long, int = DEC);
-  static void print(double, int = 2);
-
-  static void println(const String& s);
-  static void println(const char[]);
-  static void println(char, int = 0);
-  static void println(unsigned char, int = 0);
-  static void println(int, int = DEC);
-  static void println(unsigned int, int = DEC);
-  static void println(long, int = DEC);
-  static void println(unsigned long, int = DEC);
-  static void println(double, int = 2);
-  static void println();
-  operator bool() { return true; }
-
-private:
-  static void printNumber(unsigned long, const uint8_t);
-  static void printFloat(double, uint8_t);
 };

 // Serial port configuration
@@ -174,9 +141,11 @@ struct MarlinSerialCfg {
 };

 #if SERIAL_PORT >= 0
-  extern MarlinSerial<MarlinSerialCfg<SERIAL_PORT>> customizedSerial1;
+  typedef Serial0Type< MarlinSerial< MarlinSerialCfg<SERIAL_PORT> > > MSerialT;
+  extern MSerialT customizedSerial1;
 #endif

 #if defined(SERIAL_PORT_2) && SERIAL_PORT_2 >= 0
-  extern MarlinSerial<MarlinSerialCfg<SERIAL_PORT_2>> customizedSerial2;
+  typedef Serial0Type< MarlinSerial< MarlinSerialCfg<SERIAL_PORT_2> > > MSerialT2;
+  extern MSerialT2 customizedSerial2;
 #endif
@@ -33,10 +33,6 @@

 #include "MarlinSerialUSB.h"

-#if ENABLED(EMERGENCY_PARSER)
-  #include "../../feature/e_parser.h"
-#endif
-
 // Imports from Atmel USB Stack/CDC implementation
 extern "C" {
  bool usb_task_cdc_isenabled();
@@ -50,10 +46,6 @@ extern "C" {
 // Pending character
 static int pending_char = -1;

-#if ENABLED(EMERGENCY_PARSER)
-  static EmergencyParser::State emergency_state; // = EP_RESET
-#endif
-
 // Public Methods
 void MarlinSerialUSB::begin(const long) {}

@@ -73,7 +65,7 @@ int MarlinSerialUSB::peek() {

  pending_char = udi_cdc_getc();

-  TERN_(EMERGENCY_PARSER, emergency_parser.update(emergency_state, (char)pending_char));
+  TERN_(EMERGENCY_PARSER, emergency_parser.update(static_cast<MSerialT*>(this)->emergency_state, (char)pending_char));

  return pending_char;
 }
@@ -95,7 +87,7 @@ int MarlinSerialUSB::read() {

  int c = udi_cdc_getc();

-  TERN_(EMERGENCY_PARSER, emergency_parser.update(emergency_state, (char)c));
+  TERN_(EMERGENCY_PARSER, emergency_parser.update(static_cast<MSerialT*>(this)->emergency_state, (char)c));

  return c;
 }
@@ -109,15 +101,14 @@ bool MarlinSerialUSB::available() {
 }

 void MarlinSerialUSB::flush() { }
-void MarlinSerialUSB::flushTX() { }

-void MarlinSerialUSB::write(const uint8_t c) {
+size_t MarlinSerialUSB::write(const uint8_t c) {

  /* Do not even bother sending anything if USB CDC is not enumerated
     or not configured on the PC side or there is no program on the PC
     listening to our messages */
  if (!usb_task_cdc_isenabled() || !usb_task_cdc_dtr_active())
-    return;
+    return 0;

  /* Wait until the PC has read the pending to be sent data */
  while (usb_task_cdc_isenabled() &&
@@ -129,161 +120,20 @@ void MarlinSerialUSB::write(const uint8_t c) {
     or not configured on the PC side or there is no program on the PC
     listening to our messages at this point */
  if (!usb_task_cdc_isenabled() || !usb_task_cdc_dtr_active())
-    return;
+    return 0;

  // Fifo full
  //  udi_cdc_signal_overrun();
  udi_cdc_putc(c);
-}
-
-/**
- * Imports from print.h
- */
-
-void MarlinSerialUSB::print(char c, int base) {
-  print((long)c, base);
-}
-
-void MarlinSerialUSB::print(unsigned char b, int base) {
-  print((unsigned long)b, base);
-}
-
-void MarlinSerialUSB::print(int n, int base) {
-  print((long)n, base);
-}
-
-void MarlinSerialUSB::print(unsigned int n, int base) {
-  print((unsigned long)n, base);
-}
-
-void MarlinSerialUSB::print(long n, int base) {
-  if (base == 0)
-    write(n);
-  else if (base == 10) {
-    if (n < 0) {
-      print('-');
-      n = -n;
-    }
-    printNumber(n, 10);
-  }
-  else
-    printNumber(n, base);
-}
-
-void MarlinSerialUSB::print(unsigned long n, int base) {
-  if (base == 0) write(n);
-  else printNumber(n, base);
-}
-
-void MarlinSerialUSB::print(double n, int digits) {
-  printFloat(n, digits);
-}
-
-void MarlinSerialUSB::println() {
-  print('\r');
-  print('\n');
-}
-
-void MarlinSerialUSB::println(const String& s) {
-  print(s);
-  println();
-}
-
-void MarlinSerialUSB::println(const char c[]) {
-  print(c);
-  println();
-}
-
-void MarlinSerialUSB::println(char c, int base) {
-  print(c, base);
-  println();
-}
-
-void MarlinSerialUSB::println(unsigned char b, int base) {
-  print(b, base);
-  println();
-}
-
-void MarlinSerialUSB::println(int n, int base) {
-  print(n, base);
-  println();
-}
-
-void MarlinSerialUSB::println(unsigned int n, int base) {
-  print(n, base);
-  println();
-}
-
-void MarlinSerialUSB::println(long n, int base) {
-  print(n, base);
-  println();
-}
-
-void MarlinSerialUSB::println(unsigned long n, int base) {
-  print(n, base);
-  println();
-}
-
-void MarlinSerialUSB::println(double n, int digits) {
-  print(n, digits);
-  println();
-}
-
-// Private Methods
-
-void MarlinSerialUSB::printNumber(unsigned long n, uint8_t base) {
-  if (n) {
-    unsigned char buf[8 * sizeof(long)]; // Enough space for base 2
-    int8_t i = 0;
-    while (n) {
-      buf[i++] = n % base;
-      n /= base;
-    }
-    while (i--)
-      print((char)(buf[i] + (buf[i] < 10 ? '0' : 'A' - 10)));
-  }
-  else
-    print('0');
-}
-
-void MarlinSerialUSB::printFloat(double number, uint8_t digits) {
-  // Handle negative numbers
-  if (number < 0.0) {
-    print('-');
-    number = -number;
-  }
-
-  // Round correctly so that print(1.999, 2) prints as "2.00"
-  double rounding = 0.5;
-  LOOP_L_N(i, digits)
-    rounding *= 0.1;
-
-  number += rounding;
-
-  // Extract the integer part of the number and print it
-  unsigned long int_part = (unsigned long)number;
-  double remainder = number - (double)int_part;
-  print(int_part);
-
-  // Print the decimal point, but only if there are digits beyond
-  if (digits) {
-    print('.');
-    // Extract digits from the remainder one at a time
-    while (digits--) {
-      remainder *= 10.0;
-      int toPrint = int(remainder);
-      print(toPrint);
-      remainder -= toPrint;
-    }
-  }
+  return 1;
 }

 // Preinstantiate
 #if SERIAL_PORT == -1
-  MarlinSerialUSB customizedSerial1;
+  MSerialT customizedSerial1(TERN0(EMERGENCY_PARSER, true));
 #endif
 #if SERIAL_PORT_2 == -1
-  MarlinSerialUSB customizedSerial2;
+  MSerialT customizedSerial2(TERN0(EMERGENCY_PARSER, true));
 #endif

 #endif // HAS_USB_SERIAL
@@ -27,73 +27,37 @@
 */

 #include "../../inc/MarlinConfig.h"
-
 #if HAS_USB_SERIAL

 #include <WString.h>
+#include "../../core/serial_hook.h"

-#define DEC 10
-#define HEX 16
-#define OCT 8
-#define BIN 2

-class MarlinSerialUSB {
-
-public:
-  MarlinSerialUSB() {};
-  static void begin(const long);
-  static void end();
-  static int peek();
-  static int read();
-  static void flush();
-  static void flushTX();
-  static bool available();
-  static void write(const uint8_t c);
+struct MarlinSerialUSB {
+  void begin(const long);
+  void end();
+  int peek();
+  int read();
+  void flush();
+  bool available();
+  size_t write(const uint8_t c);

  #if ENABLED(SERIAL_STATS_DROPPED_RX)
-    FORCE_INLINE static uint32_t dropped() { return 0; }
+    FORCE_INLINE uint32_t dropped() { return 0; }
  #endif

  #if ENABLED(SERIAL_STATS_MAX_RX_QUEUED)
-    FORCE_INLINE static int rxMaxEnqueued() { return 0; }
+    FORCE_INLINE int rxMaxEnqueued() { return 0; }
  #endif
-
-  FORCE_INLINE static void write(const char* str) { while (*str) write(*str++); }
-  FORCE_INLINE static void write(const uint8_t* buffer, size_t size) { while (size--) write(*buffer++); }
-  FORCE_INLINE static void print(const String& s) { for (int i = 0; i < (int)s.length(); i++) write(s[i]); }
-  FORCE_INLINE static void print(const char* str) { write(str); }
-
-  static void print(char, int = 0);
-  static void print(unsigned char, int = 0);
-  static void print(int, int = DEC);
-  static void print(unsigned int, int = DEC);
-  static void print(long, int = DEC);
-  static void print(unsigned long, int = DEC);
-  static void print(double, int = 2);
-
-  static void println(const String& s);
-  static void println(const char[]);
-  static void println(char, int = 0);
-  static void println(unsigned char, int = 0);
-  static void println(int, int = DEC);
-  static void println(unsigned int, int = DEC);
-  static void println(long, int = DEC);
-  static void println(unsigned long, int = DEC);
-  static void println(double, int = 2);
-  static void println();
-  operator bool() { return true; }
-
-private:
-  static void printNumber(unsigned long, const uint8_t);
-  static void printFloat(double, uint8_t);
 };
+typedef Serial0Type<MarlinSerialUSB> MSerialT;

 #if SERIAL_PORT == -1
-  extern MarlinSerialUSB customizedSerial1;
+  extern MSerialT customizedSerial1;
 #endif

 #if SERIAL_PORT_2 == -1
-  extern MarlinSerialUSB customizedSerial2;
+  extern MSerialT customizedSerial2;
 #endif

 #endif // HAS_USB_SERIAL
@@ -59,6 +59,7 @@

 #if ENABLED(U8GLIB_ST7920)

+#include "../../../inc/MarlinConfig.h"
 #include "../../shared/Delay.h"

 #include <U8glib.h>
@@ -59,6 +59,7 @@

 #if HAS_MARLINUI_U8GLIB

+#include "../../../inc/MarlinConfig.h"
 #include "../../shared/Delay.h"

 #include <U8glib.h>
@@ -68,7 +68,7 @@ Ctrl_status sd_mmc_spi_usb_read_10(uint32_t addr, uint16_t nb_sector) {
  {
    char buffer[80];
    sprintf_P(buffer, PSTR("SDRD: %d @ 0x%08x\n"), nb_sector, addr);
-    PORT_REDIRECT(0);
+    PORT_REDIRECT(SERIAL_PORTMASK(0));
    SERIAL_ECHO(buffer);
  }
  #endif
@@ -108,7 +108,7 @@ Ctrl_status sd_mmc_spi_usb_write_10(uint32_t addr, uint16_t nb_sector) {
  {
    char buffer[80];
    sprintf_P(buffer, PSTR("SDWR: %d @ 0x%08x\n"), nb_sector, addr);
-    PORT_REDIRECT(0);
+    PORT_REDIRECT(SERIAL_PORTMASK(0));
    SERIAL_ECHO(buffer);
  }
  #endif
@@ -24,10 +24,7 @@

 #ifdef ARDUINO_ARCH_ESP32

-FlushableHardwareSerial::FlushableHardwareSerial(int uart_nr)
-    : HardwareSerial(uart_nr)
-{}

-FlushableHardwareSerial flushableSerial(0);
+Serial0Type<FlushableHardwareSerial> flushableSerial(false, 0);

 #endif // ARDUINO_ARCH_ESP32
@@ -24,14 +24,13 @@
 #ifdef ARDUINO_ARCH_ESP32

 #include <HardwareSerial.h>
+#include "../../core/serial_hook.h"

 class FlushableHardwareSerial : public HardwareSerial {
 public:
-  FlushableHardwareSerial(int uart_nr);
-
-  inline void flushTX() { /* No need to flush the hardware serial, but defined here for compatibility. */ }
+  FlushableHardwareSerial(int uart_nr) : HardwareSerial(uart_nr) {}
 };

-extern FlushableHardwareSerial flushableSerial;
+extern Serial0Type<FlushableHardwareSerial> flushableSerial;

 #endif // ARDUINO_ARCH_ESP32
@@ -40,6 +40,10 @@
  #endif
 #endif

+#if ENABLED(ESP3D_WIFISUPPORT)
+  DefaultSerial MSerial(false, Serial2Socket);
+#endif
+
 // ------------------------
 // Externs
 // ------------------------
@@ -86,8 +90,6 @@ volatile int numPWMUsed = 0,

 #endif

-void HAL_init() { TERN_(I2S_STEPPER_STREAM, i2s_init()); }
-
 void HAL_init_board() {

  #if ENABLED(ESP3D_WIFISUPPORT)
@@ -122,6 +124,10 @@ void HAL_init_board() {
    #endif
  #endif

+  // Initialize the i2s peripheral only if the I2S stepper stream is enabled.
+  // The following initialization is performed after Serial1 and Serial2 are defined as
+  // their native pins might conflict with the i2s stream even when they are remapped.
+  TERN_(I2S_STEPPER_STREAM, i2s_init());
 }

 void HAL_idletask() {
@@ -55,7 +55,9 @@ extern portMUX_TYPE spinlock;

 #if EITHER(WIFISUPPORT, ESP3D_WIFISUPPORT)
  #if ENABLED(ESP3D_WIFISUPPORT)
-    #define MYSERIAL1 Serial2Socket
+    typedef ForwardSerial0Type< decltype(Serial2Socket) > DefaultSerial;
+    extern DefaultSerial MSerial;
+    #define MYSERIAL1 MSerial
  #else
    #define MYSERIAL1 webSocketSerial
  #endif
@@ -67,10 +69,6 @@ extern portMUX_TYPE spinlock;
 #define ENABLE_ISRS()  if (spinlock.owner != portMUX_FREE_VAL) portEXIT_CRITICAL(&spinlock)
 #define DISABLE_ISRS() portENTER_CRITICAL(&spinlock)

-// Fix bug in pgm_read_ptr
-#undef pgm_read_ptr
-#define pgm_read_ptr(addr) (*(addr))
-
 // ------------------------
 // Types
 // ------------------------
@@ -141,7 +139,7 @@ void HAL_adc_start_conversion(const uint8_t adc_pin);
 #define HAL_IDLETASK 1
 #define BOARD_INIT() HAL_init_board();
 void HAL_idletask();
-void HAL_init();
+inline void HAL_init() {}
 void HAL_init_board();

 //
@@ -29,7 +29,7 @@
 #include "wifi.h"
 #include <ESPAsyncWebServer.h>

-WebSocketSerial webSocketSerial;
+MSerialT webSocketSerial(false);
 AsyncWebSocket ws("/ws"); // TODO Move inside the class.

 // RingBuffer impl
@@ -144,9 +144,5 @@ size_t WebSocketSerial::write(const uint8_t* buffer, size_t size) {
  return written;
 }

-void WebSocketSerial::flushTX() {
-  // No need to do anything as there's no benefit to sending partial lines over the websocket connection.
-}
-
 #endif // WIFISUPPORT
 #endif // ARDUINO_ARCH_ESP32
@@ -22,6 +22,7 @@
 #pragma once

 #include "../../inc/MarlinConfig.h"
+#include "../../core/serial_hook.h"

 #include <Stream.h>

@@ -68,12 +69,9 @@ public:
  int peek();
  int read();
  void flush();
-  void flushTX();
  size_t write(const uint8_t c);
  size_t write(const uint8_t* buffer, size_t size);

-  operator bool() { return true; }
-
  #if ENABLED(SERIAL_STATS_DROPPED_RX)
    FORCE_INLINE uint32_t dropped() { return 0; }
  #endif
@@ -83,4 +81,5 @@ public:
  #endif
 };

-extern WebSocketSerial webSocketSerial;
+typedef Serial0Type<WebSocketSerial> MSerialT;
+extern MSerialT webSocketSerial;
@@ -24,7 +24,7 @@
 #include "../../inc/MarlinConfig.h"
 #include "../shared/Delay.h"

-HalSerial usb_serial;
+MSerialT usb_serial(TERN0(EMERGENCY_PARSER, true));

 // U8glib required functions
 extern "C" {
@@ -60,7 +60,7 @@ uint8_t _getc();

 #define SHARED_SERVOS HAS_SERVOS

-extern HalSerial usb_serial;
+extern MSerialT usb_serial;
 #define MYSERIAL0 usb_serial

 #define ST7920_DELAY_1 DELAY_NS(600)
@@ -113,8 +113,3 @@ inline void HAL_reboot() {}  // reboot the board or restart the bootloader
 FORCE_INLINE static void DELAY_CYCLES(uint64_t x) {
  Clock::delayCycles(x);
 }
-
-// Add strcmp_P if missing
-#ifndef strcmp_P
-  #define strcmp_P(a, b) strcmp((a), (b))
-#endif
@@ -73,27 +73,6 @@ extern "C" {
  void GpioDisableInt(uint32_t port, uint32_t pin);
 }

-// Program Memory
-#define pgm_read_ptr(addr)        (*((void**)(addr)))
-#define pgm_read_byte_near(addr)  (*((uint8_t*)(addr)))
-#define pgm_read_float_near(addr) (*((float*)(addr)))
-#define pgm_read_word_near(addr)  (*((uint16_t*)(addr)))
-#define pgm_read_dword_near(addr) (*((uint32_t*)(addr)))
-#define pgm_read_byte(addr)       pgm_read_byte_near(addr)
-#define pgm_read_float(addr)      pgm_read_float_near(addr)
-#define pgm_read_word(addr)       pgm_read_word_near(addr)
-#define pgm_read_dword(addr)      pgm_read_dword_near(addr)
-
-using std::memcpy;
-#define memcpy_P memcpy
-#define sprintf_P sprintf
-#define strstr_P strstr
-#define strncpy_P strncpy
-#define vsnprintf_P vsnprintf
-#define strcpy_P strcpy
-#define snprintf_P snprintf
-#define strlen_P strlen
-
 // Time functions
 extern "C" void delay(const int milis);
 void _delay_ms(const int delay);
@@ -25,6 +25,7 @@
 #if ENABLED(EMERGENCY_PARSER)
  #include "../../../feature/e_parser.h"
 #endif
+#include "../../../core/serial_hook.h"

 #include <stdarg.h>
 #include <stdio.h>
@@ -73,19 +74,11 @@ private:
  volatile uint32_t index_read;
 };

-class HalSerial {
-public:
-
-  #if ENABLED(EMERGENCY_PARSER)
-    EmergencyParser::State emergency_state;
-    static inline bool emergency_parser_enabled() { return true; }
-  #endif
-
+struct HalSerial {
  HalSerial() { host_connected = true; }

  void begin(int32_t) {}
-
-  void end() {}
+  void end()          {}

  int peek() {
    uint8_t value;
@@ -100,7 +93,7 @@ public:
    return transmit_buffer.write(c);
  }

-  operator bool() { return host_connected; }
+  bool connected() { return host_connected; }

  uint16_t available() {
    return (uint16_t)receive_buffer.available();
@@ -117,92 +110,9 @@ public:
      while (transmit_buffer.available()) { /* nada */ }
  }

-  void printf(const char *format, ...) {
-    static char buffer[256];
-    va_list vArgs;
-    va_start(vArgs, format);
-    int length = vsnprintf((char *) buffer, 256, (char const *) format, vArgs);
-    va_end(vArgs);
-    if (length > 0 && length < 256) {
-      if (host_connected) {
-        for (int i = 0; i < length;) {
-          if (transmit_buffer.write(buffer[i])) {
-            ++i;
-          }
-        }
-      }
-    }
-  }
-
-  #define DEC 10
-  #define HEX 16
-  #define OCT 8
-  #define BIN 2
-
-  void print_bin(uint32_t value, uint8_t num_digits) {
-    uint32_t mask = 1 << (num_digits -1);
-    for (uint8_t i = 0; i < num_digits; i++) {
-      if (!(i %  4) && i) write(' ');
-      if (!(i % 16) && i) write(' ');
-      if (value & mask)   write('1');
-      else                write('0');
-      value <<= 1;
-    }
-  }
-
-  void print(const char value[]) { printf("%s" , value); }
-  void print(char value, int nbase = 0) {
-    if (nbase == BIN) print_bin(value, 8);
-    else if (nbase == OCT) printf("%3o", value);
-    else if (nbase == HEX) printf("%2X", value);
-    else if (nbase == DEC ) printf("%d", value);
-    else printf("%c" , value);
-  }
-  void print(unsigned char value, int nbase = 0) {
-    if (nbase == BIN) print_bin(value, 8);
-    else if (nbase == OCT) printf("%3o", value);
-    else if (nbase == HEX) printf("%2X", value);
-    else printf("%u" , value);
-  }
-  void print(int value, int nbase = 0) {
-    if (nbase == BIN) print_bin(value, 16);
-    else if (nbase == OCT) printf("%6o", value);
-    else if (nbase == HEX) printf("%4X", value);
-    else printf("%d", value);
-  }
-  void print(unsigned int value, int nbase = 0) {
-    if (nbase == BIN) print_bin(value, 16);
-    else if (nbase == OCT) printf("%6o", value);
-    else if (nbase == HEX) printf("%4X", value);
-    else printf("%u" , value);
-  }
-  void print(long value, int nbase = 0) {
-    if (nbase == BIN) print_bin(value, 32);
-    else if (nbase == OCT) printf("%11o", value);
-    else if (nbase == HEX) printf("%8X", value);
-    else printf("%ld" , value);
-  }
-  void print(unsigned long value, int nbase = 0) {
-    if (nbase == BIN) print_bin(value, 32);
-    else if (nbase == OCT) printf("%11o", value);
-    else if (nbase == HEX) printf("%8X", value);
-    else printf("%lu" , value);
-  }
-  void print(float value, int round = 6)  { printf("%f" , value); }
-  void print(double value, int round = 6) { printf("%f" , value); }
-
-  void println(const char value[]) { printf("%s\n" , value); }
-  void println(char value, int nbase = 0) { print(value, nbase); println(); }
-  void println(unsigned char value, int nbase = 0) { print(value, nbase); println(); }
-  void println(int value, int nbase = 0) { print(value, nbase); println(); }
-  void println(unsigned int value, int nbase = 0) { print(value, nbase); println(); }
-  void println(long value, int nbase = 0) { print(value, nbase); println(); }
-  void println(unsigned long value, int nbase = 0) { print(value, nbase); println(); }
-  void println(float value, int round = 6) { printf("%f\n" , value); }
-  void println(double value, int round = 6) { printf("%f\n" , value); }
-  void println() { print('\n'); }
-
  volatile RingBuffer<uint8_t, 128> receive_buffer;
  volatile RingBuffer<uint8_t, 128> transmit_buffer;
  volatile bool host_connected;
 };
+
+typedef Serial0Type<HalSerial> MSerialT;
@@ -29,6 +29,8 @@
  #include "watchdog.h"
 #endif

+DefaultSerial USBSerial(false, UsbSerial);
+
 uint32_t HAL_adc_reading = 0;

 // U8glib required functions
@@ -61,7 +63,12 @@ int16_t PARSED_PIN_INDEX(const char code, const int16_t dval) {
  return ind > -1 ? ind : dval;
 }

-void flashFirmware(const int16_t) { NVIC_SystemReset(); }
+void flashFirmware(const int16_t) {
+  delay(500);          // Give OS time to disconnect
+  USB_Connect(false);  // USB clear connection
+  delay(1000);         // Give OS time to notice
+  NVIC_SystemReset();
+}

 void HAL_clear_reset_source(void) {
  TERN_(USE_WATCHDOG, watchdog_clear_timeout_flag());
@@ -60,12 +60,15 @@ extern "C" volatile uint32_t _millis;
  #define ST7920_DELAY_3 DELAY_NS(750)
 #endif

+typedef ForwardSerial0Type< decltype(UsbSerial) > DefaultSerial;
+extern DefaultSerial USBSerial;
+
 #define _MSERIAL(X) MSerial##X
 #define MSERIAL(X) _MSERIAL(X)
 #define MSerial0 MSerial

 #if SERIAL_PORT == -1
-  #define MYSERIAL0 UsbSerial
+  #define MYSERIAL0 USBSerial
 #elif WITHIN(SERIAL_PORT, 0, 3)
  #define MYSERIAL0 MSERIAL(SERIAL_PORT)
 #else
@@ -74,7 +77,7 @@ extern "C" volatile uint32_t _millis;

 #ifdef SERIAL_PORT_2
  #if SERIAL_PORT_2 == -1
-    #define MYSERIAL1 UsbSerial
+    #define MYSERIAL1 USBSerial
  #elif WITHIN(SERIAL_PORT_2, 0, 3)
    #define MYSERIAL1 MSERIAL(SERIAL_PORT_2)
  #else
@@ -84,7 +87,7 @@ extern "C" volatile uint32_t _millis;

 #ifdef MMU2_SERIAL_PORT
  #if MMU2_SERIAL_PORT == -1
-    #define MMU2_SERIAL UsbSerial
+    #define MMU2_SERIAL USBSerial
  #elif WITHIN(MMU2_SERIAL_PORT, 0, 3)
    #define MMU2_SERIAL MSERIAL(MMU2_SERIAL_PORT)
  #else
@@ -94,7 +97,7 @@ extern "C" volatile uint32_t _millis;

 #ifdef LCD_SERIAL_PORT
  #if LCD_SERIAL_PORT == -1
-    #define LCD_SERIAL UsbSerial
+    #define LCD_SERIAL USBSerial
  #elif WITHIN(LCD_SERIAL_PORT, 0, 3)
    #define LCD_SERIAL MSERIAL(LCD_SERIAL_PORT)
  #else
@@ -214,16 +217,3 @@ void HAL_clear_reset_source(void);
 uint8_t HAL_get_reset_source(void);

 inline void HAL_reboot() {}  // reboot the board or restart the bootloader
-
-// Add strcmp_P if missing
-#ifndef strcmp_P
-  #define strcmp_P(a, b) strcmp((a), (b))
-#endif
-
-#ifndef strcat_P
-  #define strcat_P(a, b) strcat((a), (b))
-#endif
-
-#ifndef strcpy_P
-  #define strcpy_P(a, b) strcpy((a), (b))
-#endif
@@ -24,20 +24,20 @@
 #include "../../inc/MarlinConfigPre.h"
 #include "MarlinSerial.h"

-#if USING_SERIAL_0
-  MarlinSerial MSerial(LPC_UART0);
+#if ANY_SERIAL_IS(0)
+  MSerialT MSerial(true, LPC_UART0);
  extern "C" void UART0_IRQHandler() { MSerial.IRQHandler(); }
 #endif
-#if USING_SERIAL_1
-  MarlinSerial MSerial1((LPC_UART_TypeDef *) LPC_UART1);
+#if ANY_SERIAL_IS(1)
+  MSerialT MSerial1(true, (LPC_UART_TypeDef *) LPC_UART1);
  extern "C" void UART1_IRQHandler() { MSerial1.IRQHandler(); }
 #endif
-#if USING_SERIAL_2
-  MarlinSerial MSerial2(LPC_UART2);
+#if ANY_SERIAL_IS(2)
+  MSerialT MSerial2(true, LPC_UART2);
  extern "C" void UART2_IRQHandler() { MSerial2.IRQHandler(); }
 #endif
-#if USING_SERIAL_3
-  MarlinSerial MSerial3(LPC_UART3);
+#if ANY_SERIAL_IS(3)
+  MSerialT MSerial3(true, LPC_UART3);
  extern "C" void UART3_IRQHandler() { MSerial3.IRQHandler(); }
 #endif

@@ -28,6 +28,7 @@
 #if ENABLED(EMERGENCY_PARSER)
  #include "../../feature/e_parser.h"
 #endif
+#include "../../core/serial_hook.h"

 #ifndef SERIAL_PORT
  #define SERIAL_PORT 0
@@ -41,27 +42,20 @@

 class MarlinSerial : public HardwareSerial<RX_BUFFER_SIZE, TX_BUFFER_SIZE> {
 public:
-  MarlinSerial(LPC_UART_TypeDef *UARTx) :
-    HardwareSerial<RX_BUFFER_SIZE, TX_BUFFER_SIZE>(UARTx)
-    #if ENABLED(EMERGENCY_PARSER)
-      , emergency_state(EmergencyParser::State::EP_RESET)
-    #endif
-    { }
+  MarlinSerial(LPC_UART_TypeDef *UARTx) : HardwareSerial<RX_BUFFER_SIZE, TX_BUFFER_SIZE>(UARTx) { }

  void end() {}

  #if ENABLED(EMERGENCY_PARSER)
    bool recv_callback(const char c) override {
-      emergency_parser.update(emergency_state, c);
+      emergency_parser.update(static_cast<Serial0Type<MarlinSerial> *>(this)->emergency_state, c);
      return true; // do not discard character
    }
-
-    EmergencyParser::State emergency_state;
-    static inline bool emergency_parser_enabled() { return true; }
  #endif
 };

-extern MarlinSerial MSerial;
-extern MarlinSerial MSerial1;
-extern MarlinSerial MSerial2;
-extern MarlinSerial MSerial3;
+typedef Serial0Type<MarlinSerial> MSerialT;
+extern MSerialT MSerial;
+extern MSerialT MSerial1;
+extern MSerialT MSerial2;
+extern MSerialT MSerial3;
@@ -84,16 +84,16 @@ static void debug_rw(const bool write, int &pos, const uint8_t *value, const siz
  PGM_P const rw_str = write ? PSTR("write") : PSTR("read");
  SERIAL_CHAR(' ');
  serialprintPGM(rw_str);
-  SERIAL_ECHOLNPAIR("_data(", pos, ",", int(value), ",", int(size), ", ...)");
+  SERIAL_ECHOLNPAIR("_data(", pos, ",", value, ",", size, ", ...)");
  if (total) {
    SERIAL_ECHOPGM(" f_");
    serialprintPGM(rw_str);
-    SERIAL_ECHOPAIR("()=", int(s), "\n size=", int(size), "\n bytes_");
+    SERIAL_ECHOPAIR("()=", s, "\n size=", size, "\n bytes_");
    serialprintPGM(write ? PSTR("written=") : PSTR("read="));
    SERIAL_ECHOLN(total);
  }
  else
-    SERIAL_ECHOLNPAIR(" f_lseek()=", int(s));
+    SERIAL_ECHOLNPAIR(" f_lseek()=", s);
 }

 // File function return codes for type FRESULT. This goes away soon, but
@@ -26,3 +26,10 @@
 #elif EITHER(I2C_EEPROM, SPI_EEPROM)
  #define USE_SHARED_EEPROM 1
 #endif
+
+// LPC1768 boards seem to lose steps when saving to EEPROM during print (issue #20785)
+// TODO: Which other boards are incompatible?
+#if defined(MCU_LPC1768) && PRINTCOUNTER_SAVE_INTERVAL > 0
+  #warning "To prevent step loss, motion will pause for PRINTCOUNTER auto-save."
+  #define PRINTCOUNTER_SYNC 1
+#endif
@@ -31,7 +31,7 @@
 /**
 * Detect an old pins file by checking for old ADC pins values.
 */
-#define _OLD_TEMP_PIN(P) PIN_EXISTS(P) && _CAT(P,_PIN) <= 7 && _CAT(P,_PIN) != 2 && _CAT(P,_PIN) != 3
+#define _OLD_TEMP_PIN(P) PIN_EXISTS(P) && _CAT(P,_PIN) <= 7 && !WITHIN(_CAT(P,_PIN), TERN(LPC1768_IS_SKRV1_3, 0, 2), 3)  // Include P0_00 and P0_01 for SKR V1.3 board
 #if _OLD_TEMP_PIN(TEMP_BED)
  #error "TEMP_BED_PIN must be defined using the Pn_nn or Pn_nn_An format. (See the included pins files)."
 #elif _OLD_TEMP_PIN(TEMP_0)
@@ -92,7 +92,7 @@ static_assert(DISABLED(BAUD_RATE_GCODE), "BAUD_RATE_GCODE is not yet supported o
 #define ANY_TX(N,V...) DO(IS_TX##N,||,V)
 #define ANY_RX(N,V...) DO(IS_RX##N,||,V)

-#if USING_SERIAL_0
+#if ANY_SERIAL_IS(0)
  #define IS_TX0(P) (P == P0_02)
  #define IS_RX0(P) (P == P0_03)
  #if IS_TX0(TMC_SW_MISO) || IS_RX0(TMC_SW_MOSI)
@@ -106,7 +106,7 @@ static_assert(DISABLED(BAUD_RATE_GCODE), "BAUD_RATE_GCODE is not yet supported o
  #undef IS_RX0
 #endif

-#if USING_SERIAL_1
+#if ANY_SERIAL_IS(1)
  #define IS_TX1(P) (P == P0_15)
  #define IS_RX1(P) (P == P0_16)
  #define _IS_TX1_1 IS_TX1
@@ -127,7 +127,7 @@ static_assert(DISABLED(BAUD_RATE_GCODE), "BAUD_RATE_GCODE is not yet supported o
  #undef _IS_RX1_1
 #endif

-#if USING_SERIAL_2
+#if ANY_SERIAL_IS(2)
  #define IS_TX2(P) (P == P0_10)
  #define IS_RX2(P) (P == P0_11)
  #define _IS_TX2_1 IS_TX2
@@ -161,7 +161,7 @@ static_assert(DISABLED(BAUD_RATE_GCODE), "BAUD_RATE_GCODE is not yet supported o
  #undef _IS_RX2_1
 #endif

-#if USING_SERIAL_3
+#if ANY_SERIAL_IS(3)
  #define PIN_IS_TX3(P) (PIN_EXISTS(P) && P##_PIN == P0_00)
  #define PIN_IS_RX3(P) (P##_PIN == P0_01)
  #if PIN_IS_TX3(X_MIN) || PIN_IS_RX3(X_MAX)
@@ -119,9 +119,9 @@ void HAL_init() {
  #endif

  USB_Init();                               // USB Initialization
-  USB_Connect(FALSE);                       // USB clear connection
+  USB_Connect(false);                       // USB clear connection
  delay(1000);                              // Give OS time to notice
-  USB_Connect(TRUE);
+  USB_Connect(true);

  #if HAS_SD_HOST_DRIVE
    MSC_SD_Init(0);                         // Enable USB SD card access
@@ -24,6 +24,11 @@
 #include <Adafruit_ZeroDMA.h>
 #include <wiring_private.h>

+#ifdef ADAFRUIT_GRAND_CENTRAL_M4
+  DefaultSerial MSerial(false, Serial);
+  DefaultSerial1 MSerial1(false, Serial1);
+#endif
+
 // ------------------------
 // Local defines
 // ------------------------
@@ -32,15 +32,19 @@
  #include "MarlinSerial_AGCM4.h"

  // Serial ports
+  typedef ForwardSerial0Type< decltype(Serial) > DefaultSerial;
+  extern DefaultSerial MSerial;
+  typedef ForwardSerial0Type< decltype(Serial1) > DefaultSerial1;
+  extern DefaultSerial1 MSerial1;

  // MYSERIAL0 required before MarlinSerial includes!

-  #define __MSERIAL(X) Serial##X
+  #define __MSERIAL(X) MSerial##X
  #define _MSERIAL(X) __MSERIAL(X)
  #define MSERIAL(X) _MSERIAL(INCREMENT(X))

  #if SERIAL_PORT == -1
-    #define MYSERIAL0 Serial
+    #define MYSERIAL0 MSerial
  #elif WITHIN(SERIAL_PORT, 0, 3)
    #define MYSERIAL0 MSERIAL(SERIAL_PORT)
  #else
@@ -49,7 +53,7 @@

  #ifdef SERIAL_PORT_2
    #if SERIAL_PORT_2 == -1
-      #define MYSERIAL1 Serial
+      #define MYSERIAL1 MSerial
    #elif WITHIN(SERIAL_PORT_2, 0, 3)
      #define MYSERIAL1 MSERIAL(SERIAL_PORT_2)
    #else
@@ -59,7 +63,7 @@

  #ifdef MMU2_SERIAL_PORT
    #if MMU2_SERIAL_PORT == -1
-      #define MMU2_SERIAL Serial
+      #define MMU2_SERIAL MSerial
    #elif WITHIN(MMU2_SERIAL_PORT, 0, 3)
      #define MMU2_SERIAL MSERIAL(MMU2_SERIAL_PORT)
    #else
@@ -69,7 +73,7 @@

  #ifdef LCD_SERIAL_PORT
    #if LCD_SERIAL_PORT == -1
-      #define LCD_SERIAL Serial
+      #define LCD_SERIAL MSerial
    #elif WITHIN(LCD_SERIAL_PORT, 0, 3)
      #define LCD_SERIAL MSERIAL(LCD_SERIAL_PORT)
    #else
@@ -27,24 +27,24 @@

 #include "../../inc/MarlinConfig.h"

-#if USING_SERIAL_1
-  Uart Serial2(&sercom4, PIN_SERIAL2_RX, PIN_SERIAL2_TX, PAD_SERIAL2_RX, PAD_SERIAL2_TX);
+#if ANY_SERIAL_IS(1)
+  UartT Serial2(false, &sercom4, PIN_SERIAL2_RX, PIN_SERIAL2_TX, PAD_SERIAL2_RX, PAD_SERIAL2_TX);
  void SERCOM4_0_Handler() { Serial2.IrqHandler(); }
  void SERCOM4_1_Handler() { Serial2.IrqHandler(); }
  void SERCOM4_2_Handler() { Serial2.IrqHandler(); }
  void SERCOM4_3_Handler() { Serial2.IrqHandler(); }
 #endif

-#if USING_SERIAL_2
-  Uart Serial3(&sercom1, PIN_SERIAL3_RX, PIN_SERIAL3_TX, PAD_SERIAL3_RX, PAD_SERIAL3_TX);
+#if ANY_SERIAL_IS(2)
+  UartT Serial3(false, &sercom1, PIN_SERIAL3_RX, PIN_SERIAL3_TX, PAD_SERIAL3_RX, PAD_SERIAL3_TX);
  void SERCOM1_0_Handler() { Serial3.IrqHandler(); }
  void SERCOM1_1_Handler() { Serial3.IrqHandler(); }
  void SERCOM1_2_Handler() { Serial3.IrqHandler(); }
  void SERCOM1_3_Handler() { Serial3.IrqHandler(); }
 #endif

-#if USING_SERIAL_3
-  Uart Serial4(&sercom5, PIN_SERIAL4_RX, PIN_SERIAL4_TX, PAD_SERIAL4_RX, PAD_SERIAL4_TX);
+#if ANY_SERIAL_IS(3)
+  UartT Serial4(false, &sercom5, PIN_SERIAL4_RX, PIN_SERIAL4_TX, PAD_SERIAL4_RX, PAD_SERIAL4_TX);
  void SERCOM5_0_Handler() { Serial4.IrqHandler(); }
  void SERCOM5_1_Handler() { Serial4.IrqHandler(); }
  void SERCOM5_2_Handler() { Serial4.IrqHandler(); }
@@ -20,6 +20,10 @@
 */
 #pragma once

-extern Uart Serial2;
-extern Uart Serial3;
-extern Uart Serial4;
+#include "../../core/serial_hook.h"
+
+typedef Serial0Type<Uart> UartT;
+
+extern UartT Serial2;
+extern UartT Serial3;
+extern UartT Serial4;
@@ -28,6 +28,10 @@
 #include "../../inc/MarlinConfig.h"
 #include "../shared/Delay.h"

+#ifdef USBCON
+  DefaultSerial MSerial(false, SerialUSB);
+#endif
+
 #if ENABLED(SRAM_EEPROM_EMULATION)
  #if STM32F7xx
    #include <stm32f7xx_ll_pwr.h>
@@ -38,6 +42,11 @@
  #endif
 #endif

+#if HAS_SD_HOST_DRIVE
+  #include "msc_sd.h"
+  #include "usbd_cdc_if.h"
+#endif
+
 // ------------------------
 // Public Variables
 // ------------------------
@@ -48,17 +57,6 @@ uint16_t HAL_adc_result;
 // Public functions
 // ------------------------

-// Needed for DELAY_NS() / DELAY_US() on CORTEX-M7
-#if (defined(__arm__) || defined(__thumb__)) && __CORTEX_M == 7
-  // HAL pre-initialization task
-  // Force the preinit function to run between the premain() and main() function
-  // of the STM32 arduino core
-  __attribute__((constructor (102)))
-  void HAL_preinit() {
-    enableCycleCounter();
-  }
-#endif
-
 // HAL initialization task
 void HAL_init() {
  FastIO_init();
@@ -84,6 +82,19 @@ void HAL_init() {
  #if ENABLED(EMERGENCY_PARSER) && USBD_USE_CDC
    USB_Hook_init();
  #endif
+
+  #if HAS_SD_HOST_DRIVE
+    MSC_SD_init();                         // Enable USB SD card access
+  #endif
+}
+
+// HAL idle task
+void HAL_idletask() {
+  #if HAS_SHARED_MEDIA
+    // Stm32duino currently doesn't have a "loop/idle" method
+    CDC_resume_receive();
+    CDC_continue_transmit();
+  #endif
 }

 void HAL_clear_reset_source() { __HAL_RCC_CLEAR_RESET_FLAGS(); }
@@ -39,6 +39,9 @@

 #ifdef USBCON
  #include <USBSerial.h>
+  #include "../../core/serial_hook.h"
+  typedef ForwardSerial0Type< decltype(SerialUSB) > DefaultSerial;
+  extern DefaultSerial MSerial;
 #endif

 // ------------------------
@@ -48,7 +51,7 @@
 #define MSERIAL(X) _MSERIAL(X)

 #if SERIAL_PORT == -1
-  #define MYSERIAL0 SerialUSB
+  #define MYSERIAL0 MSerial
 #elif WITHIN(SERIAL_PORT, 1, 6)
  #define MYSERIAL0 MSERIAL(SERIAL_PORT)
 #else
@@ -57,7 +60,7 @@

 #ifdef SERIAL_PORT_2
  #if SERIAL_PORT_2 == -1
-    #define MYSERIAL1 SerialUSB
+    #define MYSERIAL1 MSerial
  #elif WITHIN(SERIAL_PORT_2, 1, 6)
    #define MYSERIAL1 MSERIAL(SERIAL_PORT_2)
  #else
@@ -67,7 +70,7 @@

 #ifdef MMU2_SERIAL_PORT
  #if MMU2_SERIAL_PORT == -1
-    #define MMU2_SERIAL SerialUSB
+    #define MMU2_SERIAL MSerial
  #elif WITHIN(MMU2_SERIAL_PORT, 1, 6)
    #define MMU2_SERIAL MSERIAL(MMU2_SERIAL_PORT)
  #else
@@ -77,13 +80,13 @@

 #ifdef LCD_SERIAL_PORT
  #if LCD_SERIAL_PORT == -1
-    #define LCD_SERIAL SerialUSB
+    #define LCD_SERIAL MSerial
  #elif WITHIN(LCD_SERIAL_PORT, 1, 6)
    #define LCD_SERIAL MSERIAL(LCD_SERIAL_PORT)
  #else
    #error "LCD_SERIAL_PORT must be -1 or from 1 to 6. Please update your configuration."
  #endif
-  #if HAS_DGUS_LCD
+  #if HAS_DGUS_LCD || ENABLED(DGUS_LCD_UI_CREALITY_TOUCH)
    #define SERIAL_GET_TX_BUFFER_FREE() LCD_SERIAL.availableForWrite()
  #endif
 #endif
@@ -106,14 +109,6 @@
 // On AVR this is in math.h?
 #define square(x) ((x)*(x))

-#ifndef strncpy_P
-  #define strncpy_P(dest, src, num) strncpy((dest), (src), (num))
-#endif
-
-// Fix bug in pgm_read_ptr
-#undef pgm_read_ptr
-#define pgm_read_ptr(addr) (*(addr))
-
 // ------------------------
 // Types
 // ------------------------
@@ -140,6 +135,8 @@ extern uint16_t HAL_adc_result;

 // Enable hooks into  setup for HAL
 void HAL_init();
+#define HAL_IDLETASK 1
+void HAL_idletask();

 // Clear reset reason
 void HAL_clear_reset_source();
@@ -197,6 +194,7 @@ void flashFirmware(const int16_t);
 typedef void (*systickCallback_t)(void);
 void systick_attach_callback(systickCallback_t cb);
 void HAL_SYSTICK_Callback();
+
 extern volatile uint32_t systick_uptime_millis;

 #define HAL_CAN_SET_PWM_FREQ   // This HAL supports PWM Frequency adjustment
@@ -51,18 +51,28 @@ static SPISettings spiConfig;
    OUT_WRITE(SD_MOSI_PIN, HIGH);
  }

-  static uint16_t delay_STM32_soft_spi;
+  // Use function with compile-time value so we can actually reach the desired frequency
+  // Need to adjust this a little bit: on a 72MHz clock, we have 14ns/clock
+  // and we'll use ~3 cycles to jump to the method and going back, so it'll take ~40ns from the given clock here
+  #define CALLING_COST_NS  (3U * 1000000000U) / (F_CPU)
+  void (*delaySPIFunc)();
+  void delaySPI_125()  { DELAY_NS(125 - CALLING_COST_NS); }
+  void delaySPI_250()  { DELAY_NS(250 - CALLING_COST_NS); }
+  void delaySPI_500()  { DELAY_NS(500 - CALLING_COST_NS); }
+  void delaySPI_1000() { DELAY_NS(1000 - CALLING_COST_NS); }
+  void delaySPI_2000() { DELAY_NS(2000 - CALLING_COST_NS); }
+  void delaySPI_4000() { DELAY_NS(4000 - CALLING_COST_NS); }

  void spiInit(uint8_t spiRate) {
    // Use datarates Marlin uses
    switch (spiRate) {
-      case SPI_FULL_SPEED:   delay_STM32_soft_spi =  125; break;  // desired: 8,000,000  actual: ~1.1M
-      case SPI_HALF_SPEED:   delay_STM32_soft_spi =  125; break;  // desired: 4,000,000  actual: ~1.1M
-      case SPI_QUARTER_SPEED:delay_STM32_soft_spi =  250; break;  // desired: 2,000,000  actual: ~890K
-      case SPI_EIGHTH_SPEED: delay_STM32_soft_spi =  500; break;  // desired: 1,000,000  actual: ~590K
-      case SPI_SPEED_5:      delay_STM32_soft_spi = 1000; break;  // desired:   500,000  actual: ~360K
-      case SPI_SPEED_6:      delay_STM32_soft_spi = 2000; break;  // desired:   250,000  actual: ~210K
-      default:               delay_STM32_soft_spi = 4000; break;  // desired:   125,000  actual: ~123K
+      case SPI_FULL_SPEED:   delaySPIFunc =  &delaySPI_125; break;  // desired: 8,000,000  actual: ~1.1M
+      case SPI_HALF_SPEED:   delaySPIFunc =  &delaySPI_125; break;  // desired: 4,000,000  actual: ~1.1M
+      case SPI_QUARTER_SPEED:delaySPIFunc =  &delaySPI_250; break;  // desired: 2,000,000  actual: ~890K
+      case SPI_EIGHTH_SPEED: delaySPIFunc =  &delaySPI_500; break;  // desired: 1,000,000  actual: ~590K
+      case SPI_SPEED_5:      delaySPIFunc = &delaySPI_1000; break;  // desired:   500,000  actual: ~360K
+      case SPI_SPEED_6:      delaySPIFunc = &delaySPI_2000; break;  // desired:   250,000  actual: ~210K
+      default:               delaySPIFunc = &delaySPI_4000; break;  // desired:   125,000  actual: ~123K
    }
    SPI.begin();
  }
@@ -75,9 +85,9 @@ static SPISettings spiConfig;
      WRITE(SD_SCK_PIN, LOW);
      WRITE(SD_MOSI_PIN, b & 0x80);

-      DELAY_NS(delay_STM32_soft_spi);
+      delaySPIFunc();
      WRITE(SD_SCK_PIN, HIGH);
-      DELAY_NS(delay_STM32_soft_spi);
+      delaySPIFunc();

      b <<= 1;        // little setup time
      b |= (READ(SD_MISO_PIN) != 0);
@@ -35,7 +35,7 @@

 #define DECLARE_SERIAL_PORT(ser_num) \
  void _rx_complete_irq_ ## ser_num (serial_t * obj); \
-  MarlinSerial MSerial ## ser_num (USART ## ser_num, &_rx_complete_irq_ ## ser_num); \
+  MSerialT MSerial ## ser_num (true, USART ## ser_num, &_rx_complete_irq_ ## ser_num); \
  void _rx_complete_irq_ ## ser_num (serial_t * obj) { MSerial ## ser_num ._rx_complete_irq(obj); }

 #define DECLARE_SERIAL_PORT_EXP(ser_num) DECLARE_SERIAL_PORT(ser_num)
@@ -81,7 +81,7 @@ void MarlinSerial::_rx_complete_irq(serial_t *obj) {
    }

    #if ENABLED(EMERGENCY_PARSER)
-      emergency_parser.update(emergency_state, c);
+      emergency_parser.update(static_cast<MSerialT*>(this)->emergency_state, c);
    #endif
  }
 }
@@ -24,21 +24,15 @@
  #include "../../feature/e_parser.h"
 #endif

+#include "../../core/serial_hook.h"
+
 typedef void (*usart_rx_callback_t)(serial_t * obj);

-class MarlinSerial : public HardwareSerial {
-public:
+struct MarlinSerial : public HardwareSerial {
  MarlinSerial(void* peripheral, usart_rx_callback_t rx_callback) :
      HardwareSerial(peripheral), _rx_callback(rx_callback)
-      #if ENABLED(EMERGENCY_PARSER)
-        , emergency_state(EmergencyParser::State::EP_RESET)
-      #endif
  { }

-  #if ENABLED(EMERGENCY_PARSER)
-    static inline bool emergency_parser_enabled() { return true; }
-  #endif
-
  void begin(unsigned long baud, uint8_t config);
  inline void begin(unsigned long baud) { begin(baud, SERIAL_8N1); }

@@ -46,19 +40,17 @@ public:

 protected:
  usart_rx_callback_t _rx_callback;
-  #if ENABLED(EMERGENCY_PARSER)
-    EmergencyParser::State emergency_state;
-  #endif
 };

-extern MarlinSerial MSerial1;
-extern MarlinSerial MSerial2;
-extern MarlinSerial MSerial3;
-extern MarlinSerial MSerial4;
-extern MarlinSerial MSerial5;
-extern MarlinSerial MSerial6;
-extern MarlinSerial MSerial7;
-extern MarlinSerial MSerial8;
-extern MarlinSerial MSerial9;
-extern MarlinSerial MSerial10;
-extern MarlinSerial MSerialLP1;
+typedef Serial0Type<MarlinSerial> MSerialT;
+extern MSerialT MSerial1;
+extern MSerialT MSerial2;
+extern MSerialT MSerial3;
+extern MSerialT MSerial4;
+extern MSerialT MSerial5;
+extern MSerialT MSerial6;
+extern MSerialT MSerial7;
+extern MSerialT MSerial8;
+extern MSerialT MSerial9;
+extern MSerialT MSerial10;
+extern MSerialT MSerialLP1;
@@ -163,7 +163,6 @@
    GPIO_InitStruct.Pin = GPIO_PIN_2;
    HAL_GPIO_Init(GPIOD, &GPIO_InitStruct);

-
    #if DISABLED(STM32F1xx)
      // TODO: use __HAL_RCC_SDIO_RELEASE_RESET() and __HAL_RCC_SDIO_CLK_ENABLE();
      RCC->APB2RSTR &= ~RCC_APB2RSTR_SDIORST_Msk;  // take SDIO out of reset
@@ -61,7 +61,9 @@
    #define FLASH_UNIT_SIZE       0x20000 // 128kB
  #endif

-  #define FLASH_ADDRESS_START     (FLASH_END - ((FLASH_SECTOR_TOTAL - (FLASH_SECTOR)) * (FLASH_UNIT_SIZE)) + 1)
+  #ifndef FLASH_ADDRESS_START
+    #define FLASH_ADDRESS_START   (FLASH_END - ((FLASH_SECTOR_TOTAL - (FLASH_SECTOR)) * (FLASH_UNIT_SIZE)) + 1)
+  #endif
  #define FLASH_ADDRESS_END       (FLASH_ADDRESS_START + FLASH_UNIT_SIZE  - 1)

  #define EEPROM_SLOTS            ((FLASH_UNIT_SIZE) / (MARLIN_EEPROM_SIZE))
@@ -21,6 +21,6 @@
 */
 #pragma once

-#if defined(USBD_USE_CDC_COMPOSITE) && DISABLED(NO_SD_HOST_DRIVE)
+#if defined(USBD_USE_CDC_MSC) && DISABLED(NO_SD_HOST_DRIVE)
  #define HAS_SD_HOST_DRIVE 1
 #endif
@@ -0,0 +1,112 @@
+/**
+ * Marlin 3D Printer Firmware
+ *
+ * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ * Copyright (c) 2019 BigTreeTech [https://github.com/bigtreetech]
+ *
+ * This program 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#include "../../inc/MarlinConfigPre.h"
+
+#if defined(ARDUINO_ARCH_STM32) && !defined(STM32GENERIC) && HAS_SD_HOST_DRIVE
+
+#include "msc_sd.h"
+#include "../shared/Marduino.h"
+#include "usbd_core.h"
+#include <USB.h>
+#include <USBMscHandler.h>
+
+#define BLOCK_SIZE 512
+#define PRODUCT_ID 0x29
+
+#include "../../sd/cardreader.h"
+
+class Sd2CardUSBMscHandler : public USBMscHandler {
+public:
+  bool GetCapacity(uint32_t *pBlockNum, uint16_t *pBlockSize) {
+    *pBlockNum = card.getSd2Card().cardSize();
+    *pBlockSize = BLOCK_SIZE;
+    return true;
+  }
+
+  bool Write(uint8_t *pBuf, uint32_t blkAddr, uint16_t blkLen) {
+    auto sd2card = card.getSd2Card();
+    // single block
+    if (blkLen == 1) {
+      watchdog_refresh();
+      sd2card.writeBlock(blkAddr, pBuf);
+      return true;
+    }
+
+    // multi block optmization
+    sd2card.writeStart(blkAddr, blkLen);
+    while (blkLen--) {
+      watchdog_refresh();
+      sd2card.writeData(pBuf);
+      pBuf += BLOCK_SIZE;
+    }
+    sd2card.writeStop();
+    return true;
+  }
+
+  bool Read(uint8_t *pBuf, uint32_t blkAddr, uint16_t blkLen) {
+    auto sd2card = card.getSd2Card();
+    // single block
+    if (blkLen == 1) {
+      watchdog_refresh();
+      sd2card.readBlock(blkAddr, pBuf);
+      return true;
+    }
+
+    // multi block optmization
+    sd2card.readStart(blkAddr);
+    while (blkLen--) {
+      watchdog_refresh();
+      sd2card.readData(pBuf);
+      pBuf += BLOCK_SIZE;
+    }
+    sd2card.readStop();
+    return true;
+  }
+
+  bool IsReady() {
+    return card.isMounted();
+  }
+};
+
+Sd2CardUSBMscHandler usbMscHandler;
+
+/* USB Mass storage Standard Inquiry Data */
+uint8_t  Marlin_STORAGE_Inquirydata[] = { /* 36 */
+  /* LUN 0 */
+  0x00,
+  0x80,
+  0x02,
+  0x02,
+  (STANDARD_INQUIRY_DATA_LEN - 5),
+  0x00,
+  0x00,
+  0x00,
+  'M', 'A', 'R', 'L', 'I', 'N', ' ', ' ', /* Manufacturer : 8 bytes */
+  'P', 'r', 'o', 'd', 'u', 'c', 't', ' ', /* Product      : 16 Bytes */
+  ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ',
+  '0', '.', '0', '1',                     /* Version      : 4 Bytes */
+};
+
+USBMscHandler *pSingleMscHandler = &usbMscHandler;
+
+void MSC_SD_init() {
+  USBDevice.end();
+  delay(200);
+  USBDevice.begin();
+  USBDevice.registerMscHandlers(1, &pSingleMscHandler, Marlin_STORAGE_Inquirydata);
+}
+
+#endif // __STM32F1__ && HAS_SD_HOST_DRIVE
@@ -0,0 +1,18 @@
+/**
+ * Marlin 3D Printer Firmware
+ *
+ * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ * Copyright (c) 2019 BigTreeTech [https://github.com/bigtreetech]
+ *
+ * This program 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+void MSC_SD_init();
@@ -84,6 +84,32 @@

 #if defined(SERIAL_USB) && !HAS_SD_HOST_DRIVE
  USBSerial SerialUSB;
+  DefaultSerial MSerial(true, SerialUSB);
+
+  #if ENABLED(EMERGENCY_PARSER)
+    #include "../libmaple/usb/stm32f1/usb_reg_map.h"
+    #include "libmaple/usb_cdcacm.h"
+    // The original callback is not called (no way to retrieve address).
+    // That callback detects a special STM32 reset sequence: this functionality is not essential
+    // as M997 achieves the same.
+    void my_rx_callback(unsigned int, void*) {
+      // max length of 16 is enough to contain all emergency commands
+      uint8 buf[16];
+
+      //rx is usbSerialPart.endpoints[2]
+      uint16 len = usb_get_ep_rx_count(USB_CDCACM_RX_ENDP);
+      uint32 total = usb_cdcacm_data_available();
+
+      if (len == 0 || total == 0 || !WITHIN(total, len, COUNT(buf)))
+        return;
+
+      // cannot get character by character due to bug in composite_cdcacm_peek_ex
+      len = usb_cdcacm_peek(buf, total);
+
+      for (uint32 i = 0; i < len; i++)
+        emergency_parser.update(MSerial.emergency_state, buf[i + total - len]);
+    }
+  #endif
 #endif

 uint16_t HAL_adc_result;
@@ -253,6 +279,8 @@ void HAL_init() {
  #endif
  #if HAS_SD_HOST_DRIVE
    MSC_SD_init();
+  #elif BOTH(SERIAL_USB, EMERGENCY_PARSER)
+    usb_cdcacm_set_hooks(USB_CDCACM_HOOK_RX, my_rx_callback);
  #endif
  #if PIN_EXISTS(USB_CONNECT)
    OUT_WRITE(USB_CONNECT_PIN, !USB_CONNECT_INVERTING);  // USB clear connection
@@ -61,8 +61,11 @@
 #endif

 #ifdef SERIAL_USB
+  typedef ForwardSerial0Type< USBSerial > DefaultSerial;
+  extern DefaultSerial MSerial;
+
  #if !HAS_SD_HOST_DRIVE
-    #define UsbSerial Serial
+    #define UsbSerial MSerial
  #else
    #define UsbSerial MarlinCompositeSerial
  #endif
@@ -121,7 +124,7 @@
  #else
    #error "LCD_SERIAL_PORT must be -1 or from 1 to 3. Please update your configuration."
  #endif
-  #if HAS_DGUS_LCD
+  #if HAS_DGUS_LCD || ENABLED(DGUS_LCD_UI_CREALITY_TOUCH)
    #define SERIAL_GET_TX_BUFFER_FREE() LCD_SERIAL.availableForWrite()
  #endif
 #endif
@@ -152,14 +155,6 @@ void HAL_idletask();
 // On AVR this is in math.h?
 #define square(x) ((x)*(x))

-#ifndef strncpy_P
-  #define strncpy_P(dest, src, num) strncpy((dest), (src), (num))
-#endif
-
-// Fix bug in pgm_read_ptr
-#undef pgm_read_ptr
-#define pgm_read_ptr(addr) (*(addr))
-
 #define RST_POWER_ON   1
 #define RST_EXTERNAL   2
 #define RST_BROWN_OUT  4
@@ -28,7 +28,7 @@

 // Copied from ~/.platformio/packages/framework-arduinoststm32-maple/STM32F1/system/libmaple/usart_private.h
 // Changed to handle Emergency Parser
-static inline __always_inline void my_usart_irq(ring_buffer *rb, ring_buffer *wb, usart_reg_map *regs, MarlinSerial &serial) {
+static inline __always_inline void my_usart_irq(ring_buffer *rb, ring_buffer *wb, usart_reg_map *regs, MSerialT &serial) {
 /* Handle RXNEIE and TXEIE interrupts.
  * RXNE signifies availability of a byte in DR.
  *
@@ -90,20 +90,20 @@ constexpr bool serial_handles_emergency(int port) {
  ;
 }

-#define DEFINE_HWSERIAL_MARLIN(name, n)   \
-  MarlinSerial name(USART##n,             \
-            BOARD_USART##n##_TX_PIN,      \
-            BOARD_USART##n##_RX_PIN,      \
-            serial_handles_emergency(n)); \
-  extern "C" void __irq_usart##n(void) {  \
+#define DEFINE_HWSERIAL_MARLIN(name, n)     \
+  MSerialT name(serial_handles_emergency(n),\
+            USART##n,                       \
+            BOARD_USART##n##_TX_PIN,        \
+            BOARD_USART##n##_RX_PIN);       \
+  extern "C" void __irq_usart##n(void) {    \
    my_usart_irq(USART##n->rb, USART##n->wb, USART##n##_BASE, MSerial##n); \
  }

 #define DEFINE_HWSERIAL_UART_MARLIN(name, n) \
-  MarlinSerial name(UART##n,                 \
+  MSerialT name(serial_handles_emergency(n), \
+          UART##n,                           \
          BOARD_USART##n##_TX_PIN,           \
-          BOARD_USART##n##_RX_PIN,           \
-          serial_handles_emergency(n));      \
+          BOARD_USART##n##_RX_PIN);          \
  extern "C" void __irq_usart##n(void) {     \
    my_usart_irq(UART##n->rb, UART##n->wb, UART##n##_BASE, MSerial##n); \
  }
@@ -26,28 +26,13 @@
 #include <WString.h>

 #include "../../inc/MarlinConfigPre.h"
-#if ENABLED(EMERGENCY_PARSER)
-  #include "../../feature/e_parser.h"
-#endif
+#include "../../core/serial_hook.h"

 // Increase priority of serial interrupts, to reduce overflow errors
 #define UART_IRQ_PRIO 1

-class MarlinSerial : public HardwareSerial {
-public:
-  #if ENABLED(EMERGENCY_PARSER)
-    const bool ep_enabled;
-    EmergencyParser::State emergency_state;
-    inline bool emergency_parser_enabled() { return ep_enabled; }
-  #endif
-
-  MarlinSerial(struct usart_dev *usart_device, uint8 tx_pin, uint8 rx_pin, bool TERN_(EMERGENCY_PARSER, ep_capable)) :
-    HardwareSerial(usart_device, tx_pin, rx_pin)
-    #if ENABLED(EMERGENCY_PARSER)
-      , ep_enabled(ep_capable)
-      , emergency_state(EmergencyParser::State::EP_RESET)
-    #endif
-    { }
+struct MarlinSerial : public HardwareSerial {
+  MarlinSerial(struct usart_dev *usart_device, uint8 tx_pin, uint8 rx_pin) : HardwareSerial(usart_device, tx_pin, rx_pin) { }

  #ifdef UART_IRQ_PRIO
    // Shadow the parent methods to set IRQ priority after begin()
@@ -62,10 +47,12 @@ public:
  #endif
 };

-extern MarlinSerial MSerial1;
-extern MarlinSerial MSerial2;
-extern MarlinSerial MSerial3;
+typedef Serial0Type<MarlinSerial> MSerialT;
+
+extern MSerialT MSerial1;
+extern MSerialT MSerial2;
+extern MSerialT MSerial3;
 #if EITHER(STM32_HIGH_DENSITY, STM32_XL_DENSITY)
-  extern MarlinSerial MSerial4;
-  extern MarlinSerial MSerial5;
+  extern MSerialT MSerial4;
+  extern MSerialT MSerial5;
 #endif
@@ -19,11 +19,12 @@

 #include "msc_sd.h"
 #include "SPI.h"
+#include "usb_reg_map.h"

 #define PRODUCT_ID 0x29

 USBMassStorage MarlinMSC;
-MarlinUSBCompositeSerial MarlinCompositeSerial;
+Serial0Type<USBCompositeSerial> MarlinCompositeSerial(true);

 #include "../../inc/MarlinConfig.h"

@@ -41,14 +42,27 @@ MarlinUSBCompositeSerial MarlinCompositeSerial;
 #endif

 #if ENABLED(EMERGENCY_PARSER)
-  void (*real_rx_callback)(void);

-  void my_rx_callback(void) {
-    real_rx_callback();
-    int len = MarlinCompositeSerial.available();
-    while (len-- > 0) // >0 because available() may return a negative value
-      emergency_parser.update(MarlinCompositeSerial.emergency_state, MarlinCompositeSerial.peek());
-  }
+// The original callback is not called (no way to retrieve address).
+// That callback detects a special STM32 reset sequence: this functionality is not essential
+// as M997 achieves the same.
+void my_rx_callback(unsigned int, void*) {
+  // max length of 16 is enough to contain all emergency commands
+  uint8 buf[16];
+
+  //rx is usbSerialPart.endpoints[2]
+  uint16 len = usb_get_ep_rx_count(usbSerialPart.endpoints[2].address);
+  uint32 total = composite_cdcacm_data_available();
+
+  if (len == 0 || total == 0 || !WITHIN(total, len, COUNT(buf)))
+    return;
+
+  // cannot get character by character due to bug in composite_cdcacm_peek_ex
+  len = composite_cdcacm_peek(buf, total);
+
+  for (uint32 i = 0; i < len; i++)
+    emergency_parser.update(MarlinCompositeSerial.emergency_state, buf[i+total-len]);
+}
 #endif

 void MSC_SD_init() {
@@ -73,9 +87,7 @@ void MSC_SD_init() {
  MarlinCompositeSerial.registerComponent();
  USBComposite.begin();
  #if ENABLED(EMERGENCY_PARSER)
-    //rx is usbSerialPart.endpoints[2]
-    real_rx_callback = usbSerialPart.endpoints[2].callback;
-    usbSerialPart.endpoints[2].callback = my_rx_callback;
+  	composite_cdcacm_set_hooks(USBHID_CDCACM_HOOK_RX, my_rx_callback);
  #endif
 }

@@ -18,25 +18,9 @@
 #include <USBComposite.h>

 #include "../../inc/MarlinConfigPre.h"
-#if ENABLED(EMERGENCY_PARSER)
-  #include "../../feature/e_parser.h"
-#endif
-
-class MarlinUSBCompositeSerial : public USBCompositeSerial {
-public:
-  MarlinUSBCompositeSerial() : USBCompositeSerial()
-    #if ENABLED(EMERGENCY_PARSER)
-      , emergency_state(EmergencyParser::State::EP_RESET)
-    #endif
-    { }
-
-  #if ENABLED(EMERGENCY_PARSER)
-    EmergencyParser::State emergency_state;
-    inline bool emergency_parser_enabled() { return true; }
-  #endif
-};
+#include "../../core/serial_hook.h"

 extern USBMassStorage MarlinMSC;
-extern MarlinUSBCompositeSerial MarlinCompositeSerial;
+extern Serial0Type<USBCompositeSerial> MarlinCompositeSerial;

 void MSC_SD_init();
@@ -31,6 +31,9 @@

 #include <Wire.h>

+DefaultSerial MSerial(false);
+USBSerialType USBSerial(false, SerialUSB);
+
 uint16_t HAL_adc_result;

 static const uint8_t pin2sc1a[] = {
@@ -50,12 +50,18 @@
  #define IS_TEENSY32 1
 #endif

-#define _MSERIAL(X) Serial##X
+#include "../../core/serial_hook.h"
+typedef Serial0Type<decltype(Serial)> DefaultSerial;
+extern DefaultSerial MSerial;
+typedef ForwardSerial0Type<decltype(SerialUSB)> USBSerialType;
+extern USBSerialType USBSerial;
+
+#define _MSERIAL(X) MSerial##X
 #define MSERIAL(X) _MSERIAL(X)
-#define Serial0 Serial
+#define MSerial0 MSerial

 #if SERIAL_PORT == -1
-  #define MYSERIAL0 SerialUSB
+  #define MYSERIAL0 USBSerial
 #elif WITHIN(SERIAL_PORT, 0, 3)
  #define MYSERIAL0 MSERIAL(SERIAL_PORT)
 #endif
@@ -74,17 +80,6 @@ typedef int8_t pin_t;
 #define ENABLE_ISRS()  __enable_irq()
 #define DISABLE_ISRS() __disable_irq()

-#ifndef strncpy_P
-  #define strncpy_P(dest, src, num) strncpy((dest), (src), (num))
-#endif
-
-// Fix bug in pgm_read_ptr
-#undef pgm_read_ptr
-#define pgm_read_ptr(addr) (*((void**)(addr)))
-// Add type-checking to pgm_read_word
-#undef pgm_read_word
-#define pgm_read_word(addr) (*((uint16_t*)(addr)))
-
 inline void HAL_init() {}

 // Clear the reset reason
@@ -31,6 +31,9 @@

 #include <Wire.h>

+DefaultSerial MSerial(false);
+USBSerialType USBSerial(false, SerialUSB);
+
 uint16_t HAL_adc_result, HAL_adc_select;

 static const uint8_t pin2sc1a[] = {
@@ -53,12 +53,18 @@
  #define IS_TEENSY35 1
 #endif

-#define _MSERIAL(X) Serial##X
+#include "../../core/serial_hook.h"
+typedef Serial0Type<decltype(Serial)> DefaultSerial;
+extern DefaultSerial MSerial;
+typedef ForwardSerial0Type<decltype(SerialUSB)> USBSerialType;
+extern USBSerialType USBSerial;
+
+#define _MSERIAL(X) MSerial##X
 #define MSERIAL(X) _MSERIAL(X)
-#define Serial0 Serial
+#define MSerial0 MSerial

 #if SERIAL_PORT == -1
-  #define MYSERIAL0 SerialUSB
+  #define MYSERIAL0 USBSerial
 #elif WITHIN(SERIAL_PORT, 0, 3)
  #define MYSERIAL0 MSERIAL(SERIAL_PORT)
 #endif
@@ -80,17 +86,6 @@ typedef int8_t pin_t;
 #undef sq
 #define sq(x) ((x)*(x))

-#ifndef strncpy_P
-  #define strncpy_P(dest, src, num) strncpy((dest), (src), (num))
-#endif
-
-// Fix bug in pgm_read_ptr
-#undef pgm_read_ptr
-#define pgm_read_ptr(addr) (*((void**)(addr)))
-// Add type-checking to pgm_read_word
-#undef pgm_read_word
-#define pgm_read_word(addr) (*((uint16_t*)(addr)))
-
 inline void HAL_init() {}

 // Clear reset reason
@@ -32,6 +32,9 @@

 #include <Wire.h>

+DefaultSerial MSerial(false);
+USBSerialType USBSerial(false, SerialUSB);
+
 uint16_t HAL_adc_result, HAL_adc_select;

 static const uint8_t pin2sc1a[] = {
@@ -37,6 +37,10 @@
 #include <stdint.h>
 #include <util/atomic.h>

+#if HAS_ETHERNET
+  #include "../../feature/ethernet.h"
+#endif
+
 //#define ST7920_DELAY_1 DELAY_NS(600)
 //#define ST7920_DELAY_2 DELAY_NS(750)
 //#define ST7920_DELAY_3 DELAY_NS(750)
@@ -51,9 +55,15 @@
  #define IS_TEENSY41 1
 #endif

-#define _MSERIAL(X) Serial##X
+#include "../../core/serial_hook.h"
+typedef Serial0Type<decltype(Serial)> DefaultSerial;
+extern DefaultSerial MSerial;
+typedef ForwardSerial0Type<decltype(SerialUSB)> USBSerialType;
+extern USBSerialType USBSerial;
+
+#define _MSERIAL(X) MSerial##X
 #define MSERIAL(X) _MSERIAL(X)
-#define Serial0 Serial
+#define MSerial0 MSerial

 #if SERIAL_PORT == -1
  #define MYSERIAL0 SerialUSB
@@ -92,21 +102,10 @@ typedef int8_t pin_t;
 #undef sq
 #define sq(x) ((x)*(x))

-#ifndef strncpy_P
-  #define strncpy_P(dest, src, num) strncpy((dest), (src), (num))
-#endif
-
 // Don't place string constants in PROGMEM
 #undef PSTR
 #define PSTR(str) ({static const char *data = (str); &data[0];})

-// Fix bug in pgm_read_ptr
-#undef pgm_read_ptr
-#define pgm_read_ptr(addr) (*((void**)(addr)))
-// Add type-checking to pgm_read_word
-#undef pgm_read_word
-#define pgm_read_word(addr) (*((uint16_t*)(addr)))
-
 // Enable hooks into idle and setup for HAL
 #define HAL_IDLETASK 1
 FORCE_INLINE void HAL_idletask() {}
@@ -0,0 +1,178 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program 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.
+ *
+ * This program 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 this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#include "Delay.h"
+
+#include "../../inc/MarlinConfig.h"
+
+#if defined(__arm__) || defined(__thumb__)
+
+  static uint32_t ASM_CYCLES_PER_ITERATION = 4;   // Initial bet which will be adjusted in calibrate_delay_loop
+
+  // Simple assembler loop counting down
+  void delay_asm(uint32_t cy) {
+    cy = _MAX(cy / ASM_CYCLES_PER_ITERATION, 1U); // Zero is forbidden here
+    __asm__ __volatile__(
+      A(".syntax unified") // is to prevent CM0,CM1 non-unified syntax
+      L("1")
+      A("subs %[cnt],#1")
+      A("bne 1b")
+      : [cnt]"+r"(cy)   // output: +r means input+output
+      :                 // input:
+      : "cc"            // clobbers:
+    );
+  }
+
+  // We can't use CMSIS since it's not available on all platform, so fallback to hardcoded register values
+  #define HW_REG(X)   *(volatile uint32_t *)(X)
+  #define _DWT_CTRL   0xE0001000
+  #define _DWT_CYCCNT 0xE0001004      // CYCCNT is 32bits, takes 37s or so to wrap.
+  #define _DEM_CR     0xE000EDFC
+  #define _LAR        0xE0001FB0
+
+  // Use hardware cycle counter instead, it's much safer
+  void delay_dwt(uint32_t count) {
+    // Reuse the ASM_CYCLES_PER_ITERATION variable to avoid wasting another useless variable
+    register uint32_t start = HW_REG(_DWT_CYCCNT) - ASM_CYCLES_PER_ITERATION, elapsed;
+    do {
+      elapsed = HW_REG(_DWT_CYCCNT) - start;
+    } while (elapsed < count);
+  }
+
+  // Pointer to asm function, calling the functions has a ~20 cycles overhead
+  DelayImpl DelayCycleFnc = delay_asm;
+
+  void calibrate_delay_loop() {
+    // Check if we have a working DWT implementation in the CPU (see https://developer.arm.com/documentation/ddi0439/b/Data-Watchpoint-and-Trace-Unit/DWT-Programmers-Model)
+    if (!HW_REG(_DWT_CTRL)) {
+      // No DWT present, so fallback to plain old ASM nop counting
+      // Unfortunately, we don't exactly know how many iteration it'll take to decrement a counter in a loop
+      // It depends on the CPU architecture, the code current position (flash vs SRAM)
+      // So, instead of wild guessing and making mistake, instead
+      // compute it once for all
+      ASM_CYCLES_PER_ITERATION = 1;
+      // We need to fetch some reference clock before waiting
+      cli();
+        uint32_t start = micros();
+        delay_asm(1000); // On a typical CPU running in MHz, waiting 1000 "unknown cycles" means it'll take between 1ms to 6ms, that's perfectly acceptable
+        uint32_t end = micros();
+      sei();
+      uint32_t expectedCycles = (end - start) * ((F_CPU) / 1000000UL); // Convert microseconds to cycles
+      // Finally compute the right scale
+      ASM_CYCLES_PER_ITERATION = (uint32_t)(expectedCycles / 1000);
+
+      // No DWT present, likely a Cortex M0 so NOP counting is our best bet here
+      DelayCycleFnc = delay_asm;
+    }
+    else {
+      // Enable DWT counter
+      // From https://stackoverflow.com/a/41188674/1469714
+      HW_REG(_DEM_CR) = HW_REG(_DEM_CR) | 0x01000000;   // Enable trace
+      #if __CORTEX_M == 7
+        HW_REG(_LAR) = 0xC5ACCE55;                      // Unlock access to DWT registers, see https://developer.arm.com/documentation/ihi0029/e/ section B2.3.10
+      #endif
+      HW_REG(_DWT_CYCCNT) = 0;                          // Clear DWT cycle counter
+      HW_REG(_DWT_CTRL) = HW_REG(_DWT_CTRL) | 1;        // Enable DWT cycle counter
+
+      // Then calibrate the constant offset from the counter
+      ASM_CYCLES_PER_ITERATION = 0;
+      uint32_t s = HW_REG(_DWT_CYCCNT);
+      uint32_t e = HW_REG(_DWT_CYCCNT);  // (e - s) contains the number of cycle required to read the cycle counter
+      delay_dwt(0);
+      uint32_t f = HW_REG(_DWT_CYCCNT);  // (f - e) contains the delay to call the delay function + the time to read the cycle counter
+      ASM_CYCLES_PER_ITERATION = (f - e) - (e - s);
+
+      // Use safer DWT function
+      DelayCycleFnc = delay_dwt;
+    }
+  }
+
+  #if ENABLED(MARLIN_DEV_MODE)
+    void dump_delay_accuracy_check() {
+      auto report_call_time = [](PGM_P const name, PGM_P const unit, const uint32_t cycles, const uint32_t total, const bool do_flush=true) {
+        SERIAL_ECHOPGM("Calling ");
+        serialprintPGM(name);
+        SERIAL_ECHOLNPAIR(" for ", cycles);
+        serialprintPGM(unit);
+        SERIAL_ECHOLNPAIR(" took: ", total);
+        serialprintPGM(unit);
+        if (do_flush) SERIAL_FLUSH();
+      };
+
+      uint32_t s, e;
+
+      SERIAL_ECHOLNPAIR("Computed delay calibration value: ", ASM_CYCLES_PER_ITERATION);
+      SERIAL_FLUSH();
+      // Display the results of the calibration above
+      constexpr uint32_t testValues[] = { 1, 5, 10, 20, 50, 100, 150, 200, 350, 500, 750, 1000 };
+      for (auto i : testValues) {
+        s = micros(); DELAY_US(i); e = micros();
+        report_call_time(PSTR("delay"), PSTR("us"), i, e - s);
+      }
+
+      if (HW_REG(_DWT_CTRL)) {
+        for (auto i : testValues) {
+          s = HW_REG(_DWT_CYCCNT); DELAY_CYCLES(i); e = HW_REG(_DWT_CYCCNT);
+          report_call_time(PSTR("runtime delay"), PSTR("cycles"), i, e - s);
+        }
+
+        // Measure the delay to call a real function compared to a function pointer
+        s = HW_REG(_DWT_CYCCNT); delay_dwt(1); e = HW_REG(_DWT_CYCCNT);
+        report_call_time(PSTR("delay_dwt"), PSTR("cycles"), 1, e - s);
+
+        static PGMSTR(dcd, "DELAY_CYCLES directly ");
+
+        s = HW_REG(_DWT_CYCCNT); DELAY_CYCLES( 1); e = HW_REG(_DWT_CYCCNT);
+        report_call_time(dcd, PSTR("cycles"),  1, e - s, false);
+
+        s = HW_REG(_DWT_CYCCNT); DELAY_CYCLES( 5); e = HW_REG(_DWT_CYCCNT);
+        report_call_time(dcd, PSTR("cycles"),  5, e - s, false);
+
+        s = HW_REG(_DWT_CYCCNT); DELAY_CYCLES(10); e = HW_REG(_DWT_CYCCNT);
+        report_call_time(dcd, PSTR("cycles"), 10, e - s, false);
+
+        s = HW_REG(_DWT_CYCCNT); DELAY_CYCLES(20); e = HW_REG(_DWT_CYCCNT);
+        report_call_time(dcd, PSTR("cycles"), 20, e - s, false);
+
+        s = HW_REG(_DWT_CYCCNT); DELAY_CYCLES(50); e = HW_REG(_DWT_CYCCNT);
+        report_call_time(dcd, PSTR("cycles"), 50, e - s, false);
+
+        s = HW_REG(_DWT_CYCCNT); DELAY_CYCLES(100); e = HW_REG(_DWT_CYCCNT);
+        report_call_time(dcd, PSTR("cycles"), 100, e - s, false);
+
+        s = HW_REG(_DWT_CYCCNT); DELAY_CYCLES(200); e = HW_REG(_DWT_CYCCNT);
+        report_call_time(dcd, PSTR("cycles"), 200, e - s, false);
+      }
+    }
+  #endif // MARLIN_DEV_MODE
+
+
+#else
+
+  void calibrate_delay_loop() {}
+  #if ENABLED(MARLIN_DEV_MODE)
+    void dump_delay_accuracy_check() {
+      static PGMSTR(none, "N/A on this platform");
+      serialprintPGM(none);
+    }
+  #endif
+
+#endif
@@ -21,6 +21,8 @@
 */
 #pragma once

+#include "../../inc/MarlinConfigPre.h"
+
 /**
 * Busy wait delay cycles routines:
 *
@@ -31,79 +33,68 @@

 #include "../../core/macros.h"

+void calibrate_delay_loop();
+
 #if defined(__arm__) || defined(__thumb__)

-  #if __CORTEX_M == 7
+  // We want to have delay_cycle function with the lowest possible overhead, so we adjust at the function at runtime based on the current CPU best feature
+  typedef void (*DelayImpl)(uint32_t);
+  extern DelayImpl DelayCycleFnc;

-    // Cortex-M3 through M7 can use the cycle counter of the DWT unit
-    // https://www.anthonyvh.com/2017/05/18/cortex_m-cycle_counter/
+  // I've measured 36 cycles on my system to call the cycle waiting method, but it shouldn't change much to have a bit more margin, it only consume a bit more flash
+  #define TRIP_POINT_FOR_CALLING_FUNCTION   40

-    FORCE_INLINE static void enableCycleCounter() {
-      CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
-
-      #if __CORTEX_M == 7
-        DWT->LAR = 0xC5ACCE55; // Unlock DWT on the M7
-      #endif
-
-      DWT->CYCCNT = 0;
-      DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk;
+  // A simple recursive template class that output exactly one 'nop' of code per recursion
+  template <int N> struct NopWriter {
+    FORCE_INLINE static void build() {
+      __asm__ __volatile__("nop");
+      NopWriter<N-1>::build();
    }
+  };
+  // End the loop
+  template <> struct NopWriter<0> { FORCE_INLINE static void build() {} };

-    FORCE_INLINE volatile uint32_t getCycleCount() { return DWT->CYCCNT; }
-
-    FORCE_INLINE static void DELAY_CYCLES(const uint32_t x) {
-      const uint32_t endCycles = getCycleCount() + x;
-      while (PENDING(getCycleCount(), endCycles)) {}
-    }
-
-  #else
-
-    // https://blueprints.launchpad.net/gcc-arm-embedded/+spec/delay-cycles
-
-    #define nop() __asm__ __volatile__("nop;\n\t":::)
-
-    FORCE_INLINE static void __delay_4cycles(uint32_t cy) { // +1 cycle
-      #if ARCH_PIPELINE_RELOAD_CYCLES < 2
-        #define EXTRA_NOP_CYCLES A("nop")
-      #else
-        #define EXTRA_NOP_CYCLES ""
-      #endif
-
-      __asm__ __volatile__(
-        A(".syntax unified") // is to prevent CM0,CM1 non-unified syntax
-        L("1")
-        A("subs %[cnt],#1")
-        EXTRA_NOP_CYCLES
-        A("bne 1b")
-        : [cnt]"+r"(cy)   // output: +r means input+output
-        :                 // input:
-        : "cc"            // clobbers:
-      );
-    }
-
-    // Delay in cycles
-    FORCE_INLINE static void DELAY_CYCLES(uint32_t x) {
-
-      if (__builtin_constant_p(x)) {
-        #define MAXNOPS 4
-
-        if (x <= (MAXNOPS)) {
-          switch (x) { case 4: nop(); case 3: nop(); case 2: nop(); case 1: nop(); }
-        }
-        else { // because of +1 cycle inside delay_4cycles
-          const uint32_t rem = (x - 1) % (MAXNOPS);
-          switch (rem) { case 3: nop(); case 2: nop(); case 1: nop(); }
-          if ((x = (x - 1) / (MAXNOPS)))
-            __delay_4cycles(x); // if need more then 4 nop loop is more optimal
-        }
-        #undef MAXNOPS
+  namespace Private {
+    // Split recursing template in 2 different class so we don't reach the maximum template instantiation depth limit
+    template <bool belowTP, int N> struct Helper {
+      FORCE_INLINE static void build() {
+        DelayCycleFnc(N - 2); //  Approximative cost of calling the function (might be off by one or 2 cycles)
      }
-      else if ((x >>= 2))
-        __delay_4cycles(x);
-    }
-    #undef nop
+    };

-  #endif
+    template <int N> struct Helper<true, N> {
+      FORCE_INLINE static void build() {
+        NopWriter<N - 1>::build();
+      }
+    };
+
+    template <> struct Helper<true, 0> {
+      FORCE_INLINE static void build() {}
+    };
+
+  }
+  // Select a behavior based on the constexpr'ness of the parameter
+  // If called with a compile-time parameter, then write as many NOP as required to reach the asked cycle count
+  // (there is some tripping point here to start looping when it's more profitable than gruntly executing NOPs)
+  // If not called from a compile-time parameter, fallback to a runtime loop counting version instead
+  template <bool compileTime, int Cycles>
+  struct SmartDelay {
+    FORCE_INLINE SmartDelay(int) {
+      if (Cycles == 0) return;
+      Private::Helper<Cycles < TRIP_POINT_FOR_CALLING_FUNCTION, Cycles>::build();
+    }
+  };
+  // Runtime version below. There is no way this would run under less than ~TRIP_POINT_FOR_CALLING_FUNCTION cycles
+  template <int T>
+  struct SmartDelay<false, T> {
+    FORCE_INLINE SmartDelay(int v) { DelayCycleFnc(v); }
+  };
+
+  #define DELAY_CYCLES(X) do { SmartDelay<IS_CONSTEXPR(X), IS_CONSTEXPR(X) ? X : 0> _smrtdly_X(X); } while(0)
+
+  // For delay in microseconds, no smart delay selection is required, directly call the delay function
+  // Teensy compiler is too old and does not accept smart delay compile-time / run-time selection correctly
+  #define DELAY_US(x) DelayCycleFnc((x) * ((F_CPU) / 1000000UL))

 #elif defined(__AVR__)

@@ -144,10 +135,15 @@
  }
  #undef nop

+  // Delay in microseconds
+  #define DELAY_US(x) DELAY_CYCLES((x) * ((F_CPU) / 1000000UL))
+
 #elif defined(__PLAT_LINUX__) || defined(ESP32)

-  // specified inside platform
+  // DELAY_CYCLES specified inside platform

+  // Delay in microseconds
+  #define DELAY_US(x) DELAY_CYCLES((x) * ((F_CPU) / 1000000UL))
 #else

  #error "Unsupported MCU architecture"
@@ -157,5 +153,5 @@
 // Delay in nanoseconds
 #define DELAY_NS(x) DELAY_CYCLES((x) * ((F_CPU) / 1000000UL) / 1000UL)

-// Delay in microseconds
-#define DELAY_US(x) DELAY_CYCLES((x) * ((F_CPU) / 1000000UL))
+
+
@@ -81,3 +81,5 @@
 #ifndef UNUSED
  #define UNUSED(x) ((void)(x))
 #endif
+
+#include "progmem.h"
@@ -34,10 +34,10 @@ static bool UnwReportOut(void* ctx, const UnwReport* bte) {

  (*p)++;

-  SERIAL_CHAR('#'); SERIAL_PRINT(*p, DEC); SERIAL_ECHOPGM(" : ");
-  SERIAL_ECHOPGM(bte->name ? bte->name : "unknown"); SERIAL_ECHOPGM("@0x"); SERIAL_PRINT(bte->function, HEX);
-  SERIAL_CHAR('+'); SERIAL_PRINT(bte->address - bte->function,DEC);
-  SERIAL_ECHOPGM(" PC:"); SERIAL_PRINT(bte->address,HEX); SERIAL_CHAR('\n');
+  SERIAL_CHAR('#'); SERIAL_ECHO(*p); SERIAL_ECHOPGM(" : ");
+  SERIAL_ECHOPGM(bte->name ? bte->name : "unknown"); SERIAL_ECHOPGM("@0x"); SERIAL_PRINT(bte->function, PrintBase::Hex);
+  SERIAL_CHAR('+'); SERIAL_ECHO(bte->address - bte->function);
+  SERIAL_ECHOPGM(" PC:"); SERIAL_PRINT(bte->address, PrintBase::Hex); SERIAL_CHAR('\n');
  return true;
 }

@@ -0,0 +1,189 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program 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.
+ *
+ * This program 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 this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+#ifndef __AVR__
+#ifndef __PGMSPACE_H_
+// This define should prevent reading the system pgmspace.h if included elsewhere
+// This is not normally needed.
+#define __PGMSPACE_H_ 1
+#endif
+
+#ifndef PROGMEM
+#define PROGMEM
+#endif
+#ifndef PGM_P
+#define PGM_P  const char *
+#endif
+#ifndef PSTR
+#define PSTR(str) (str)
+#endif
+#ifndef F
+#define F(str) (str)
+#endif
+#ifndef _SFR_BYTE
+#define _SFR_BYTE(n) (n)
+#endif
+#ifndef memchr_P
+#define memchr_P(str, c, len) memchr((str), (c), (len))
+#endif
+#ifndef memcmp_P
+#define memcmp_P(a, b, n) memcmp((a), (b), (n))
+#endif
+#ifndef memcpy_P
+#define memcpy_P(dest, src, num) memcpy((dest), (src), (num))
+#endif
+#ifndef memmem_P
+#define memmem_P(a, alen, b, blen) memmem((a), (alen), (b), (blen))
+#endif
+#ifndef memrchr_P
+#define memrchr_P(str, val, len) memrchr((str), (val), (len))
+#endif
+#ifndef strcat_P
+#define strcat_P(dest, src) strcat((dest), (src))
+#endif
+#ifndef strchr_P
+#define strchr_P(str, c) strchr((str), (c))
+#endif
+#ifndef strchrnul_P
+#define strchrnul_P(str, c) strchrnul((str), (c))
+#endif
+#ifndef strcmp_P
+#define strcmp_P(a, b) strcmp((a), (b))
+#endif
+#ifndef strcpy_P
+#define strcpy_P(dest, src) strcpy((dest), (src))
+#endif
+#ifndef strcasecmp_P
+#define strcasecmp_P(a, b) strcasecmp((a), (b))
+#endif
+#ifndef strcasestr_P
+#define strcasestr_P(a, b) strcasestr((a), (b))
+#endif
+#ifndef strlcat_P
+#define strlcat_P(dest, src, len) strlcat((dest), (src), (len))
+#endif
+#ifndef strlcpy_P
+#define strlcpy_P(dest, src, len) strlcpy((dest), (src), (len))
+#endif
+#ifndef strlen_P
+#define strlen_P(s) strlen((const char *)(s))
+#endif
+#ifndef strnlen_P
+#define strnlen_P(str, len) strnlen((str), (len))
+#endif
+#ifndef strncmp_P
+#define strncmp_P(a, b, n) strncmp((a), (b), (n))
+#endif
+#ifndef strncasecmp_P
+#define strncasecmp_P(a, b, n) strncasecmp((a), (b), (n))
+#endif
+#ifndef strncat_P
+#define strncat_P(a, b, n) strncat((a), (b), (n))
+#endif
+#ifndef strncpy_P
+#define strncpy_P(a, b, n) strncpy((a), (b), (n))
+#endif
+#ifndef strpbrk_P
+#define strpbrk_P(str, chrs) strpbrk((str), (chrs))
+#endif
+#ifndef strrchr_P
+#define strrchr_P(str, c) strrchr((str), (c))
+#endif
+#ifndef strsep_P
+#define strsep_P(strp, delim) strsep((strp), (delim))
+#endif
+#ifndef strspn_P
+#define strspn_P(str, chrs) strspn((str), (chrs))
+#endif
+#ifndef strstr_P
+#define strstr_P(a, b) strstr((a), (b))
+#endif
+#ifndef sprintf_P
+#define sprintf_P(s, ...) sprintf((s), __VA_ARGS__)
+#endif
+#ifndef vfprintf_P
+#define vfprintf_P(s, ...) vfprintf((s), __VA_ARGS__)
+#endif
+#ifndef printf_P
+#define printf_P(...) printf(__VA_ARGS__)
+#endif
+#ifndef snprintf_P
+#define snprintf_P(s, n, ...) snprintf((s), (n), __VA_ARGS__)
+#endif
+#ifndef vsprintf_P
+#define vsprintf_P(s, ...) vsprintf((s),__VA_ARGS__)
+#endif
+#ifndef vsnprintf_P
+#define vsnprintf_P(s, n, ...) vsnprintf((s), (n),__VA_ARGS__)
+#endif
+#ifndef fprintf_P
+#define fprintf_P(s, ...) fprintf((s), __VA_ARGS__)
+#endif
+
+#ifndef pgm_read_byte
+#define pgm_read_byte(addr) (*(const unsigned char *)(addr))
+#endif
+#ifndef pgm_read_word
+#define pgm_read_word(addr) (*(const unsigned short *)(addr))
+#endif
+#ifndef pgm_read_dword
+#define pgm_read_dword(addr) (*(const unsigned long *)(addr))
+#endif
+#ifndef pgm_read_float
+#define pgm_read_float(addr) (*(const float *)(addr))
+#endif
+
+#ifndef pgm_read_byte_near
+#define pgm_read_byte_near(addr) pgm_read_byte(addr)
+#endif
+#ifndef pgm_read_word_near
+#define pgm_read_word_near(addr) pgm_read_word(addr)
+#endif
+#ifndef pgm_read_dword_near
+#define pgm_read_dword_near(addr) pgm_read_dword(addr)
+#endif
+#ifndef pgm_read_float_near
+#define pgm_read_float_near(addr) pgm_read_float(addr)
+#endif
+#ifndef pgm_read_byte_far
+#define pgm_read_byte_far(addr) pgm_read_byte(addr)
+#endif
+#ifndef pgm_read_word_far
+#define pgm_read_word_far(addr) pgm_read_word(addr)
+#endif
+#ifndef pgm_read_dword_far
+#define pgm_read_dword_far(addr) pgm_read_dword(addr)
+#endif
+#ifndef pgm_read_float_far
+#define pgm_read_float_far(addr) pgm_read_float(addr)
+#endif
+
+#ifndef pgm_read_pointer
+#define pgm_read_pointer
+#endif
+
+// Fix bug in pgm_read_ptr
+#undef pgm_read_ptr
+#define pgm_read_ptr(addr) (*((void**)(addr)))
+
+#endif // __AVR__
@@ -43,6 +43,7 @@
 #include <math.h>

 #include "core/utility.h"
+
 #include "module/motion.h"
 #include "module/planner.h"
 #include "module/endstops.h"
@@ -57,6 +58,7 @@
 #include "gcode/parser.h"
 #include "gcode/queue.h"

+#include "feature/pause.h"
 #include "sd/cardreader.h"

 #include "lcd/marlinui.h"
@@ -139,7 +141,6 @@

 #if ENABLED(EXPERIMENTAL_I2CBUS)
  #include "feature/twibus.h"
-  TWIBus i2c;
 #endif

 #if ENABLED(I2C_POSITION_ENCODERS)
@@ -173,10 +174,6 @@
  #include "feature/bedlevel/bedlevel.h"
 #endif

-#if BOTH(ADVANCED_PAUSE_FEATURE, PAUSE_PARK_NO_STEPPER_TIMEOUT)
-  #include "feature/pause.h"
-#endif
-
 #if ENABLED(GCODE_REPEAT_MARKERS)
  #include "feature/repeat.h"
 #endif
@@ -233,18 +230,7 @@
  #include "feature/password/password.h"
 #endif

-PGMSTR(NUL_STR, "");
 PGMSTR(M112_KILL_STR, "M112 Shutdown");
-PGMSTR(G28_STR, "G28");
-PGMSTR(M21_STR, "M21");
-PGMSTR(M23_STR, "M23 %s");
-PGMSTR(M24_STR, "M24");
-PGMSTR(SP_P_STR, " P");  PGMSTR(SP_T_STR, " T");
-PGMSTR(X_STR,     "X");  PGMSTR(Y_STR,     "Y");  PGMSTR(Z_STR,     "Z");  PGMSTR(E_STR,     "E");
-PGMSTR(X_LBL,     "X:"); PGMSTR(Y_LBL,     "Y:"); PGMSTR(Z_LBL,     "Z:"); PGMSTR(E_LBL,     "E:");
-PGMSTR(SP_A_STR, " A");  PGMSTR(SP_B_STR, " B");  PGMSTR(SP_C_STR, " C");
-PGMSTR(SP_X_STR, " X");  PGMSTR(SP_Y_STR, " Y");  PGMSTR(SP_Z_STR, " Z");  PGMSTR(SP_E_STR, " E");
-PGMSTR(SP_X_LBL, " X:"); PGMSTR(SP_Y_LBL, " Y:"); PGMSTR(SP_Z_LBL, " Z:"); PGMSTR(SP_E_LBL, " E:");

 MarlinState marlin_state = MF_INITIALIZING;

@@ -267,40 +253,12 @@ bool wait_for_heatup = true;

 #endif

-#if PIN_EXISTS(CHDK)
-  extern millis_t chdk_timeout;
-#endif
-
-#if ENABLED(I2C_POSITION_ENCODERS)
-  I2CPositionEncodersMgr I2CPEM;
-#endif
-
 /**
 * ***************************************************************************
 * ******************************** FUNCTIONS ********************************
 * ***************************************************************************
 */

-void setup_killpin() {
-  #if HAS_KILL
-    #if KILL_PIN_STATE
-      SET_INPUT_PULLDOWN(KILL_PIN);
-    #else
-      SET_INPUT_PULLUP(KILL_PIN);
-    #endif
-  #endif
-}
-
-void setup_powerhold() {
-  #if HAS_SUICIDE
-    OUT_WRITE(SUICIDE_PIN, !SUICIDE_PIN_INVERTING);
-  #endif
-  #if ENABLED(PSU_CONTROL)
-    powersupply_on = ENABLED(PSU_DEFAULT_OFF);
-    if (ENABLED(PSU_DEFAULT_OFF)) PSU_OFF(); else PSU_ON();
-  #endif
-}
-
 /**
 * Stepper Reset (RigidBoard, et.al.)
 */
@@ -309,18 +267,6 @@ void setup_powerhold() {
  void enableStepperDrivers()  { SET_INPUT(STEPPER_RESET_PIN); }      // Set to input, allowing pullups to pull the pin high
 #endif

-#if ENABLED(EXPERIMENTAL_I2CBUS) && I2C_SLAVE_ADDRESS > 0
-
-  void i2c_on_receive(int bytes) { // just echo all bytes received to serial
-    i2c.receive(bytes);
-  }
-
-  void i2c_on_request() {          // just send dummy data for now
-    i2c.reply("Hello World!\n");
-  }
-
-#endif
-
 /**
 * Sensitive pin test for M42, M226
 */
@@ -342,17 +288,6 @@ bool pin_is_protected(const pin_t pin) {

 #pragma GCC diagnostic pop

-void protected_pin_err() {
-  SERIAL_ERROR_MSG(STR_ERR_PROTECTED_PIN);
-}
-
-void quickstop_stepper() {
-  planner.quick_stop();
-  planner.synchronize();
-  set_current_from_steppers_for_axis(ALL_AXES);
-  sync_plan_position();
-}
-
 void enable_e_steppers() {
  #define _ENA_E(N) ENABLE_AXIS_E##N();
  REPEAT(E_STEPPERS, _ENA_E)
@@ -389,41 +324,6 @@ void disable_all_steppers() {
  TERN_(EXTENSIBLE_UI, ExtUI::onSteppersDisabled());
 }

-#if ENABLED(G29_RETRY_AND_RECOVER)
-
-  void event_probe_failure() {
-    #ifdef ACTION_ON_G29_FAILURE
-      host_action(PSTR(ACTION_ON_G29_FAILURE));
-    #endif
-    #ifdef G29_FAILURE_COMMANDS
-      gcode.process_subcommands_now_P(PSTR(G29_FAILURE_COMMANDS));
-    #endif
-    #if ENABLED(G29_HALT_ON_FAILURE)
-      #ifdef ACTION_ON_CANCEL
-        host_action_cancel();
-      #endif
-      kill(GET_TEXT(MSG_LCD_PROBING_FAILED));
-    #endif
-  }
-
-  void event_probe_recover() {
-    TERN_(HOST_PROMPT_SUPPORT, host_prompt_do(PROMPT_INFO, PSTR("G29 Retrying"), DISMISS_STR));
-    #ifdef ACTION_ON_G29_RECOVER
-      host_action(PSTR(ACTION_ON_G29_RECOVER));
-    #endif
-    #ifdef G29_RECOVER_COMMANDS
-      gcode.process_subcommands_now_P(PSTR(G29_RECOVER_COMMANDS));
-    #endif
-  }
-
-#endif
-
-#if ENABLED(ADVANCED_PAUSE_FEATURE)
-  #include "feature/pause.h"
-#else
-  constexpr bool did_pause_print = false;
-#endif
-
 /**
 * A Print Job exists when the timer is running or SD printing
 */
@@ -462,19 +362,20 @@ void startOrResumeJob() {
  inline void abortSDPrinting() {
    IF_DISABLED(NO_SD_AUTOSTART, card.autofile_cancel());
    card.endFilePrint(TERN_(SD_RESORT, true));
+
    queue.clear();
    quickstop_stepper();
-    print_job_timer.stop();
-    #if DISABLED(SD_ABORT_NO_COOLDOWN)
-      thermalManager.disable_all_heaters();
-    #endif
-    #if !HAS_CUTTER
-      thermalManager.zero_fan_speeds();
-    #else
-      cutter.kill();              // Full cutter shutdown including ISR control
-    #endif
+
+    print_job_timer.abort();
+
+    IF_DISABLED(SD_ABORT_NO_COOLDOWN, thermalManager.disable_all_heaters());
+
+    TERN(HAS_CUTTER, cutter.kill(), thermalManager.zero_fan_speeds()); // Full cutter shutdown including ISR control
+
    wait_for_heatup = false;
+
    TERN_(POWER_LOSS_RECOVERY, recovery.purge());
+
    #ifdef EVENT_GCODE_SD_ABORT
      queue.inject_P(PSTR(EVENT_GCODE_SD_ABORT));
    #endif
@@ -511,15 +412,14 @@ inline void manage_inactivity(const bool ignore_stepper_queue=false) {

  // Prevent steppers timing-out in the middle of M600
  // unless PAUSE_PARK_NO_STEPPER_TIMEOUT is disabled
-  const bool parked_or_ignoring = ignore_stepper_queue ||
-     (BOTH(ADVANCED_PAUSE_FEATURE, PAUSE_PARK_NO_STEPPER_TIMEOUT) && did_pause_print);
+  const bool parked_or_ignoring = ignore_stepper_queue
+                               || TERN0(PAUSE_PARK_NO_STEPPER_TIMEOUT, did_pause_print);

  // Reset both the M18/M84 activity timeout and the M85 max 'kill' timeout
  if (parked_or_ignoring) gcode.reset_stepper_timeout(ms);

  if (gcode.stepper_max_timed_out(ms)) {
-    SERIAL_ERROR_START();
-    SERIAL_ECHOLNPAIR(STR_KILL_INACTIVE_TIME, parser.command_ptr);
+    SERIAL_ERROR_MSG(STR_KILL_INACTIVE_TIME, parser.command_ptr);
    kill();
  }

@@ -550,6 +450,7 @@ inline void manage_inactivity(const bool ignore_stepper_queue=false) {
  }

  #if PIN_EXISTS(CHDK) // Check if pin should be set to LOW (after M240 set it HIGH)
+    extern millis_t chdk_timeout;
    if (chdk_timeout && ELAPSED(ms, chdk_timeout)) {
      chdk_timeout = 0;
      WRITE(CHDK_PIN, LOW);
@@ -712,8 +613,8 @@ inline void manage_inactivity(const bool ignore_stepper_queue=false) {
 */
 void idle(TERN_(ADVANCED_PAUSE_FEATURE, bool no_stepper_sleep/*=false*/)) {
  #if ENABLED(MARLIN_DEV_MODE)
-    static uint8_t idle_depth = 0;
-    if (++idle_depth > 5) SERIAL_ECHOLNPAIR("idle() call depth: ", int(idle_depth));
+    static uint16_t idle_depth = 0;
+    if (++idle_depth > 5) SERIAL_ECHOLNPAIR("idle() call depth: ", idle_depth);
  #endif

  // Core Marlin activities
@@ -785,8 +686,8 @@ void idle(TERN_(ADVANCED_PAUSE_FEATURE, bool no_stepper_sleep/*=false*/)) {
  // Auto-report Temperatures / SD Status
  #if HAS_AUTO_REPORTING
    if (!gcode.autoreport_paused) {
-      TERN_(AUTO_REPORT_TEMPERATURES, thermalManager.auto_report_temperatures());
-      TERN_(AUTO_REPORT_SD_STATUS, card.auto_report_sd_status());
+      TERN_(AUTO_REPORT_TEMPERATURES, thermalManager.auto_reporter.tick());
+      TERN_(AUTO_REPORT_SD_STATUS, card.auto_reporter.tick());
    }
  #endif

@@ -882,6 +783,7 @@ void minkill(const bool steppers_off/*=false*/) {
 */
 void stop() {
  thermalManager.disable_all_heaters(); // 'unpause' taken care of in here
+
  print_job_timer.stop();

  #if ENABLED(PROBING_FANS_OFF)
@@ -983,6 +885,38 @@ void setup() {
  #endif
  #define SETUP_RUN(C) do{ SETUP_LOG(STRINGIFY(C)); C; }while(0)

+  MYSERIAL0.begin(BAUDRATE);
+  millis_t serial_connect_timeout = millis() + 1000UL;
+  while (!MYSERIAL0.connected() && PENDING(millis(), serial_connect_timeout)) { /*nada*/ }
+
+  #if HAS_MULTI_SERIAL && !HAS_ETHERNET
+    MYSERIAL1.begin(BAUDRATE);
+    serial_connect_timeout = millis() + 1000UL;
+    while (!MYSERIAL1.connected() && PENDING(millis(), serial_connect_timeout)) { /*nada*/ }
+  #endif
+  SERIAL_ECHOLNPGM("start");
+
+  // Set up these pins early to prevent suicide
+  #if HAS_KILL
+    SETUP_LOG("KILL_PIN");
+    #if KILL_PIN_STATE
+      SET_INPUT_PULLDOWN(KILL_PIN);
+    #else
+      SET_INPUT_PULLUP(KILL_PIN);
+    #endif
+  #endif
+
+  #if HAS_SUICIDE
+    SETUP_LOG("SUICIDE_PIN");
+    OUT_WRITE(SUICIDE_PIN, !SUICIDE_PIN_INVERTING);
+  #endif
+
+  #if ENABLED(PSU_CONTROL)
+    SETUP_LOG("PSU_CONTROL");
+    powersupply_on = ENABLED(PSU_DEFAULT_OFF);
+    if (ENABLED(PSU_DEFAULT_OFF)) PSU_OFF(); else PSU_ON();
+  #endif
+
  #if EITHER(DISABLE_DEBUG, DISABLE_JTAG)
    // Disable any hardware debug to free up pins for IO
    #if ENABLED(DISABLE_DEBUG) && defined(JTAGSWD_DISABLE)
@@ -994,17 +928,6 @@ void setup() {
    #endif
  #endif

-  MYSERIAL0.begin(BAUDRATE);
-  millis_t serial_connect_timeout = millis() + 1000UL;
-  while (!MYSERIAL0 && PENDING(millis(), serial_connect_timeout)) { /*nada*/ }
-
-  #if HAS_MULTI_SERIAL && !HAS_ETHERNET
-    MYSERIAL1.begin(BAUDRATE);
-    serial_connect_timeout = millis() + 1000UL;
-    while (!MYSERIAL1 && PENDING(millis(), serial_connect_timeout)) { /*nada*/ }
-  #endif
-  SERIAL_ECHOLNPGM("start");
-
  #if BOTH(HAS_TFT_LVGL_UI, MKS_WIFI_MODULE)
    mks_esp_wifi_init();
    WIFISERIAL.begin(WIFI_BAUDRATE);
@@ -1014,11 +937,11 @@ void setup() {

  SETUP_RUN(HAL_init());

-  // Init and disable SPI thermocouples
-  #if HEATER_0_USES_MAX6675
+  // Init and disable SPI thermocouples; this is still needed
+  #if TEMP_SENSOR_0_IS_MAX_TC
    OUT_WRITE(MAX6675_SS_PIN, HIGH);  // Disable
  #endif
-  #if HEATER_1_USES_MAX6675
+  #if TEMP_SENSOR_1_IS_MAX_TC
    OUT_WRITE(MAX6675_SS2_PIN, HIGH); // Disable
  #endif

@@ -1038,14 +961,10 @@ void setup() {
    SETUP_RUN(recovery.setup());
  #endif

-  SETUP_RUN(setup_killpin());
-
  #if HAS_TMC220x
    SETUP_RUN(tmc_serial_begin());
  #endif

-  SETUP_RUN(setup_powerhold());
-
  #if HAS_STEPPER_RESET
    SETUP_RUN(disableStepperDrivers());
  #endif
@@ -1084,7 +1003,10 @@ void setup() {
    );
  #endif
  SERIAL_ECHO_MSG("Compiled: " __DATE__);
-  SERIAL_ECHO_MSG(STR_FREE_MEMORY, freeMemory(), STR_PLANNER_BUFFER_BYTES, (int)sizeof(block_t) * (BLOCK_BUFFER_SIZE));
+  SERIAL_ECHO_MSG(STR_FREE_MEMORY, freeMemory(), STR_PLANNER_BUFFER_BYTES, sizeof(block_t) * (BLOCK_BUFFER_SIZE));
+
+  // Some HAL need precise delay adjustment
+  calibrate_delay_loop();

  // Init buzzer pin(s)
  #if USE_BEEPER
@@ -37,11 +37,6 @@ void stop();
 void idle(TERN_(ADVANCED_PAUSE_FEATURE, bool no_stepper_sleep=false));
 inline void idle_no_sleep() { idle(TERN_(ADVANCED_PAUSE_FEATURE, true)); }

-#if ENABLED(EXPERIMENTAL_I2CBUS)
-  #include "feature/twibus.h"
-  extern TWIBus i2c;
-#endif
-
 #if ENABLED(G38_PROBE_TARGET)
  extern uint8_t G38_move;          // Flag to tell the ISR that G38 is in progress, and the type
  extern bool G38_did_trigger;      // Flag from the ISR to indicate the endstop changed
@@ -59,8 +54,6 @@ void disable_all_steppers();
 void kill(PGM_P const lcd_error=nullptr, PGM_P const lcd_component=nullptr, const bool steppers_off=false);
 void minkill(const bool steppers_off=false);

-void quickstop_stepper();
-
 // Global State of the firmware
 enum MarlinState : uint8_t {
  MF_INITIALIZING =  0,
@@ -103,7 +96,6 @@ extern bool wait_for_heatup;
 #endif

 bool pin_is_protected(const pin_t pin);
-void protected_pin_err();

 #if HAS_SUICIDE
  inline void suicide() { OUT_WRITE(SUICIDE_PIN, SUICIDE_PIN_INVERTING); }
@@ -116,12 +108,4 @@ void protected_pin_err();
  inline bool kill_state() { return READ(KILL_PIN) == KILL_PIN_STATE; }
 #endif

-#if ENABLED(G29_RETRY_AND_RECOVER)
-  void event_probe_recover();
-  void event_probe_failure();
-#endif
-
-extern const char NUL_STR[], M112_KILL_STR[], G28_STR[], M21_STR[], M23_STR[], M24_STR[],
-                  SP_A_STR[], SP_B_STR[], SP_C_STR[],
-                  SP_P_STR[], SP_T_STR[], SP_X_STR[], SP_Y_STR[], SP_Z_STR[], SP_E_STR[],
-                  X_LBL[], Y_LBL[], Z_LBL[], E_LBL[], SP_X_LBL[], SP_Y_LBL[], SP_Z_LBL[], SP_E_LBL[];
+extern const char M112_KILL_STR[];
@@ -229,7 +229,6 @@
 #define BOARD_BTT_SKR_V1_1            2012  // BigTreeTech SKR v1.1 (Power outputs: Hotend0, Hotend1, Fan, Bed)
 #define BOARD_BTT_SKR_V1_3            2013  // BigTreeTech SKR v1.3 (Power outputs: Hotend0, Hotend1, Fan, Bed)
 #define BOARD_BTT_SKR_V1_4            2014  // BigTreeTech SKR v1.4 (Power outputs: Hotend0, Hotend1, Fan, Bed)
-#define BOARD_GMARSH_EBAB         2015  // GMARSH EBAB board, revision 1 prototype

 //
 // LPC1769 ARM Cortex M3
@@ -339,6 +338,7 @@
 #define BOARD_FLY_MINI                4045  // FLY MINI (STM32F103RCT6)
 #define BOARD_FLSUN_HISPEED           4046  // FLSUN HiSpeedV1 (STM32F103VET6)
 #define BOARD_BEAST                   4047  // STM32F103RET6 Libmaple-based controller
+#define BOARD_MINGDA_MPX_ARM_MINI     4048  // STM32F103ZET6 Mingda MD-16

 //
 // ARM Cortex-M4F
@@ -374,6 +374,7 @@
 #define BOARD_MKS_ROBIN_NANO_V3       4220  // MKS Robin Nano V3 (STM32F407VG)
 #define BOARD_ANET_ET4                4221  // ANET ET4 V1.x (STM32F407VGT6)
 #define BOARD_ANET_ET4P               4222  // ANET ET4P V1.x (STM32F407VGT6)
+#define BOARD_FYSETC_CHEETAH_V20      4223  // FYSETC Cheetah V2.0

 //
 // ARM Cortex M7
@@ -414,5 +415,3 @@

 #define _MB_1(B)  (defined(BOARD_##B) && MOTHERBOARD==BOARD_##B)
 #define MB(V...)  DO(MB,||,V)
-
-#define IS_MELZI MB(MELZI, MELZI_CREALITY, MELZI_MAKR3D, MELZI_MALYAN, MELZI_TRONXY, MELZI_V2)
@@ -68,6 +68,7 @@
 // ro         Romanian
 // ru         Russian
 // sk         Slovak
+// sv         Swedish
 // tr         Turkish
 // uk         Ukrainian
 // vi         Vietnamese
@@ -53,6 +53,7 @@

 #define _FORCE_INLINE_ __attribute__((__always_inline__)) __inline__
 #define  FORCE_INLINE  __attribute__((always_inline)) inline
+#define NO_INLINE      __attribute__((noinline))
 #define _UNUSED      __attribute__((unused))
 #define _O0          __attribute__((optimize("O0")))
 #define _Os          __attribute__((optimize("Os")))
@@ -60,6 +61,8 @@
 #define _O2          __attribute__((optimize("O2")))
 #define _O3          __attribute__((optimize("O3")))

+#define IS_CONSTEXPR(...) __builtin_constant_p(__VA_ARGS__) // Only valid solution with C++14. Should use std::is_constant_evaluated() in C++20 instead
+
 #ifndef UNUSED
  #define UNUSED(x) ((void)(x))
 #endif
@@ -127,20 +130,20 @@

  #define NOLESS(v, n) \
    do{ \
-      __typeof__(n) _n = (n); \
+      __typeof__(v) _n = (n); \
      if (_n > v) v = _n; \
    }while(0)

  #define NOMORE(v, n) \
    do{ \
-      __typeof__(n) _n = (n); \
+      __typeof__(v) _n = (n); \
      if (_n < v) v = _n; \
    }while(0)

  #define LIMIT(v, n1, n2) \
    do{ \
-      __typeof__(n1) _n1 = (n1); \
-      __typeof__(n2) _n2 = (n2); \
+      __typeof__(v) _n1 = (n1); \
+      __typeof__(v) _n2 = (n2); \
      if (_n1 > v) v = _n1; \
      else if (_n2 < v) v = _n2; \
    }while(0)
@@ -162,6 +165,7 @@
 #define _DO_12(W,C,A,V...) (_##W##_1(A) C _DO_11(W,C,V))
 #define _DO_13(W,C,A,V...) (_##W##_1(A) C _DO_12(W,C,V))
 #define _DO_14(W,C,A,V...) (_##W##_1(A) C _DO_13(W,C,V))
+#define _DO_15(W,C,A,V...) (_##W##_1(A) C _DO_14(W,C,V))
 #define __DO_N(W,C,N,V...) _DO_##N(W,C,V)
 #define _DO_N(W,C,N,V...)  __DO_N(W,C,N,V)
 #define DO(W,C,V...)       (_DO_N(W,C,NUM_ARGS(V),V))
@@ -313,6 +317,38 @@

  #endif

+  // C++11 solution that is standard compliant. <type_traits> is not available on all platform
+  namespace Private {
+    template<bool, typename _Tp = void> struct enable_if { };
+    template<typename _Tp>              struct enable_if<true, _Tp> { typedef _Tp type; };
+
+    template<typename T, typename U> struct is_same { enum { value = false }; };
+    template<typename T> struct is_same<T, T> { enum { value = true }; };
+
+    template <typename T, typename ... Args> struct first_type_of { typedef T type; };
+    template <typename T> struct first_type_of<T> { typedef T type; };
+  }
+  // C++11 solution using SFINAE to detect the existance of a member in a class at compile time.
+  // It creates a HasMember<Type> structure containing 'value' set to true if the member exists
+  #define HAS_MEMBER_IMPL(Member) \
+    namespace Private { \
+      template <typename Type, typename Yes=char, typename No=long> struct HasMember_ ## Member { \
+        template <typename C> static Yes& test( decltype(&C::Member) ) ; \
+        template <typename C> static No& test(...); \
+        enum { value = sizeof(test<Type>(0)) == sizeof(Yes) }; }; \
+    }
+
+  // Call the method if it exists, but do nothing if it does not. The method is detected at compile time.
+  // If the method exists, this is inlined and does not cost anything. Else, an "empty" wrapper is created, returning a default value
+  #define CALL_IF_EXISTS_IMPL(Return, Method, ...) \
+    HAS_MEMBER_IMPL(Method) \
+    namespace Private { \
+      template <typename T, typename ... Args> FORCE_INLINE typename enable_if<HasMember_ ## Method <T>::value, Return>::type Call_ ## Method(T * t, Args... a) { return static_cast<Return>(t->Method(a...)); } \
+                                                      _UNUSED static                                                  Return  Call_ ## Method(...) { return __VA_ARGS__; } \
+    }
+  #define CALL_IF_EXISTS(Return, That, Method, ...) \
+    static_cast<Return>(Private::Call_ ## Method(That, ##__VA_ARGS__))
+
 #else

  #define MIN_2(a,b)      ((a)<(b)?(a):(b))
@@ -20,6 +20,8 @@
 ****************************************************************************/
 #pragma once

+#include "../inc/MarlinConfigPre.h"
+
 typedef const char Language_Str[];

 #ifdef LCD_LANGUAGE_5
@@ -57,26 +59,27 @@ typedef const char Language_Str[];
 #define GET_LANG(LANG) _GET_LANG(LANG)

 #if NUM_LANGUAGES > 1
-  extern uint8_t lang;
+  #define HAS_MULTI_LANGUAGE 1
  #define GET_TEXT(MSG) ( \
-    lang == 0 ? GET_LANG(LCD_LANGUAGE)::MSG : \
-    lang == 1 ? GET_LANG(LCD_LANGUAGE_2)::MSG : \
-    lang == 2 ? GET_LANG(LCD_LANGUAGE_3)::MSG : \
-    lang == 3 ? GET_LANG(LCD_LANGUAGE_4)::MSG : \
-                GET_LANG(LCD_LANGUAGE_5)::MSG \
-    )
-  #define MAX_LANG_CHARSIZE _MAX(GET_LANG(LCD_LANGUAGE)::CHARSIZE, \
-                                GET_LANG(LCD_LANGUAGE_2)::CHARSIZE, \
-                                GET_LANG(LCD_LANGUAGE_3)::CHARSIZE, \
-                                GET_LANG(LCD_LANGUAGE_4)::CHARSIZE, \
-                                GET_LANG(LCD_LANGUAGE_5)::CHARSIZE)
+    ui.language == 0 ? GET_LANG(LCD_LANGUAGE  )::MSG : \
+    ui.language == 1 ? GET_LANG(LCD_LANGUAGE_2)::MSG : \
+    ui.language == 2 ? GET_LANG(LCD_LANGUAGE_3)::MSG : \
+    ui.language == 3 ? GET_LANG(LCD_LANGUAGE_4)::MSG : \
+                       GET_LANG(LCD_LANGUAGE_5)::MSG   )
+  #define MAX_LANG_CHARSIZE _MAX(GET_LANG(LCD_LANGUAGE  )::CHARSIZE, \
+                                 GET_LANG(LCD_LANGUAGE_2)::CHARSIZE, \
+                                 GET_LANG(LCD_LANGUAGE_3)::CHARSIZE, \
+                                 GET_LANG(LCD_LANGUAGE_4)::CHARSIZE, \
+                                 GET_LANG(LCD_LANGUAGE_5)::CHARSIZE  )
 #else
  #define GET_TEXT(MSG) GET_LANG(LCD_LANGUAGE)::MSG
-  #define MAX_LANG_CHARSIZE GET_LANG(LCD_LANGUAGE)::CHARSIZE
+  #define MAX_LANG_CHARSIZE LANG_CHARSIZE
 #endif
 #define GET_TEXT_F(MSG) (const __FlashStringHelper*)GET_TEXT(MSG)

 #define GET_LANGUAGE_NAME(INDEX) GET_LANG(LCD_LANGUAGE_##INDEX)::LANGUAGE
+#define LANG_CHARSIZE GET_TEXT(CHARSIZE)
+#define USE_WIDE_GLYPH (LANG_CHARSIZE > 2)

 #define MSG_1_LINE(A)     A "\0"   "\0"
 #define MSG_2_LINE(A,B)   A "\0" B "\0"
@@ -23,27 +23,50 @@
 #include "serial.h"
 #include "../inc/MarlinConfig.h"

+#if HAS_ETHERNET
+  #include "../feature/ethernet.h"
+#endif
+
 uint8_t marlin_debug_flags = MARLIN_DEBUG_NONE;

-static PGMSTR(errormagic, "Error:");
-static PGMSTR(echomagic, "echo:");
+// Commonly-used strings in serial output
+PGMSTR(NUL_STR,   "");   PGMSTR(SP_P_STR, " P");  PGMSTR(SP_T_STR, " T");
+PGMSTR(X_STR,     "X");  PGMSTR(Y_STR,     "Y");  PGMSTR(Z_STR,     "Z");  PGMSTR(E_STR,     "E");
+PGMSTR(X_LBL,     "X:"); PGMSTR(Y_LBL,     "Y:"); PGMSTR(Z_LBL,     "Z:"); PGMSTR(E_LBL,     "E:");
+PGMSTR(SP_A_STR, " A");  PGMSTR(SP_B_STR, " B");  PGMSTR(SP_C_STR, " C");
+PGMSTR(SP_X_STR, " X");  PGMSTR(SP_Y_STR, " Y");  PGMSTR(SP_Z_STR, " Z");  PGMSTR(SP_E_STR, " E");
+PGMSTR(SP_X_LBL, " X:"); PGMSTR(SP_Y_LBL, " Y:"); PGMSTR(SP_Z_LBL, " Z:"); PGMSTR(SP_E_LBL, " E:");

 #if HAS_MULTI_SERIAL
-  int8_t serial_port_index = 0;
+  #ifdef SERIAL_CATCHALL
+    SerialOutputT multiSerial(MYSERIAL, SERIAL_CATCHALL);
+  #else
+    #if HAS_ETHERNET
+      // Runtime checking of the condition variable
+      ConditionalSerial<decltype(MYSERIAL1)> serialOut1(ethernet.have_telnet_client, MYSERIAL1, false); // Takes reference here
+    #else
+      // Don't pay for runtime checking a true variable, instead use the output directly
+      #define serialOut1 MYSERIAL1
+    #endif
+    SerialOutputT multiSerial(MYSERIAL0, serialOut1);
+  #endif
 #endif

 void serialprintPGM(PGM_P str) {
  while (const char c = pgm_read_byte(str++)) SERIAL_CHAR(c);
 }
-void serial_echo_start()  { serialprintPGM(echomagic); }
-void serial_error_start() { serialprintPGM(errormagic); }

+void serial_echo_start()  { static PGMSTR(echomagic, "echo:"); serialprintPGM(echomagic); }
+void serial_error_start() { static PGMSTR(errormagic, "Error:"); serialprintPGM(errormagic); }
+
+void serial_echopair_PGM(PGM_P const s_P, serial_char_t v) { serialprintPGM(s_P); SERIAL_CHAR(v.c); }
 void serial_echopair_PGM(PGM_P const s_P, const char *v)   { serialprintPGM(s_P); SERIAL_ECHO(v); }
-void serial_echopair_PGM(PGM_P const s_P, char v)          { serialprintPGM(s_P); SERIAL_CHAR(v); }
+void serial_echopair_PGM(PGM_P const s_P, char v)          { serialprintPGM(s_P); SERIAL_ECHO(v); }
 void serial_echopair_PGM(PGM_P const s_P, int v)           { serialprintPGM(s_P); SERIAL_ECHO(v); }
 void serial_echopair_PGM(PGM_P const s_P, long v)          { serialprintPGM(s_P); SERIAL_ECHO(v); }
 void serial_echopair_PGM(PGM_P const s_P, float v)         { serialprintPGM(s_P); SERIAL_DECIMAL(v); }
 void serial_echopair_PGM(PGM_P const s_P, double v)        { serialprintPGM(s_P); SERIAL_DECIMAL(v); }
+void serial_echopair_PGM(PGM_P const s_P, unsigned char v) { serialprintPGM(s_P); SERIAL_ECHO(v); }
 void serial_echopair_PGM(PGM_P const s_P, unsigned int v)  { serialprintPGM(s_P); SERIAL_ECHO(v); }
 void serial_echopair_PGM(PGM_P const s_P, unsigned long v) { serialprintPGM(s_P); SERIAL_ECHO(v); }

@@ -65,8 +88,6 @@ void print_bin(uint16_t val) {
  }
 }

-extern const char SP_X_STR[], SP_Y_STR[], SP_Z_STR[];
-
 void print_xyz(const float &x, const float &y, const float &z, PGM_P const prefix/*=nullptr*/, PGM_P const suffix/*=nullptr*/) {
  if (prefix) serialprintPGM(prefix);
  SERIAL_ECHOPAIR_P(SP_X_STR, x, SP_Y_STR, y, SP_Z_STR, z);
@@ -22,14 +22,19 @@
 #pragma once

 #include "../inc/MarlinConfig.h"
+#include "serial_hook.h"

-#if HAS_ETHERNET
-  #include "../feature/ethernet.h"
-#endif
+// Commonly-used strings in serial output
+extern const char NUL_STR[], SP_P_STR[], SP_T_STR[],
+                  X_STR[], Y_STR[], Z_STR[], E_STR[],
+                  X_LBL[], Y_LBL[], Z_LBL[], E_LBL[],
+                  SP_A_STR[], SP_B_STR[], SP_C_STR[],
+                  SP_X_STR[], SP_Y_STR[], SP_Z_STR[], SP_E_STR[],
+                  SP_X_LBL[], SP_Y_LBL[], SP_Z_LBL[], SP_E_LBL[];

-/**
- * Define debug bit-masks
- */
+//
+// Debugging flags for use by M111
+//
 enum MarlinDebugFlags : uint8_t {
  MARLIN_DEBUG_NONE          = 0,
  MARLIN_DEBUG_ECHO          = _BV(0), ///< Echo commands in order as they are processed
@@ -50,65 +55,75 @@ enum MarlinDebugFlags : uint8_t {
 extern uint8_t marlin_debug_flags;
 #define DEBUGGING(F) (marlin_debug_flags & (MARLIN_DEBUG_## F))

-#define SERIAL_BOTH 0x7F
+//
+// Serial redirection
+//
+typedef int8_t serial_index_t;
+#define SERIAL_ALL 0x7F
 #if HAS_MULTI_SERIAL
-  extern int8_t serial_port_index;
-  #define _PORT_REDIRECT(n,p)   REMEMBER(n,serial_port_index,p)
-  #define _PORT_RESTORE(n)      RESTORE(n)
-
+  #define _PORT_REDIRECT(n,p)   REMEMBER(n,multiSerial.portMask,p)
+  #define SERIAL_ASSERT(P)      if(multiSerial.portMask!=(P)){ debugger(); }
  #ifdef SERIAL_CATCHALL
-    #define SERIAL_OUT(WHAT, V...) (void)CAT(MYSERIAL,SERIAL_CATCHALL).WHAT(V)
+    typedef MultiSerial<decltype(MYSERIAL), decltype(SERIAL_CATCHALL), 0> SerialOutputT;
  #else
-    #define SERIAL_OUT(WHAT, V...) do{ \
-      const bool port2_open = TERN1(HAS_ETHERNET, ethernet.have_telnet_client); \
-      if ( serial_port_index == 0 || serial_port_index == SERIAL_BOTH)                (void)MYSERIAL0.WHAT(V); \
-      if ((serial_port_index == 1 || serial_port_index == SERIAL_BOTH) && port2_open) (void)MYSERIAL1.WHAT(V); \
-    }while(0)
+    typedef MultiSerial<decltype(MYSERIAL0), TERN(HAS_ETHERNET, ConditionalSerial<decltype(MYSERIAL1)>, decltype(MYSERIAL1)), 0>      SerialOutputT;
  #endif
-
-  #define SERIAL_ASSERT(P)      if(serial_port_index!=(P)){ debugger(); }
+  extern SerialOutputT          multiSerial;
+  #define SERIAL_IMPL           multiSerial
 #else
  #define _PORT_REDIRECT(n,p)   NOOP
-  #define _PORT_RESTORE(n)      NOOP
-  #define SERIAL_OUT(WHAT, V...) (void)MYSERIAL0.WHAT(V)
  #define SERIAL_ASSERT(P)      NOOP
+  #define SERIAL_IMPL           MYSERIAL0
 #endif

+#define SERIAL_OUT(WHAT, V...)  (void)SERIAL_IMPL.WHAT(V)
+
 #define PORT_REDIRECT(p)        _PORT_REDIRECT(1,p)
-#define PORT_RESTORE()          _PORT_RESTORE(1)
+#define SERIAL_PORTMASK(P)      _BV(P)

-#define SERIAL_ECHO(x)          SERIAL_OUT(print, x)
-#define SERIAL_ECHO_F(V...)     SERIAL_OUT(print, V)
-#define SERIAL_ECHOLN(x)        SERIAL_OUT(println, x)
-#define SERIAL_PRINT(x,b)       SERIAL_OUT(print, x, b)
-#define SERIAL_PRINTLN(x,b)     SERIAL_OUT(println, x, b)
-#define SERIAL_PRINTF(V...)     SERIAL_OUT(printf, V)
-#define SERIAL_FLUSH()          SERIAL_OUT(flush)
+//
+// SERIAL_CHAR - Print one or more individual chars
+//
+inline void SERIAL_CHAR(char a) { SERIAL_IMPL.write(a); }
+template <typename ... Args>
+void SERIAL_CHAR(char a, Args ... args) { SERIAL_IMPL.write(a); SERIAL_CHAR(args ...); }

-#ifdef ARDUINO_ARCH_STM32
-  #define SERIAL_FLUSHTX()      SERIAL_OUT(flush)
-#elif TX_BUFFER_SIZE > 0
-  #define SERIAL_FLUSHTX()      SERIAL_OUT(flushTX)
-#else
-  #define SERIAL_FLUSHTX()
-#endif
+/**
+ * SERIAL_ECHO - Print a single string or value.
+ *   Any numeric parameter (including char) is printed as a base-10 number.
+ *   A string pointer or literal will be output as a string.
+ *
+ * NOTE: Use SERIAL_CHAR to print char as a single character.
+ */
+template <typename T>
+void SERIAL_ECHO(T x) { SERIAL_IMPL.print(x); }

-// Print up to 10 chars from a list
-#define __CHAR_N(N,V...)  _CHAR_##N(V)
-#define _CHAR_N(N,V...)   __CHAR_N(N,V)
-#define _CHAR_1(c)        SERIAL_OUT(write, c)
-#define _CHAR_2(a,b)      do{ _CHAR_1(a); _CHAR_1(b); }while(0)
-#define _CHAR_3(a,V...)   do{ _CHAR_1(a); _CHAR_2(V); }while(0)
-#define _CHAR_4(a,V...)   do{ _CHAR_1(a); _CHAR_3(V); }while(0)
-#define _CHAR_5(a,V...)   do{ _CHAR_1(a); _CHAR_4(V); }while(0)
-#define _CHAR_6(a,V...)   do{ _CHAR_1(a); _CHAR_5(V); }while(0)
-#define _CHAR_7(a,V...)   do{ _CHAR_1(a); _CHAR_6(V); }while(0)
-#define _CHAR_8(a,V...)   do{ _CHAR_1(a); _CHAR_7(V); }while(0)
-#define _CHAR_9(a,V...)   do{ _CHAR_1(a); _CHAR_8(V); }while(0)
-#define _CHAR_10(a,V...)  do{ _CHAR_1(a); _CHAR_9(V); }while(0)
+// Wrapper for ECHO commands to interpret a char
+typedef struct SerialChar { char c; SerialChar(char n) : c(n) { } } serial_char_t;
+inline void SERIAL_ECHO(serial_char_t x) { SERIAL_IMPL.write(x.c); }
+#define AS_CHAR(C) serial_char_t(C)

-#define SERIAL_CHAR(V...) _CHAR_N(NUM_ARGS(V),V)
+// SERIAL_ECHO_F prints a floating point value with optional precision
+inline void SERIAL_ECHO_F(EnsureDouble x, int digit = 2) { SERIAL_IMPL.print(x, digit); }

+template <typename T>
+void SERIAL_ECHOLN(T x) { SERIAL_IMPL.println(x); }
+
+// SERIAL_PRINT works like SERIAL_ECHO but allow to specify the encoding base of the number printed
+template <typename T, typename U>
+void SERIAL_PRINT(T x, U y) { SERIAL_IMPL.print(x, y); }
+
+template <typename T, typename U>
+void SERIAL_PRINTLN(T x, U y) { SERIAL_IMPL.println(x, y); }
+
+// Flush the serial port
+inline void SERIAL_FLUSH()    { SERIAL_IMPL.flush(); }
+inline void SERIAL_FLUSHTX()  { SERIAL_IMPL.flushTX(); }
+
+// Print a single PROGMEM string to serial
+void serialprintPGM(PGM_P str);
+
+// SERIAL_ECHOPAIR / SERIAL_ECHOPAIR_P is used to output a key value pair. The key must be a string and the value can be anything
 // Print up to 12 pairs of values. Odd elements auto-wrapped in PSTR().
 #define __SEP_N(N,V...)   _SEP_##N(V)
 #define _SEP_N(N,V...)    __SEP_N(N,V)
@@ -167,6 +182,7 @@ extern uint8_t marlin_debug_flags;
 #define _SEP_23_P(a,b,V...) do{ _SEP_2_P(a,b); _SEP_21_P(V); }while(0)
 #define _SEP_24_P(a,b,V...) do{ _SEP_2_P(a,b); _SEP_22_P(V); }while(0)

+// SERIAL_ECHOPAIR_P is used to output a key value pair. Unlike SERIAL_ECHOPAIR, the key must be a PGM string already and the value can be anything
 #define SERIAL_ECHOPAIR_P(V...) _SEP_N_P(NUM_ARGS(V),V)

 // Print up to 12 pairs of values followed by newline
@@ -241,32 +257,39 @@ extern uint8_t marlin_debug_flags;

 #define SERIAL_ECHOLNPAIR_P(V...) _SELP_N_P(NUM_ARGS(V),V)

-// Print up to 20 comma-separated pairs of values
-#define __SLST_N(N,V...)   _SLST_##N(V)
-#define _SLST_N(N,V...)    __SLST_N(N,V)
-#define _SLST_1(a)         SERIAL_ECHO(a)
-#define _SLST_2(a,b)       do{ SERIAL_ECHO(a); SERIAL_ECHOPAIR(", ",b);     }while(0)
-#define _SLST_3(a,b,c)     do{ SERIAL_ECHO(a); _SEP_2(", ",b); _SLST_1(c);  }while(0)
-#define _SLST_4(a,b,V...)  do{ SERIAL_ECHO(a); _SEP_2(", ",b); _SLST_2(V);  }while(0)
-#define _SLST_5(a,b,V...)  do{ SERIAL_ECHO(a); _SEP_2(", ",b); _SLST_3(V);  }while(0)
-#define _SLST_6(a,b,V...)  do{ SERIAL_ECHO(a); _SEP_2(", ",b); _SLST_4(V);  }while(0)
-#define _SLST_7(a,b,V...)  do{ SERIAL_ECHO(a); _SEP_2(", ",b); _SLST_5(V);  }while(0)
-#define _SLST_8(a,b,V...)  do{ SERIAL_ECHO(a); _SEP_2(", ",b); _SLST_6(V);  }while(0)
-#define _SLST_9(a,b,V...)  do{ SERIAL_ECHO(a); _SEP_2(", ",b); _SLST_7(V);  }while(0)
-#define _SLST_10(a,b,V...) do{ SERIAL_ECHO(a); _SEP_2(", ",b); _SLST_8(V);  }while(0)
-#define _SLST_11(a,b,V...) do{ SERIAL_ECHO(a); _SEP_2(", ",b); _SLST_9(V);  }while(0)
-#define _SLST_12(a,b,V...) do{ SERIAL_ECHO(a); _SEP_2(", ",b); _SLST_10(V); }while(0)
-#define _SLST_13(a,b,V...) do{ SERIAL_ECHO(a); _SEP_2(", ",b); _SLST_11(V); }while(0)
-#define _SLST_14(a,b,V...) do{ SERIAL_ECHO(a); _SEP_2(", ",b); _SLST_12(V); }while(0)
-#define _SLST_15(a,b,V...) do{ SERIAL_ECHO(a); _SEP_2(", ",b); _SLST_13(V); }while(0)
-#define _SLST_16(a,b,V...) do{ SERIAL_ECHO(a); _SEP_2(", ",b); _SLST_14(V); }while(0)
-#define _SLST_17(a,b,V...) do{ SERIAL_ECHO(a); _SEP_2(", ",b); _SLST_15(V); }while(0)
-#define _SLST_18(a,b,V...) do{ SERIAL_ECHO(a); _SEP_2(", ",b); _SLST_16(V); }while(0)
-#define _SLST_19(a,b,V...) do{ SERIAL_ECHO(a); _SEP_2(", ",b); _SLST_17(V); }while(0)
-#define _SLST_20(a,b,V...) do{ SERIAL_ECHO(a); _SEP_2(", ",b); _SLST_18(V); }while(0) // Eat two args, pass the rest up
+#ifdef AllowDifferentTypeInList

-#define SERIAL_ECHOLIST(pre,V...)   do{ SERIAL_ECHOPGM(pre); _SLST_N(NUM_ARGS(V),V); }while(0)
-#define SERIAL_ECHOLIST_N(N,V...)   _SLST_N(N,LIST_N(N,V))
+  inline void SERIAL_ECHOLIST_IMPL() {}
+  template <typename T>
+  void SERIAL_ECHOLIST_IMPL(T && t) { SERIAL_IMPL.print(t); }
+
+  template <typename T, typename ... Args>
+  void SERIAL_ECHOLIST_IMPL(T && t, Args && ... args) {
+    SERIAL_IMPL.print(t);
+    serialprintPGM(PSTR(", "));
+    SERIAL_ECHOLIST_IMPL(args...);
+  }
+
+  template <typename ... Args>
+  void SERIAL_ECHOLIST(PGM_P const str, Args && ... args) {
+    SERIAL_IMPL.print(str);
+    SERIAL_ECHOLIST_IMPL(args...);
+  }
+
+#else // Optimization if the listed type are all the same (seems to be the case in the codebase so use that instead)
+
+  template <typename ... Args>
+  void SERIAL_ECHOLIST(PGM_P const str, Args && ... args) {
+    serialprintPGM(str);
+    typename Private::first_type_of<Args...>::type values[] = { args... };
+    constexpr size_t argsSize = sizeof...(args);
+    for (size_t i = 0; i < argsSize; i++) {
+      if (i) serialprintPGM(PSTR(", "));
+      SERIAL_IMPL.print(values[i]);
+    }
+  }
+
+#endif

 #define SERIAL_ECHOPGM_P(P)         (serialprintPGM(P))
 #define SERIAL_ECHOLNPGM_P(P)       (serialprintPGM(P "\n"))
@@ -300,19 +323,19 @@ extern uint8_t marlin_debug_flags;
 //
 // Functions for serial printing from PROGMEM. (Saves loads of SRAM.)
 //
+void serial_echopair_PGM(PGM_P const s_P, serial_char_t v);
 void serial_echopair_PGM(PGM_P const s_P, const char *v);
 void serial_echopair_PGM(PGM_P const s_P, char v);
 void serial_echopair_PGM(PGM_P const s_P, int v);
-void serial_echopair_PGM(PGM_P const s_P, unsigned int v);
 void serial_echopair_PGM(PGM_P const s_P, long v);
-void serial_echopair_PGM(PGM_P const s_P, unsigned long v);
 void serial_echopair_PGM(PGM_P const s_P, float v);
 void serial_echopair_PGM(PGM_P const s_P, double v);
-inline void serial_echopair_PGM(PGM_P const s_P, uint8_t v) { serial_echopair_PGM(s_P, (int)v); }
+void serial_echopair_PGM(PGM_P const s_P, unsigned char v);
+void serial_echopair_PGM(PGM_P const s_P, unsigned int v);
+void serial_echopair_PGM(PGM_P const s_P, unsigned long v);
 inline void serial_echopair_PGM(PGM_P const s_P, bool v)    { serial_echopair_PGM(s_P, (int)v); }
 inline void serial_echopair_PGM(PGM_P const s_P, void *v)   { serial_echopair_PGM(s_P, (uintptr_t)v); }

-void serialprintPGM(PGM_P str);
 void serial_echo_start();
 void serial_error_start();
 void serial_ternary(const bool onoff, PGM_P const pre, PGM_P const on, PGM_P const off, PGM_P const post=nullptr);
@@ -0,0 +1,193 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program 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.
+ *
+ * This program 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 this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+#include "../inc/MarlinConfigPre.h"
+#include "macros.h"
+
+#if ENABLED(EMERGENCY_PARSER)
+  #include "../feature/e_parser.h"
+#endif
+
+// flushTX is not implemented in all HAL, so use SFINAE to call the method where it is.
+CALL_IF_EXISTS_IMPL(void, flushTX );
+CALL_IF_EXISTS_IMPL(bool, connected, true);
+
+// In order to catch usage errors in code, we make the base to encode number explicit
+// If given a number (and not this enum), the compiler will reject the overload, falling back to the (double, digit) version
+// We don't want hidden conversion of the first parameter to double, so it has to be as hard to do for the compiler as creating this enum
+enum class PrintBase {
+  Dec = 10,
+  Hex = 16,
+  Oct = 8,
+  Bin = 2
+};
+
+// A simple forward struct that prevent the compiler to select print(double, int) as a default overload for any type different than 
+// double or float. For double or float, a conversion exists so the call will be transparent
+struct EnsureDouble {
+  double a;
+  FORCE_INLINE operator double() { return a; }
+  // If the compiler breaks on ambiguity here, it's likely because you're calling print(X, base) with X not a double or a float, and a
+  // base that's not one of PrintBase's value. This exact code is made to detect such error, you NEED to set a base explicitely like this:
+  // SERIAL_PRINT(v, PrintBase::Hex) 
+  FORCE_INLINE EnsureDouble(double a) : a(a) {}
+  FORCE_INLINE EnsureDouble(float a) : a(a) {}
+};
+
+// Using Curiously Recurring Template Pattern here to avoid virtual table cost when compiling.
+// Since the real serial class is known at compile time, this results in the compiler writing
+// a completely efficient code.
+template <class Child>
+struct SerialBase {
+  #if ENABLED(EMERGENCY_PARSER)
+    const bool ep_enabled;
+    EmergencyParser::State emergency_state;
+    inline bool emergency_parser_enabled() { return ep_enabled; }
+    SerialBase(bool ep_capable) : ep_enabled(ep_capable), emergency_state(EmergencyParser::State::EP_RESET) {}
+  #else
+    SerialBase(const bool) {}
+  #endif
+
+  // Static dispatch methods below:
+  // The most important method here is where it all ends to:
+  size_t write(uint8_t c)           { return static_cast<Child*>(this)->write(c); }
+  // Called when the parser finished processing an instruction, usually build to nothing
+  void msgDone()                    { static_cast<Child*>(this)->msgDone(); }
+  // Called upon initialization
+  void begin(const long baudRate)   { static_cast<Child*>(this)->begin(baudRate); }
+  // Called upon destruction
+  void end()                        { static_cast<Child*>(this)->end(); }
+  /** Check for available data from the port
+      @param index  The port index, usually 0 */
+  bool available(uint8_t index = 0) { return static_cast<Child*>(this)->available(index); }
+  /** Read a value from the port
+      @param index  The port index, usually 0 */
+  int  read(uint8_t index = 0)      { return static_cast<Child*>(this)->read(index); }
+  // Check if the serial port is connected (usually bypassed)
+  bool connected()                  { return static_cast<Child*>(this)->connected(); }
+  // Redirect flush
+  void flush()                      { static_cast<Child*>(this)->flush(); }
+  // Not all implementation have a flushTX, so let's call them only if the child has the implementation
+  void flushTX()                    { CALL_IF_EXISTS(void, static_cast<Child*>(this), flushTX); }
+
+  // Glue code here
+  FORCE_INLINE void write(const char* str)                    { while (*str) write(*str++); }
+  FORCE_INLINE void write(const uint8_t* buffer, size_t size) { while (size--) write(*buffer++); }
+  FORCE_INLINE void print(const char* str)                    { write(str); }
+  // No default argument to avoid ambiguity
+  NO_INLINE void print(char c, PrintBase base)                { printNumber((signed long)c, (uint8_t)base); }
+  NO_INLINE void print(unsigned char c, PrintBase base)       { printNumber((unsigned long)c, (uint8_t)base); }
+  NO_INLINE void print(int c, PrintBase base)                 { printNumber((signed long)c, (uint8_t)base); }
+  NO_INLINE void print(unsigned int c, PrintBase base)        { printNumber((unsigned long)c, (uint8_t)base); }
+  void print(unsigned long c, PrintBase base)                 { printNumber((unsigned long)c, (uint8_t)base); }
+  void print(long c, PrintBase base)                          { printNumber((signed long)c, (uint8_t)base); }
+  void print(EnsureDouble c, int digits)                      { printFloat(c, digits); }
+
+  // Forward the call to the former's method
+  FORCE_INLINE void print(char c)                { print(c, PrintBase::Dec); }
+  FORCE_INLINE void print(unsigned char c)       { print(c, PrintBase::Dec); }
+  FORCE_INLINE void print(int c)                 { print(c, PrintBase::Dec); }
+  FORCE_INLINE void print(unsigned int c)        { print(c, PrintBase::Dec); }
+  FORCE_INLINE void print(unsigned long c)       { print(c, PrintBase::Dec); }
+  FORCE_INLINE void print(long c)                { print(c, PrintBase::Dec); }
+  FORCE_INLINE void print(double c)              { print(c, 2); }
+
+  FORCE_INLINE void println(const char s[])                  { print(s); println(); }
+  FORCE_INLINE void println(char c, PrintBase base)          { print(c, base); println(); }
+  FORCE_INLINE void println(unsigned char c, PrintBase base) { print(c, base); println(); }
+  FORCE_INLINE void println(int c, PrintBase base)           { print(c, base); println(); }
+  FORCE_INLINE void println(unsigned int c, PrintBase base)  { print(c, base); println(); }
+  FORCE_INLINE void println(long c, PrintBase base)          { print(c, base); println(); }
+  FORCE_INLINE void println(unsigned long c, PrintBase base) { print(c, base); println(); }
+  FORCE_INLINE void println(double c, int digits)            { print(c, digits); println(); }
+  FORCE_INLINE void println()                                { write('\r'); write('\n'); }
+
+  // Forward the call to the former's method
+  FORCE_INLINE void println(char c)                { println(c, PrintBase::Dec); }
+  FORCE_INLINE void println(unsigned char c)       { println(c, PrintBase::Dec); }
+  FORCE_INLINE void println(int c)                 { println(c, PrintBase::Dec); }
+  FORCE_INLINE void println(unsigned int c)        { println(c, PrintBase::Dec); }
+  FORCE_INLINE void println(unsigned long c)       { println(c, PrintBase::Dec); }
+  FORCE_INLINE void println(long c)                { println(c, PrintBase::Dec); }
+  FORCE_INLINE void println(double c)              { println(c, 2); }
+
+  // Print a number with the given base
+  NO_INLINE void printNumber(unsigned long n, const uint8_t base) {
+    if (!base) return; // Hopefully, this should raise visible bug immediately
+
+    if (n) {
+      unsigned char buf[8 * sizeof(long)]; // Enough space for base 2
+      int8_t i = 0;
+      while (n) {
+        buf[i++] = n % base;
+        n /= base;
+      }
+      while (i--) write((char)(buf[i] + (buf[i] < 10 ? '0' : 'A' - 10)));
+    }
+    else write('0');
+  }
+  void printNumber(signed long n, const uint8_t base) {
+    if (base == 10 && n < 0) { 
+      n = -n; // This works because all platforms Marlin's builds on are using 2-complement encoding for negative number
+              // On such CPU, changing the sign of a number is done by inverting the bits and adding one, so if n = 0x80000000 = -2147483648 then
+              // -n = 0x7FFFFFFF + 1 => 0x80000000 = 2147483648 (if interpreted as unsigned) or -2147483648 if interpreted as signed.
+              // On non 2-complement CPU, there would be no possible representation for 2147483648.
+      write('-'); 
+    } 
+    printNumber((unsigned long)n , base);
+  }
+
+  // Print a decimal number
+  NO_INLINE void printFloat(double number, uint8_t digits) {
+    // Handle negative numbers
+    if (number < 0.0) {
+      write('-');
+      number = -number;
+    }
+
+    // Round correctly so that print(1.999, 2) prints as "2.00"
+    double rounding = 0.5;
+    LOOP_L_N(i, digits) rounding *= 0.1;
+    number += rounding;
+
+    // Extract the integer part of the number and print it
+    unsigned long int_part = (unsigned long)number;
+    double remainder = number - (double)int_part;
+    printNumber(int_part, 10);
+
+    // Print the decimal point, but only if there are digits beyond
+    if (digits) {
+      write('.');
+      // Extract digits from the remainder one at a time
+      while (digits--) {
+        remainder *= 10.0;
+        unsigned long toPrint = (unsigned long)remainder;
+        printNumber(toPrint, 10);
+        remainder -= toPrint;
+      }
+    }
+  }
+};
+
+// All serial instances will be built by chaining the features required
+// for the function in the form of a template type definition.
@@ -0,0 +1,240 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program 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.
+ *
+ * This program 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 this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+#include "macros.h"
+#include "serial_base.h"
+
+// The most basic serial class: it dispatch to the base serial class with no hook whatsoever. This will compile to nothing but the base serial class
+template <class SerialT>
+struct BaseSerial : public SerialBase< BaseSerial<SerialT> >, public SerialT {
+  typedef SerialBase< BaseSerial<SerialT> > BaseClassT;
+
+  // It's required to implement a write method here to help compiler disambiguate what method to call
+  using SerialT::write;
+  using SerialT::flush;
+
+  void msgDone() {}
+
+  bool available(uint8_t index) { return index == 0 && SerialT::available(); }
+  int read(uint8_t index)       { return index == 0 ? SerialT::read() : -1; }
+  bool connected()              { return CALL_IF_EXISTS(bool, static_cast<SerialT*>(this), connected);; }
+  void flushTX()                { CALL_IF_EXISTS(void, static_cast<SerialT*>(this), flushTX); }
+
+  // We have 2 implementation of the same method in both base class, let's say which one we want
+  using SerialT::available;
+  using SerialT::read;
+  using SerialT::begin;
+  using SerialT::end;
+
+  using BaseClassT::print;
+  using BaseClassT::println;
+
+  BaseSerial(const bool e) : BaseClassT(e) {}
+
+  // Forward constructor
+  template <typename... Args>
+  BaseSerial(const bool e, Args... args) : BaseClassT(e), SerialT(args...) {}
+};
+
+// A serial with a condition checked at runtime for its output
+// A bit less efficient than static dispatching but since it's only used for ethernet's serial output right now, it's ok.
+template <class SerialT>
+struct ConditionalSerial : public SerialBase< ConditionalSerial<SerialT> > {
+  typedef SerialBase< ConditionalSerial<SerialT> > BaseClassT;
+
+  bool    & condition;
+  SerialT & out;
+  NO_INLINE size_t write(uint8_t c) { if (condition) return out.write(c); return 0; }
+  void flush()            { if (condition) out.flush();  }
+  void begin(long br)     { out.begin(br); }
+  void end()              { out.end(); }
+
+  void msgDone() {}
+  bool connected()              { return CALL_IF_EXISTS(bool, &out, connected); }
+  void flushTX()                { CALL_IF_EXISTS(void, &out, flushTX); }
+
+  bool available(uint8_t index) { return index == 0 && out.available(); }
+  int read(uint8_t index)       { return index == 0 ? out.read() : -1; }
+  using BaseClassT::available;
+  using BaseClassT::read;
+
+  ConditionalSerial(bool & conditionVariable, SerialT & out, const bool e) : BaseClassT(e), condition(conditionVariable), out(out) {}
+};
+
+// A simple foward class that taking a reference to an existing serial instance (likely created in their respective framework)
+template <class SerialT>
+struct ForwardSerial : public SerialBase< ForwardSerial<SerialT> > {
+  typedef SerialBase< ForwardSerial<SerialT> > BaseClassT;
+
+  SerialT & out;
+  NO_INLINE size_t write(uint8_t c) { return out.write(c); }
+  void flush()            { out.flush();  }
+  void begin(long br)     { out.begin(br); }
+  void end()              { out.end(); }
+
+  void msgDone() {}
+  // Existing instances implement Arduino's operator bool, so use that if it's available
+  bool connected()              { return Private::HasMember_connected<SerialT>::value ? CALL_IF_EXISTS(bool, &out, connected) : (bool)out; }
+  void flushTX()                { CALL_IF_EXISTS(void, &out, flushTX); }
+
+  bool available(uint8_t index) { return index == 0 && out.available(); }
+  int read(uint8_t index)       { return index == 0 ? out.read() : -1; }
+  bool available()              { return out.available(); }
+  int read()                    { return out.read(); }
+
+  ForwardSerial(const bool e, SerialT & out) : BaseClassT(e), out(out) {}
+};
+
+// A class that's can be hooked and unhooked at runtime, useful to capturing the output of the serial interface
+template <class SerialT>
+struct RuntimeSerial : public SerialBase< RuntimeSerial<SerialT> >, public SerialT {
+  typedef SerialBase< RuntimeSerial<SerialT> > BaseClassT;
+  typedef void (*WriteHook)(void * userPointer, uint8_t c);
+  typedef void (*EndOfMessageHook)(void * userPointer);
+
+  WriteHook        writeHook;
+  EndOfMessageHook eofHook;
+  void *           userPointer;
+
+  NO_INLINE size_t write(uint8_t c) {
+    if (writeHook) writeHook(userPointer, c);
+    return SerialT::write(c);
+  }
+
+  NO_INLINE void msgDone() {
+    if (eofHook) eofHook(userPointer);
+  }
+
+  bool available(uint8_t index) { return index == 0 && SerialT::available(); }
+  int read(uint8_t index)       { return index == 0 ? SerialT::read() : -1; }
+  using SerialT::available;
+  using SerialT::read;
+  using SerialT::flush;
+  using SerialT::begin;
+  using SerialT::end;
+
+  using BaseClassT::print;
+  using BaseClassT::println;
+
+  // Underlying implementation might use Arduino's bool operator
+  bool connected() {
+    return Private::HasMember_connected<SerialT>::value ? CALL_IF_EXISTS(bool, static_cast<SerialT*>(this), connected) : static_cast<SerialT*>(this)->operator bool();
+  }
+  void flushTX()                { CALL_IF_EXISTS(void, static_cast<SerialT*>(this), flushTX); }
+
+  void setHook(WriteHook writeHook = 0, EndOfMessageHook eofHook = 0, void * userPointer = 0) {
+    // Order is important here as serial code can be called inside interrupts
+    // When setting a hook, the user pointer must be set first so if writeHook is called as soon as it's set, it'll be valid
+    if (userPointer) this->userPointer = userPointer;
+    this->writeHook = writeHook;
+    this->eofHook = eofHook;
+    // Order is important here because of asynchronous access here
+    // When unsetting a hook, the user pointer must be unset last so that any pending writeHook is still using the old pointer
+    if (!userPointer) this->userPointer = 0;
+  }
+
+  RuntimeSerial(const bool e) : BaseClassT(e), writeHook(0), eofHook(0), userPointer(0) {}
+
+  // Forward constructor
+  template <typename... Args>
+  RuntimeSerial(const bool e, Args... args) : BaseClassT(e), SerialT(args...) {}
+};
+
+// A class that's duplicating its output conditionally to 2 serial interface
+template <class Serial0T, class Serial1T, const uint8_t offset = 0>
+struct MultiSerial : public SerialBase< MultiSerial<Serial0T, Serial1T, offset> > {
+  typedef SerialBase< MultiSerial<Serial0T, Serial1T, offset> > BaseClassT;
+
+  uint8_t    portMask;
+  Serial0T & serial0;
+  Serial1T & serial1;
+
+  enum Masks {
+    FirstOutputMask   =  (1 << offset),
+    SecondOutputMask  =  (1 << (offset + 1)),
+    AllMask           = FirstOutputMask | SecondOutputMask,
+  };
+
+  NO_INLINE size_t write(uint8_t c) {
+    size_t ret = 0;
+    if (portMask & FirstOutputMask)   ret = serial0.write(c);
+    if (portMask & SecondOutputMask)  ret = serial1.write(c) | ret;
+    return ret;
+  }
+  NO_INLINE void msgDone() {
+    if (portMask & FirstOutputMask)   serial0.msgDone();
+    if (portMask & SecondOutputMask)  serial1.msgDone();
+  }
+  bool available(uint8_t index) {
+    switch(index) {
+      case 0 + offset: return serial0.available();
+      case 1 + offset: return serial1.available();
+      default: return false;
+    }
+  }
+  NO_INLINE int read(uint8_t index) {
+    switch(index) {
+      case 0 + offset: return serial0.read();
+      case 1 + offset: return serial1.read();
+      default: return -1;
+    }
+  }
+  void begin(const long br) {
+    if (portMask & FirstOutputMask)   serial0.begin(br);
+    if (portMask & SecondOutputMask)  serial1.begin(br);
+  }
+  void end() {
+    if (portMask & FirstOutputMask)   serial0.end();
+    if (portMask & SecondOutputMask)  serial1.end();
+  }
+  bool connected() {
+    bool ret = true;
+    if (portMask & FirstOutputMask)   ret = CALL_IF_EXISTS(bool, &serial0, connected);
+    if (portMask & SecondOutputMask)  ret = ret && CALL_IF_EXISTS(bool, &serial1, connected);
+    return ret;
+  }
+
+  using BaseClassT::available;
+  using BaseClassT::read;
+
+  // Redirect flush
+  NO_INLINE void flush()      {
+    if (portMask & FirstOutputMask)   serial0.flush();
+    if (portMask & SecondOutputMask)  serial1.flush();
+  }
+  NO_INLINE void flushTX()    {
+    if (portMask & FirstOutputMask)   CALL_IF_EXISTS(void, &serial0, flushTX);
+    if (portMask & SecondOutputMask)  CALL_IF_EXISTS(void, &serial1, flushTX);
+  }
+
+  MultiSerial(Serial0T & serial0, Serial1T & serial1, int8_t mask = AllMask, const bool e = false) :
+    BaseClassT(e),
+    portMask(mask), serial0(serial0), serial1(serial1) {}
+};
+
+// Build the actual serial object depending on current configuration
+#define Serial0Type TERN(SERIAL_RUNTIME_HOOK, RuntimeSerial, BaseSerial)
+#define ForwardSerial0Type TERN(SERIAL_RUNTIME_HOOK, RuntimeSerial, ForwardSerial)
+#ifdef HAS_MULTI_SERIAL
+  #define Serial1Type ConditionalSerial
+#endif
@@ -47,11 +47,11 @@ static void extrapolate_one_point(const uint8_t x, const uint8_t y, const int8_t
  if (DEBUGGING(LEVELING)) {
    DEBUG_ECHOPGM("Extrapolate [");
    if (x < 10) DEBUG_CHAR(' ');
-    DEBUG_ECHO((int)x);
+    DEBUG_ECHO(x);
    DEBUG_CHAR(xdir ? (xdir > 0 ? '+' : '-') : ' ');
    DEBUG_CHAR(' ');
    if (y < 10) DEBUG_CHAR(' ');
-    DEBUG_ECHO((int)y);
+    DEBUG_ECHO(y);
    DEBUG_CHAR(ydir ? (ydir > 0 ? '+' : '-') : ' ');
    DEBUG_ECHOLNPGM("]");
  }
@@ -160,7 +160,7 @@ void reset_bed_level() {
    #ifndef SCAD_MESH_OUTPUT
      LOOP_L_N(x, sx) {
        serial_spaces(precision + (x < 10 ? 3 : 2));
-        SERIAL_ECHO(int(x));
+        SERIAL_ECHO(x);
      }
      SERIAL_EOL();
    #endif
@@ -172,7 +172,7 @@ void reset_bed_level() {
        SERIAL_ECHOPGM(" [");             // open sub-array
      #else
        if (y < 10) SERIAL_CHAR(' ');
-        SERIAL_ECHO(int(y));
+        SERIAL_ECHO(y);
      #endif
      LOOP_L_N(x, sx) {
        SERIAL_CHAR(' ');
@@ -196,7 +196,7 @@ void reset_bed_level() {
        #endif
      }
      #ifdef SCAD_MESH_OUTPUT
-        SERIAL_CHAR(' ', ']');            // close sub-array
+        SERIAL_ECHOPGM(" ]");            // close sub-array
        if (y < sy - 1) SERIAL_CHAR(',');
      #endif
      SERIAL_EOL();
@@ -50,7 +50,7 @@
    GRID_LOOP(x, y)
      if (!isnan(z_values[x][y])) {
        SERIAL_ECHO_START();
-        SERIAL_ECHOPAIR("  M421 I", int(x), " J", int(y));
+        SERIAL_ECHOPAIR("  M421 I", x, " J", y);
        SERIAL_ECHOLNPAIR_F_P(SP_Z_STR, z_values[x][y], 4);
        serial_delay(75); // Prevent Printrun from exploding
      }
@@ -214,7 +214,7 @@
        else if (isnan(f))
          serialprintPGM(human ? PSTR("  .   ") : PSTR("NAN"));
        else if (human || csv) {
-          if (human && f >= 0.0) SERIAL_CHAR(f > 0 ? '+' : ' ');  // Space for positive ('-' for negative)
+          if (human && f >= 0.0) SERIAL_CHAR(f > 0 ? '+' : ' ');  // Display sign also for positive numbers (' ' for 0)
          SERIAL_ECHO_F(f, 3);                                    // Positive: 5 digits, Negative: 6 digits
        }
        if (csv && i < GRID_MAX_POINTS_X - 1) SERIAL_CHAR('\t');
@@ -742,7 +742,7 @@
        if (do_ubl_mesh_map) display_map(g29_map_type);

        const int point_num = (GRID_MAX_POINTS) - count + 1;
-        SERIAL_ECHOLNPAIR("\nProbing mesh point ", point_num, "/", int(GRID_MAX_POINTS), ".\n");
+        SERIAL_ECHOLNPAIR("Probing mesh point ", point_num, "/", GRID_MAX_POINTS, ".");
        TERN_(HAS_DISPLAY, ui.status_printf_P(0, PSTR(S_FMT " %i/%i"), GET_TEXT(MSG_PROBING_MESH), point_num, int(GRID_MAX_POINTS)));

        #if HAS_LCD_MENU
@@ -1694,7 +1694,7 @@
      SERIAL_EOL();

      #if HAS_KILL
-        SERIAL_ECHOLNPAIR("Kill pin on :", int(KILL_PIN), "  state:", int(kill_state()));
+        SERIAL_ECHOLNPAIR("Kill pin on :", KILL_PIN, "  state:", kill_state());
      #endif

      SERIAL_EOL();
@@ -1707,8 +1707,8 @@
        SERIAL_ECHOLNPAIR("Meshes go from ", hex_address((void*)settings.meshes_start_index()), " to ", hex_address((void*)settings.meshes_end_index()));
        serial_delay(50);

-        SERIAL_ECHOLNPAIR("sizeof(ubl) :  ", (int)sizeof(ubl));         SERIAL_EOL();
-        SERIAL_ECHOLNPAIR("z_value[][] size: ", (int)sizeof(z_values)); SERIAL_EOL();
+        SERIAL_ECHOLNPAIR("sizeof(ubl) :  ", sizeof(ubl));         SERIAL_EOL();
+        SERIAL_ECHOLNPAIR("z_value[][] size: ", sizeof(z_values)); SERIAL_EOL();
        serial_delay(25);

        SERIAL_ECHOLNPAIR("EEPROM free for UBL: ", hex_address((void*)(settings.meshes_end_index() - settings.meshes_start_index())));
@@ -30,23 +30,11 @@
 #endif

 inline bool bs_serial_data_available(const uint8_t index) {
-  switch (index) {
-    case 0: return MYSERIAL0.available();
-    #if HAS_MULTI_SERIAL
-      case 1: return MYSERIAL1.available();
-    #endif
-  }
-  return false;
+  return SERIAL_IMPL.available(index);
 }

 inline int bs_read_serial(const uint8_t index) {
-  switch (index) {
-    case 0: return MYSERIAL0.read();
-    #if HAS_MULTI_SERIAL
-      case 1: return MYSERIAL1.read();
-    #endif
-  }
-  return -1;
+  return SERIAL_IMPL.read(index);
 }

 #if ENABLED(BINARY_STREAM_COMPRESSION)
@@ -297,7 +285,7 @@ public:
    millis_t transfer_window = millis() + RX_TIMESLICE;

    #if ENABLED(SDSUPPORT)
-      PORT_REDIRECT(card.transfer_port_index);
+      PORT_REDIRECT(SERIAL_PORTMASK(card.transfer_port_index));
    #endif

    #pragma GCC diagnostic push
@@ -364,8 +352,7 @@ public:
              }
            }
            else {
-              SERIAL_ECHO_START();
-              SERIAL_ECHOLNPAIR("Packet header(", packet.header.sync, "?) corrupt");
+              SERIAL_ECHO_MSG("Packet header(", packet.header.sync, "?) corrupt");
              stream_state = StreamState::PACKET_RESEND;
            }
          }
@@ -399,8 +386,7 @@ public:
              stream_state = StreamState::PACKET_PROCESS;
            }
            else {
-              SERIAL_ECHO_START();
-              SERIAL_ECHOLNPAIR("Packet(", packet.header.sync, ") payload corrupt");
+              SERIAL_ECHO_MSG("Packet(", packet.header.sync, ") payload corrupt");
              stream_state = StreamState::PACKET_RESEND;
            }
          }
@@ -418,8 +404,7 @@ public:
          if (packet_retries < MAX_RETRIES || MAX_RETRIES == 0) {
            packet_retries++;
            stream_state = StreamState::PACKET_RESET;
-            SERIAL_ECHO_START();
-            SERIAL_ECHOLNPAIR("Resend request ", int(packet_retries));
+            SERIAL_ECHO_MSG("Resend request ", packet_retries);
            SERIAL_ECHOLNPAIR("rs", sync);
          }
          else
@@ -64,7 +64,7 @@ void BLTouch::init(const bool set_voltage/*=false*/) {
  #else

    if (DEBUGGING(LEVELING)) {
-      DEBUG_ECHOLNPAIR("last_written_mode - ", (int)last_written_mode);
+      DEBUG_ECHOLNPAIR("last_written_mode - ", last_written_mode);
      DEBUG_ECHOLNPGM("config mode - "
        #if ENABLED(BLTOUCH_SET_5V_MODE)
          "BLTOUCH_SET_5V_MODE"
@@ -175,7 +175,7 @@ bool BLTouch::status_proc() {
  _set_SW_mode();              // Incidentally, _set_SW_mode() will also RESET any active alarm
  const bool tr = triggered(); // If triggered in SW mode, the pin is up, it is STOWED

-  if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("BLTouch is ", (int)tr);
+  if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("BLTouch is ", tr);

  if (tr) _stow(); else _deploy();  // Turn off SW mode, reset any trigger, honor pin state
  return !tr;
@@ -187,7 +187,7 @@ void BLTouch::mode_conv_proc(const bool M5V) {
   * BLTOUCH V3.0: This will set the mode (twice) and sadly, a STOW is needed at the end, because of the deploy
   * BLTOUCH V3.1: This will set the mode and store it in the eeprom. The STOW is not needed but does not hurt
   */
-  if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("BLTouch Set Mode - ", (int)M5V);
+  if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("BLTouch Set Mode - ", M5V);
  _deploy();
  if (M5V) _set_5V_mode(); else _set_OD_mode();
  _mode_store();
@@ -66,10 +66,8 @@ void CancelObject::uncancel_object(const int8_t obj) {
 }

 void CancelObject::report() {
-  if (active_object >= 0) {
-    SERIAL_ECHO_START();
-    SERIAL_ECHOLNPAIR("Active Object: ", int(active_object));
-  }
+  if (active_object >= 0)
+    SERIAL_ECHO_MSG("Active Object: ", active_object);

  if (canceled) {
    SERIAL_ECHO_START();
@@ -180,7 +180,7 @@ namespace DirectStepping {
    if (!page_states_dirty) return;
    page_states_dirty = false;

-    SERIAL_ECHO(Cfg::CONTROL_CHAR);
+    SERIAL_CHAR(Cfg::CONTROL_CHAR);
    constexpr int state_bits = 2;
    constexpr int n_bytes = Cfg::NUM_PAGES >> state_bits;
    volatile uint8_t bits_b[n_bytes] = { 0 };
@@ -192,10 +192,10 @@ namespace DirectStepping {
    uint8_t crc = 0;
    for (uint8_t i = 0 ; i < n_bytes ; i++) {
      crc ^= bits_b[i];
-      SERIAL_ECHO(bits_b[i]);
+      SERIAL_CHAR(bits_b[i]);
    }

-    SERIAL_ECHO(crc);
+    SERIAL_CHAR(crc);
    SERIAL_EOL();
  }

@@ -33,7 +33,6 @@

 // External references
 extern bool wait_for_user, wait_for_heatup;
-void quickstop_stepper();

 class EmergencyParser {

@@ -41,13 +41,15 @@

 #include <Wire.h>

+I2CPositionEncodersMgr I2CPEM;
+
 void I2CPositionEncoder::init(const uint8_t address, const AxisEnum axis) {
  encoderAxis = axis;
  i2cAddress = address;

  initialized++;

-  SERIAL_ECHOLNPAIR("Setting up encoder on ", axis_codes[encoderAxis], " axis, addr = ", address);
+  SERIAL_ECHOLNPAIR("Setting up encoder on ", AS_CHAR(axis_codes[encoderAxis]), " axis, addr = ", address);

  position = get_position();
 }
@@ -65,7 +67,7 @@ void I2CPositionEncoder::update() {
    /*
    if (trusted) { //commented out as part of the note below
      trusted = false;
-      SERIAL_ECHOLMPAIR("Fault detected on ", axis_codes[encoderAxis], " axis encoder. Disengaging error correction until module is trusted again.");
+      SERIAL_ECHOLNPAIR("Fault detected on ", AS_CHAR(axis_codes[encoderAxis]), " axis encoder. Disengaging error correction until module is trusted again.");
    }
    */
    return;
@@ -90,7 +92,7 @@ void I2CPositionEncoder::update() {
      if (millis() - lastErrorTime > I2CPE_TIME_TRUSTED) {
        trusted = true;

-        SERIAL_ECHOLNPAIR("Untrusted encoder module on ", axis_codes[encoderAxis], " axis has been fault-free for set duration, reinstating error correction.");
+        SERIAL_ECHOLNPAIR("Untrusted encoder module on ", AS_CHAR(axis_codes[encoderAxis]), " axis has been fault-free for set duration, reinstating error correction.");

        //the encoder likely lost its place when the error occured, so we'll reset and use the printer's
        //idea of where it the axis is to re-initialize
@@ -170,7 +172,7 @@ void I2CPositionEncoder::update() {
            float sumP = 0;
            LOOP_L_N(i, I2CPE_ERR_PRST_ARRAY_SIZE) sumP += errPrst[i];
            const int32_t errorP = int32_t(sumP * RECIPROCAL(I2CPE_ERR_PRST_ARRAY_SIZE));
-            SERIAL_ECHO(axis_codes[encoderAxis]);
+            SERIAL_CHAR(axis_codes[encoderAxis]);
            SERIAL_ECHOLNPAIR(" : CORRECT ERR ", errorP * planner.steps_to_mm[encoderAxis], "mm");
            babystep.add_steps(encoderAxis, -LROUND(errorP));
            errPrstIdx = 0;
@@ -190,8 +192,8 @@ void I2CPositionEncoder::update() {
    if (ABS(error) > I2CPE_ERR_CNT_THRESH * planner.settings.axis_steps_per_mm[encoderAxis]) {
      const millis_t ms = millis();
      if (ELAPSED(ms, nextErrorCountTime)) {
-        SERIAL_ECHO(axis_codes[encoderAxis]);
-        SERIAL_ECHOLNPAIR(" : LARGE ERR ", int(error), "; diffSum=", diffSum);
+        SERIAL_CHAR(axis_codes[encoderAxis]);
+        SERIAL_ECHOLNPAIR(" : LARGE ERR ", error, "; diffSum=", diffSum);
        errorCount++;
        nextErrorCountTime = ms + I2CPE_ERR_CNT_DEBOUNCE_MS;
      }
@@ -211,7 +213,7 @@ void I2CPositionEncoder::set_homed() {
    trusted++;

    #ifdef I2CPE_DEBUG
-      SERIAL_ECHO(axis_codes[encoderAxis]);
+      SERIAL_CHAR(axis_codes[encoderAxis]);
      SERIAL_ECHOLNPAIR(" axis encoder homed, offset of ", zeroOffset, " ticks.");
    #endif
  }
@@ -222,7 +224,7 @@ void I2CPositionEncoder::set_unhomed() {
  homed = trusted = false;

  #ifdef I2CPE_DEBUG
-    SERIAL_ECHO(axis_codes[encoderAxis]);
+    SERIAL_CHAR(axis_codes[encoderAxis]);
    SERIAL_ECHOLNPGM(" axis encoder unhomed.");
  #endif
 }
@@ -230,7 +232,7 @@ void I2CPositionEncoder::set_unhomed() {
 bool I2CPositionEncoder::passes_test(const bool report) {
  if (report) {
    if (H != I2CPE_MAG_SIG_GOOD) SERIAL_ECHOPGM("Warning. ");
-    SERIAL_ECHO(axis_codes[encoderAxis]);
+    SERIAL_CHAR(axis_codes[encoderAxis]);
    serial_ternary(H == I2CPE_MAG_SIG_BAD, PSTR(" axis "), PSTR("magnetic strip "), PSTR("encoder "));
    switch (H) {
      case I2CPE_MAG_SIG_GOOD:
@@ -251,7 +253,7 @@ float I2CPositionEncoder::get_axis_error_mm(const bool report) {
              error = ABS(diff) > 10000 ? 0 : diff; // Huge error is a bad reading

  if (report) {
-    SERIAL_ECHO(axis_codes[encoderAxis]);
+    SERIAL_CHAR(axis_codes[encoderAxis]);
    SERIAL_ECHOLNPAIR(" axis target=", target, "mm; actual=", actual, "mm; err=", error, "mm");
  }

@@ -261,7 +263,7 @@ float I2CPositionEncoder::get_axis_error_mm(const bool report) {
 int32_t I2CPositionEncoder::get_axis_error_steps(const bool report) {
  if (!active) {
    if (report) {
-      SERIAL_ECHO(axis_codes[encoderAxis]);
+      SERIAL_CHAR(axis_codes[encoderAxis]);
      SERIAL_ECHOLNPGM(" axis encoder not active!");
    }
    return 0;
@@ -286,7 +288,7 @@ int32_t I2CPositionEncoder::get_axis_error_steps(const bool report) {
  errorPrev = error;

  if (report) {
-    SERIAL_ECHO(axis_codes[encoderAxis]);
+    SERIAL_CHAR(axis_codes[encoderAxis]);
    SERIAL_ECHOLNPAIR(" axis target=", target, "; actual=", encoderCountInStepperTicksScaled, "; err=", error);
  }

@@ -665,8 +667,7 @@ void I2CPositionEncodersMgr::report_position(const int8_t idx, const bool units,
  else {
    if (noOffset) {
      const int32_t raw_count = encoders[idx].get_raw_count();
-      SERIAL_ECHO(axis_codes[encoders[idx].get_axis()]);
-      SERIAL_CHAR(' ');
+      SERIAL_CHAR(axis_codes[encoders[idx].get_axis()], ' ');

      for (uint8_t j = 31; j > 0; j--)
        SERIAL_ECHO((bool)(0x00000001 & (raw_count >> j)));
@@ -721,7 +722,7 @@ void I2CPositionEncodersMgr::change_module_address(const uint8_t oldaddr, const
  // and enable it (it will likely have failed initialization on power-up, before the address change).
  const int8_t idx = idx_from_addr(newaddr);
  if (idx >= 0 && !encoders[idx].get_active()) {
-    SERIAL_ECHO(axis_codes[encoders[idx].get_axis()]);
+    SERIAL_CHAR(axis_codes[encoders[idx].get_axis()]);
    SERIAL_ECHOLNPGM(" axis encoder was not detected on printer startup. Trying again.");
    encoders[idx].set_active(encoders[idx].passes_test(true));
  }
@@ -746,7 +747,7 @@ void I2CPositionEncodersMgr::report_module_firmware(const uint8_t address) {
  if (Wire.requestFrom(I2C_ADDRESS(address), uint8_t(32))) {
    char c;
    while (Wire.available() > 0 && (c = (char)Wire.read()) > 0)
-      SERIAL_ECHO(c);
+      SERIAL_CHAR(c);
    SERIAL_EOL();
  }

@@ -261,32 +261,32 @@ class I2CPositionEncodersMgr {

    static void report_error_count(const int8_t idx, const AxisEnum axis) {
      CHECK_IDX();
-      SERIAL_ECHOLNPAIR("Error count on ", axis_codes[axis], " axis is ", encoders[idx].get_error_count());
+      SERIAL_ECHOLNPAIR("Error count on ", AS_CHAR(axis_codes[axis]), " axis is ", encoders[idx].get_error_count());
    }

    static void reset_error_count(const int8_t idx, const AxisEnum axis) {
      CHECK_IDX();
      encoders[idx].set_error_count(0);
-      SERIAL_ECHOLNPAIR("Error count on ", axis_codes[axis], " axis has been reset.");
+      SERIAL_ECHOLNPAIR("Error count on ", AS_CHAR(axis_codes[axis]), " axis has been reset.");
    }

    static void enable_ec(const int8_t idx, const bool enabled, const AxisEnum axis) {
      CHECK_IDX();
      encoders[idx].set_ec_enabled(enabled);
-      SERIAL_ECHOPAIR("Error correction on ", axis_codes[axis]);
+      SERIAL_ECHOPAIR("Error correction on ", AS_CHAR(axis_codes[axis]));
      SERIAL_ECHO_TERNARY(encoders[idx].get_ec_enabled(), " axis is ", "en", "dis", "abled.\n");
    }

    static void set_ec_threshold(const int8_t idx, const float newThreshold, const AxisEnum axis) {
      CHECK_IDX();
      encoders[idx].set_ec_threshold(newThreshold);
-      SERIAL_ECHOLNPAIR("Error correct threshold for ", axis_codes[axis], " axis set to ", newThreshold, "mm.");
+      SERIAL_ECHOLNPAIR("Error correct threshold for ", AS_CHAR(axis_codes[axis]), " axis set to ", newThreshold, "mm.");
    }

    static void get_ec_threshold(const int8_t idx, const AxisEnum axis) {
      CHECK_IDX();
      const float threshold = encoders[idx].get_ec_threshold();
-      SERIAL_ECHOLNPAIR("Error correct threshold for ", axis_codes[axis], " axis is ", threshold, "mm.");
+      SERIAL_ECHOLNPAIR("Error correct threshold for ", AS_CHAR(axis_codes[axis]), " axis is ", threshold, "mm.");
    }

    static int8_t idx_from_axis(const AxisEnum axis) {
@@ -38,7 +38,7 @@
 #endif

 void host_action(PGM_P const pstr, const bool eol) {
-  PORT_REDIRECT(SERIAL_BOTH);
+  PORT_REDIRECT(SERIAL_ALL);
  SERIAL_ECHOPGM("//action:");
  serialprintPGM(pstr);
  if (eol) SERIAL_EOL();
@@ -78,20 +78,20 @@ void host_action(PGM_P const pstr, const bool eol) {
  PromptReason host_prompt_reason = PROMPT_NOT_DEFINED;

  void host_action_notify(const char * const message) {
-    PORT_REDIRECT(SERIAL_BOTH);
+    PORT_REDIRECT(SERIAL_ALL);
    host_action(PSTR("notification "), false);
    SERIAL_ECHOLN(message);
  }

  void host_action_notify_P(PGM_P const message) {
-    PORT_REDIRECT(SERIAL_BOTH);
+    PORT_REDIRECT(SERIAL_ALL);
    host_action(PSTR("notification "), false);
    serialprintPGM(message);
    SERIAL_EOL();
  }

  void host_action_prompt(PGM_P const ptype, const bool eol=true) {
-    PORT_REDIRECT(SERIAL_BOTH);
+    PORT_REDIRECT(SERIAL_ALL);
    host_action(PSTR("prompt_"), false);
    serialprintPGM(ptype);
    if (eol) SERIAL_EOL();
@@ -99,7 +99,7 @@ void host_action(PGM_P const pstr, const bool eol) {

  void host_action_prompt_plus(PGM_P const ptype, PGM_P const pstr, const char extra_char='\0') {
    host_action_prompt(ptype, false);
-    PORT_REDIRECT(SERIAL_BOTH);
+    PORT_REDIRECT(SERIAL_ALL);
    SERIAL_CHAR(' ');
    serialprintPGM(pstr);
    if (extra_char != '\0') SERIAL_CHAR(extra_char);
@@ -182,7 +182,7 @@ void host_action(PGM_P const pstr, const bool eol) {
        break;
      case PROMPT_PAUSE_RESUME:
        msg = PSTR("LCD_PAUSE_RESUME");
-        #if ENABLED(ADVANCED_PAUSE_FEATURE)
+        #if BOTH(ADVANCED_PAUSE_FEATURE, SDSUPPORT)
          extern const char M24_STR[];
          queue.inject_P(M24_STR);
        #endif
@@ -147,11 +147,13 @@ void LEDLights::set_color(const LEDColor &incol
  millis_t LEDLights::led_off_time; // = 0

  void LEDLights::update_timeout(const bool power_on) {
-    const millis_t ms = millis();
-    if (power_on)
-      reset_timeout(ms);
-    else if (ELAPSED(ms, led_off_time))
-      set_off();
+    if (lights_on) {
+      const millis_t ms = millis();
+      if (power_on)
+        reset_timeout(ms);
+      else if (ELAPSED(ms, led_off_time))
+        set_off();
+    }
  }

 #endif
@@ -203,7 +203,7 @@ public:
    public:
      static inline void reset_timeout(const millis_t &ms) {
        led_off_time = ms + LED_BACKLIGHT_TIMEOUT;
-        if (!lights_on) set_default();
+        if (!lights_on) update();
      }
      static void update_timeout(const bool power_on);
  #endif
@@ -0,0 +1,228 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program 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.
+ *
+ * This program 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 this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+/**
+ * MeatPack G-code Compression
+ *
+ * Algorithm & Implementation: Scott Mudge - mail@scottmudge.com
+ * Date: Dec. 2020
+ *
+ * Character Frequencies from ~30 MB of comment-stripped gcode:
+ *  '1' -> 4451136    '4' -> 1353273   '\n' -> 1087683    '-' ->   90242
+ *  '0' -> 4253577    '9' -> 1352147    'G' -> 1075806    'Z' ->   34109
+ *  ' ' -> 3053297    '3' -> 1262929    'X' ->  975742    'M' ->   11879
+ *  '.' -> 3035310    '5' -> 1189871    'E' ->  965275    'S' ->    9910
+ *  '2' -> 1523296    '6' -> 1127900    'Y' ->  965274
+ *  '8' -> 1366812    '7' -> 1112908    'F' ->   99416
+ *
+ * When space is omitted the letter 'E' is used in its place
+ */
+
+#include "../inc/MarlinConfig.h"
+
+#if ENABLED(MEATPACK)
+
+#include "meatpack.h"
+MeatPack meatpack;
+
+#define MeatPack_ProtocolVersion "PV01"
+//#define MP_DEBUG
+
+#define DEBUG_OUT ENABLED(MP_DEBUG)
+#include "../core/debug_out.h"
+
+bool MeatPack::cmd_is_next = false;       // A command is pending
+uint8_t MeatPack::state = 0;              // Configuration state OFF
+uint8_t MeatPack::second_char = 0;        // The unpacked 2nd character from an out-of-sequence packed pair
+uint8_t MeatPack::cmd_count = 0,          // Counts how many command bytes are received (need 2)
+        MeatPack::full_char_count = 0,    // Counts how many full-width characters are to be received
+        MeatPack::char_out_count = 0;     // Stores number of characters to be read out.
+uint8_t MeatPack::char_out_buf[2];        // Output buffer for caching up to 2 characters
+
+// The 15 most-common characters used in G-code, ~90-95% of all G-code uses these characters
+// Stored in SRAM for performance.
+uint8_t meatPackLookupTable[16] = {
+  '0', '1', '2', '3', '4', '5', '6', '7', '8', '9',
+  '.', ' ', '\n', 'G', 'X',
+  '\0' // Unused. 0b1111 indicates a literal character
+};
+
+TERN_(MP_DEBUG, uint8_t chars_decoded = 0); // Log the first 64 bytes after each reset
+
+void MeatPack::reset_state() {
+  state = 0;
+  cmd_is_next = false;
+  second_char = 0;
+  cmd_count = full_char_count = char_out_count = 0;
+  TERN_(MP_DEBUG, chars_decoded = 0);
+}
+
+/**
+ * Unpack one or two characters from a packed byte into a buffer.
+ * Return flags indicating whether any literal bytes follow.
+ */
+uint8_t MeatPack::unpack_chars(const uint8_t pk, uint8_t* __restrict const chars_out) {
+  uint8_t out = 0;
+
+  // If lower nybble is 1111, the higher nybble is unused, and next char is full.
+  if ((pk & kFirstNotPacked) == kFirstNotPacked)
+    out = kFirstCharIsLiteral;
+  else {
+    const uint8_t chr = pk & 0x0F;
+    chars_out[0] = meatPackLookupTable[chr]; // Set the first char
+  }
+
+  // Check if upper nybble is 1111... if so, we don't need the second char.
+  if ((pk & kSecondNotPacked) == kSecondNotPacked)
+    out |= kSecondCharIsLiteral;
+  else {
+    const uint8_t chr = (pk >> 4) & 0x0F;
+    chars_out[1] = meatPackLookupTable[chr]; // Set the second char
+  }
+
+  return out;
+}
+
+/**
+ * Interpret a single (non-command) character
+ * according to the current MeatPack state.
+ */
+void MeatPack::handle_rx_char_inner(const uint8_t c) {
+  if (TEST(state, MPConfig_Bit_Active)) {                   // Is MeatPack active?
+    if (!full_char_count) {                                 // No literal characters to fetch?
+      uint8_t buf[2] = { 0, 0 };
+      register const uint8_t res = unpack_chars(c, buf);    // Decode the byte into one or two characters.
+      if (res & kFirstCharIsLiteral) {                      // The 1st character couldn't be packed.
+        ++full_char_count;                                  // So the next stream byte is a full character.
+        if (res & kSecondCharIsLiteral) ++full_char_count;  // The 2nd character couldn't be packed. Another stream byte is a full character.
+        else second_char = buf[1];                          // Retain the unpacked second character.
+      }
+      else {
+        handle_output_char(buf[0]);                           // Send the unpacked first character out.
+        if (buf[0] != '\n') {                                 // After a newline the next char won't be set
+          if (res & kSecondCharIsLiteral) ++full_char_count;  // The 2nd character couldn't be packed. The next stream byte is a full character.
+          else handle_output_char(buf[1]);                    // Send the unpacked second character out.
+        }
+      }
+    }
+    else {
+      handle_output_char(c);                                // Pass through the character that couldn't be packed...
+      if (second_char) {
+        handle_output_char(second_char);                    // ...and send an unpacked 2nd character, if set.
+        second_char = 0;
+      }
+      --full_char_count;                                    // One literal character was consumed
+    }
+  }
+  else // Packing not enabled, just copy character to output
+    handle_output_char(c);
+}
+
+/**
+ * Buffer a single output character which will be picked up in
+ * GCodeQueue::get_serial_commands via calls to get_result_char
+ */
+void MeatPack::handle_output_char(const uint8_t c) {
+  char_out_buf[char_out_count++] = c;
+
+  #if ENABLED(MP_DEBUG)
+    if (chars_decoded < 1024) {
+      ++chars_decoded;
+      DEBUG_ECHOPGM("RB: ");
+      MYSERIAL.print((char)c);
+      DEBUG_EOL();
+    }
+  #endif
+}
+
+/**
+ * Process a MeatPack command byte to update the state.
+ * Report the new state to serial.
+ */
+void MeatPack::handle_command(const MeatPack_Command c) {
+  switch (c) {
+    case MPCommand_QueryConfig:     break;
+    case MPCommand_EnablePacking:   SBI(state, MPConfig_Bit_Active);   DEBUG_ECHOLNPGM("[MPDBG] ENA REC");   break;
+    case MPCommand_DisablePacking:  CBI(state, MPConfig_Bit_Active);   DEBUG_ECHOLNPGM("[MPDBG] DIS REC");   break;
+    case MPCommand_ResetAll:        reset_state();                     DEBUG_ECHOLNPGM("[MPDBG] RESET REC"); break;
+    case MPCommand_EnableNoSpaces:
+      SBI(state, MPConfig_Bit_NoSpaces);
+      meatPackLookupTable[kSpaceCharIdx] = kSpaceCharReplace;          DEBUG_ECHOLNPGM("[MPDBG] ENA NSP");   break;
+    case MPCommand_DisableNoSpaces:
+      CBI(state, MPConfig_Bit_NoSpaces);
+      meatPackLookupTable[kSpaceCharIdx] = ' ';                        DEBUG_ECHOLNPGM("[MPDBG] DIS NSP");   break;
+    default:                                                           DEBUG_ECHOLNPGM("[MPDBG] UNK CMD REC");
+  }
+  report_state();
+}
+
+void MeatPack::report_state() {
+  // NOTE: if any configuration vars are added below, the outgoing sync text for host plugin
+  // should not contain the "PV' substring, as this is used to indicate protocol version
+  SERIAL_ECHOPGM("[MP] ");
+  SERIAL_ECHOPGM(MeatPack_ProtocolVersion " ");
+  serialprint_onoff(TEST(state, MPConfig_Bit_Active));
+  serialprintPGM(TEST(state, MPConfig_Bit_NoSpaces) ? PSTR(" NSP\n") : PSTR(" ESP\n"));
+}
+
+/**
+ * Interpret a single character received from serial
+ * according to the current meatpack state.
+ */
+void MeatPack::handle_rx_char(const uint8_t c, const serial_index_t serial_ind) {
+  if (c == kCommandByte) {                // A command (0xFF) byte?
+    if (cmd_count) {                      // In fact, two in a row?
+      cmd_is_next = true;                 // Then a MeatPack command follows
+      cmd_count = 0;
+    }
+    else
+      ++cmd_count; // cmd_count = 1       // One command byte received so far...
+    return;
+  }
+
+  if (cmd_is_next) {                      // Were two command bytes received?
+    PORT_REDIRECT(serial_ind);
+    handle_command((MeatPack_Command)c);  // Then the byte is a MeatPack command
+    cmd_is_next = false;
+    return;
+  }
+
+  if (cmd_count) {                        // Only a single 0xFF was received
+    handle_rx_char_inner(kCommandByte);   // A single 0xFF is passed on literally so it can be interpreted as kFirstNotPacked|kSecondNotPacked
+    cmd_count = 0;
+  }
+
+  handle_rx_char_inner(c);                // Other characters are passed on for MeatPack decoding
+}
+
+uint8_t MeatPack::get_result_char(char* const __restrict out) {
+  uint8_t res = 0;
+  if (char_out_count) {
+    res = char_out_count;
+    char_out_count = 0;
+    for (register uint8_t i = 0; i < res; ++i)
+      out[i] = (char)char_out_buf[i];
+  }
+  return res;
+}
+
+#endif // MEATPACK
@@ -0,0 +1,123 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program 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.
+ *
+ * This program 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 this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+/*
+ * MeatPack G-code Compression
+ *
+ * Algorithm & Implementation: Scott Mudge - mail@scottmudge.com
+ * Date: Dec. 2020
+ *
+ * Specifically optimized for 3D printing G-Code, this is a zero-cost data compression method
+ * which packs ~180-190% more data into the same amount of bytes going to the CNC controller.
+ * As a majority of G-Code can be represented by a restricted alphabet, I performed histogram
+ * analysis on a wide variety of 3D printing gcode samples, and found ~93% of all gcode could
+ * be represented by the same 15-character alphabet.
+ *
+ * This allowed me to design a system of packing 2 8-bit characters into a single byte, assuming
+ * they fall within this limited 15-character alphabet. Using a 4-bit lookup table, these 8-bit
+ * characters can be represented by a 4-bit index.
+ *
+ * Combined with some logic to allow commingling of full-width characters outside of this 15-
+ * character alphabet (at the cost of an extra 8-bits per full-width character), and by stripping
+ * out unnecessary comments, the end result is gcode which is roughly half the original size.
+ *
+ * Why did I do this? I noticed micro-stuttering and other data-bottleneck issues while printing
+ * objects with high curvature, especially at high speeds. There is also the issue of the limited
+ * baud rate provided by Prusa's Atmega2560-based boards, over the USB serial connection. So soft-
+ * ware like OctoPrint would also suffer this same micro-stuttering and poor print quality issue.
+ *
+ */
+#pragma once
+
+#include <stdint.h>
+
+/**
+ * Commands sent to MeatPack to control its behavior.
+ * They are sent by first sending 2x MeatPack_CommandByte (0xFF) in sequence,
+ *      followed by one of the command bytes below.
+ * Provided that 0xFF is an exceedingly rare character that is virtually never
+ * present in G-code naturally, it is safe to assume 2 in sequence should never
+ * happen naturally, and so it is used as a signal here.
+ *
+ * 0xFF *IS* used in "packed" G-code (used to denote that the next 2 characters are
+ * full-width), however 2 in a row will never occur, as the next 2 bytes will always
+ * some non-0xFF character.
+ */
+enum MeatPack_Command : uint8_t {
+  MPCommand_None            = 0,
+  MPCommand_EnablePacking   = 0xFB,
+  MPCommand_DisablePacking  = 0xFA,
+  MPCommand_ResetAll        = 0xF9,
+  MPCommand_QueryConfig     = 0xF8,
+  MPCommand_EnableNoSpaces  = 0xF7,
+  MPCommand_DisableNoSpaces = 0xF6
+};
+
+enum MeatPack_ConfigStateBits : uint8_t {
+  MPConfig_Bit_Active   = 0,
+  MPConfig_Bit_NoSpaces = 1
+};
+
+class MeatPack {
+private:
+  friend class GCodeQueue;
+
+  // Utility definitions
+  static const uint8_t kCommandByte         = 0b11111111,
+                       kFirstNotPacked      = 0b00001111,
+                       kSecondNotPacked     = 0b11110000,
+                       kFirstCharIsLiteral  = 0b00000001,
+                       kSecondCharIsLiteral = 0b00000010;
+
+  static const uint8_t kSpaceCharIdx = 11;
+  static const char kSpaceCharReplace = 'E';
+
+  static bool cmd_is_next;        // A command is pending
+  static uint8_t state;           // Configuration state
+  static uint8_t second_char;     // Buffers a character if dealing with out-of-sequence pairs
+  static uint8_t cmd_count,       // Counter of command bytes received (need 2)
+                 full_char_count, // Counter for full-width characters to be received
+                 char_out_count;  // Stores number of characters to be read out.
+  static uint8_t char_out_buf[2]; // Output buffer for caching up to 2 characters
+
+  // Pass in a character rx'd by SD card or serial. Automatically parses command/ctrl sequences,
+  // and will control state internally.
+  static void handle_rx_char(const uint8_t c, const serial_index_t serial_ind);
+
+  /**
+   * After passing in rx'd char using above method, call this to get characters out.
+   * Can return from 0 to 2 characters at once.
+   * @param out [in] Output pointer for unpacked/processed data.
+   * @return Number of characters returned. Range from 0 to 2.
+   */
+  static uint8_t get_result_char(char* const __restrict out);
+
+  static void reset_state();
+  static void report_state();
+  static uint8_t unpacked_char(register const uint8_t in);
+  static uint8_t unpack_chars(const uint8_t pk, uint8_t* __restrict const chars_out);
+  static void handle_command(const MeatPack_Command c);
+  static void handle_output_char(const uint8_t c);
+  static void handle_rx_char_inner(const uint8_t c);
+};
+
+extern MeatPack meatpack;
@@ -135,11 +135,11 @@ void Mixer::refresh_collector(const float proportion/*=1.0*/, const uint8_t t/*=
    cmax = _MAX(cmax, v);
    csum += v;
  }
-  //SERIAL_ECHOPAIR("Mixer::refresh_collector(", proportion, ", ", int(t), ") cmax=", cmax, "  csum=", csum, "  color");
+  //SERIAL_ECHOPAIR("Mixer::refresh_collector(", proportion, ", ", t, ") cmax=", cmax, "  csum=", csum, "  color");
  const float inv_prop = proportion / csum;
  MIXER_STEPPER_LOOP(i) {
    c[i] = color[t][i] * inv_prop;
-    //SERIAL_ECHOPAIR(" [", int(t), "][", int(i), "] = ", int(color[t][i]), " (", c[i], ")  ");
+    //SERIAL_ECHOPAIR(" [", t, "][", i, "] = ", color[t][i], " (", c[i], ")  ");
  }
  //SERIAL_EOL();
 }
@@ -139,9 +139,9 @@ class Mixer {

      #ifdef MIXER_NORMALIZER_DEBUG
        SERIAL_ECHOPGM("Mix [ ");
-        SERIAL_ECHOLIST_N(MIXING_STEPPERS, int(mix[0]), int(mix[1]), int(mix[2]), int(mix[3]), int(mix[4]), int(mix[5]));
+        SERIAL_ECHOLIST_N(MIXING_STEPPERS, mix[0], mix[1], mix[2], mix[3], mix[4], mix[5]);
        SERIAL_ECHOPGM(" ] to Color [ ");
-        SERIAL_ECHOLIST_N(MIXING_STEPPERS, int(tcolor[0]), int(tcolor[1]), int(tcolor[2]), int(tcolor[3]), int(tcolor[4]), int(tcolor[5]));
+        SERIAL_ECHOLIST_N(MIXING_STEPPERS, tcolor[0], tcolor[1], tcolor[2], tcolor[3], tcolor[4], tcolor[5]);
        SERIAL_ECHOLNPGM(" ]");
      #endif
    }
@@ -153,10 +153,10 @@ class Mixer {
      MIXER_STEPPER_LOOP(i) mix[i] = mixer_perc_t(100.0f * color[j][i] / ctot);

      #ifdef MIXER_NORMALIZER_DEBUG
-        SERIAL_ECHOPAIR("V-tool ", int(j), " [ ");
-        SERIAL_ECHOLIST_N(MIXING_STEPPERS, int(color[j][0]), int(color[j][1]), int(color[j][2]), int(color[j][3]), int(color[j][4]), int(color[j][5]));
+        SERIAL_ECHOPAIR("V-tool ", j, " [ ");
+        SERIAL_ECHOLIST_N(MIXING_STEPPERS, color[j][0], color[j][1], color[j][2], color[j][3], color[j][4], color[j][5]);
        SERIAL_ECHOPGM(" ] to Mix [ ");
-        SERIAL_ECHOLIST_N(MIXING_STEPPERS, int(mix[0]), int(mix[1]), int(mix[2]), int(mix[3]), int(mix[4]), int(mix[5]));
+        SERIAL_ECHOLIST_N(MIXING_STEPPERS, mix[0], mix[1], mix[2], mix[3], mix[4], mix[5]);
        SERIAL_ECHOLNPGM(" ]");
      #endif
    }
@@ -199,9 +199,9 @@ class Mixer {

      #ifdef MIXER_NORMALIZER_DEBUG
        SERIAL_ECHOPGM("Gradient [ ");
-        SERIAL_ECHOLIST_N(MIXING_STEPPERS, int(gradient.color[0]), int(gradient.color[1]), int(gradient.color[2]), int(gradient.color[3]), int(gradient.color[4]), int(gradient.color[5]));
+        SERIAL_ECHOLIST_N(MIXING_STEPPERS, gradient.color[0], gradient.color[1], gradient.color[2], gradient.color[3], gradient.color[4], gradient.color[5]);
        SERIAL_ECHOPGM(" ] to Mix [ ");
-        SERIAL_ECHOLIST_N(MIXING_STEPPERS, int(mix[0]), int(mix[1]), int(mix[2]), int(mix[3]), int(mix[4]), int(mix[5]));
+        SERIAL_ECHOLIST_N(MIXING_STEPPERS, mix[0], mix[1], mix[2], mix[3], mix[4], mix[5]);
        SERIAL_ECHOLNPGM(" ]");
      #endif
    }
@@ -167,6 +167,8 @@ void MMU2::mmu_loop() {

    case -1:
      if (rx_start()) {
+        prev_P0_request = millis();   // Initialize finda sensor timeout
+
        DEBUG_ECHOLNPGM("MMU => 'start'");
        DEBUG_ECHOLNPGM("MMU <= 'S1'");

@@ -311,7 +313,7 @@ void MMU2::mmu_loop() {
        // if (finda_runout_valid) DEBUG_ECHOLNPAIR_F("MMU <= 'P0'\nMMU => ", finda, 6);

        if (!finda && finda_runout_valid) filament_runout();
-        if (cmd == 0) ready = true;
+        if (cmd == MMU_CMD_NONE) ready = true;
        state = 1;
      }
      else if (ELAPSED(millis(), prev_request + MMU_P0_TIMEOUT)) // Resend request after timeout (3s)
@@ -333,18 +335,20 @@ void MMU2::mmu_loop() {
      #endif

      if (rx_ok()) {
-        // Response to C0 mmu command in MMU2S model
-        bool can_reset = true;
        #if HAS_PRUSA_MMU2S
-          if (!mmu2s_triggered && last_cmd == MMU_CMD_C0) {
-            can_reset = false;
+          // Respond to C0 MMU command in MMU2S model
+          const bool keep_trying = !mmu2s_triggered && last_cmd == MMU_CMD_C0;
+          if (keep_trying) {
            // MMU ok received but filament sensor not triggered, retrying...
            DEBUG_ECHOLNPGM("MMU => 'ok' (filament not present in gears)");
            DEBUG_ECHOLNPGM("MMU <= 'C0' (keep trying)");
            MMU2_COMMAND("C0");
          }
+        #else
+          constexpr bool keep_trying = false;
        #endif
-        if (can_reset) {
+
+        if (!keep_trying) {
          DEBUG_ECHOLNPGM("MMU => 'ok'");
          ready = true;
          state = 1;
@@ -370,11 +374,7 @@ void MMU2::mmu_loop() {
 */
 bool MMU2::rx_start() {
  // check for start message
-  if (rx_str_P(PSTR("start\n"))) {
-    prev_P0_request = millis();
-    return true;
-  }
-  return false;
+  return rx_str_P(PSTR("start\n"));
 }

 /**
@@ -385,13 +385,13 @@ bool MMU2::rx_str_P(const char* str) {

  while (MMU2_SERIAL.available()) {
    rx_buffer[i++] = MMU2_SERIAL.read();
-    rx_buffer[i] = '\0';

    if (i == sizeof(rx_buffer) - 1) {
      DEBUG_ECHOLNPGM("rx buffer overrun");
      break;
    }
  }
+  rx_buffer[i] = '\0';

  uint8_t len = strlen_P(str);

@@ -416,7 +416,6 @@ void MMU2::tx_str_P(const char* str) {
  clear_rx_buffer();
  uint8_t len = strlen_P(str);
  LOOP_L_N(i, len) MMU2_SERIAL.write(pgm_read_byte(str++));
-  rx_buffer[0] = '\0';
  prev_request = millis();
 }

@@ -427,7 +426,6 @@ void MMU2::tx_printf_P(const char* format, int argument = -1) {
  clear_rx_buffer();
  uint8_t len = sprintf_P(tx_buffer, format, argument);
  LOOP_L_N(i, len) MMU2_SERIAL.write(tx_buffer[i]);
-  rx_buffer[0] = '\0';
  prev_request = millis();
 }

@@ -438,7 +436,6 @@ void MMU2::tx_printf_P(const char* format, int argument1, int argument2) {
  clear_rx_buffer();
  uint8_t len = sprintf_P(tx_buffer, format, argument1, argument2);
  LOOP_L_N(i, len) MMU2_SERIAL.write(tx_buffer[i]);
-  rx_buffer[0] = '\0';
  prev_request = millis();
 }

@@ -517,8 +514,7 @@ static void mmu2_not_responding() {
        extruder = index; // filament change is finished
        active_extruder = 0;
        ENABLE_AXIS_E0();
-        SERIAL_ECHO_START();
-        SERIAL_ECHOLNPAIR(STR_ACTIVE_EXTRUDER, int(extruder));
+        SERIAL_ECHO_MSG(STR_ACTIVE_EXTRUDER, extruder);
      }
      ui.reset_status();
    }
@@ -570,7 +566,7 @@ static void mmu2_not_responding() {

        case 'c': {
          while (!thermalManager.wait_for_hotend(active_extruder, false)) safe_delay(100);
-          execute_extruder_sequence((const E_Step *)load_to_nozzle_sequence, COUNT(load_to_nozzle_sequence));
+          load_to_nozzle();
        } break;
      }

@@ -605,8 +601,7 @@ static void mmu2_not_responding() {
      active_extruder = 0;

      ENABLE_AXIS_E0();
-      SERIAL_ECHO_START();
-      SERIAL_ECHOLNPAIR(STR_ACTIVE_EXTRUDER, int(extruder));
+      SERIAL_ECHO_MSG(STR_ACTIVE_EXTRUDER, extruder);

      ui.reset_status();
    }
@@ -701,8 +696,7 @@ static void mmu2_not_responding() {
      extruder = index; //filament change is finished
      active_extruder = 0;
      ENABLE_AXIS_E0();
-      SERIAL_ECHO_START();
-      SERIAL_ECHOLNPAIR(STR_ACTIVE_EXTRUDER, int(extruder));
+      SERIAL_ECHO_MSG(STR_ACTIVE_EXTRUDER, extruder);
      ui.reset_status();
    }

@@ -791,7 +785,7 @@ bool MMU2::get_response() {
 }

 /**
- * Wait for response and deal with timeout if nexcessary
+ * Wait for response and deal with timeout if necessary
 */
 void MMU2::manage_response(const bool move_axes, const bool turn_off_nozzle) {

@@ -917,6 +911,7 @@ void MMU2::filament_runout() {
 // Load filament into MMU2
 void MMU2::load_filament(const uint8_t index) {
  if (!enabled) return;
+
  command(MMU_CMD_L0 + index);
  manage_response(false, false);
  BUZZ(200, 404);
@@ -935,6 +930,7 @@ bool MMU2::load_filament_to_nozzle(const uint8_t index) {
    return false;
  }

+  DISABLE_AXIS_E0();
  command(MMU_CMD_T0 + index);
  manage_response(true, true);

@@ -957,7 +953,6 @@ bool MMU2::load_filament_to_nozzle(const uint8_t index) {
 * filament to nozzle.
 */
 void MMU2::load_to_nozzle() {
-  if (!enabled) return;
  execute_extruder_sequence((const E_Step *)load_to_nozzle_sequence, COUNT(load_to_nozzle_sequence));
 }

@@ -1020,7 +1015,8 @@ bool MMU2::unload() {
    return false;
  }

-  filament_ramming();
+  // Unload sequence to optimize shape of the tip of the unloaded filament
+  execute_extruder_sequence((const E_Step *)ramming_sequence, sizeof(ramming_sequence) / sizeof(E_Step));

  command(MMU_CMD_U0);
  manage_response(false, true);
@@ -1035,13 +1031,6 @@ bool MMU2::unload() {
  return true;
 }

-/**
- * Unload sequence to optimize shape of the tip of the unloaded filament
- */
-void MMU2::filament_ramming() {
-  execute_extruder_sequence((const E_Step *)ramming_sequence, sizeof(ramming_sequence) / sizeof(E_Step));
-}
-
 void MMU2::execute_extruder_sequence(const E_Step * sequence, int steps) {

  planner.synchronize();
@@ -71,7 +71,6 @@ private:
  static void manage_response(const bool move_axes, const bool turn_off_nozzle);

  static void load_to_nozzle();
-  static void filament_ramming();
  static void execute_extruder_sequence(const E_Step * sequence, int steps);

  static void filament_runout();
@@ -130,17 +130,14 @@ fil_change_settings_t fc_settings[EXTRUDERS];
 */
 static bool ensure_safe_temperature(const bool wait=true, const PauseMode mode=PAUSE_MODE_SAME) {
  DEBUG_SECTION(est, "ensure_safe_temperature", true);
-  DEBUG_ECHOLNPAIR("... wait:", int(wait), " mode:", int(mode));
+  DEBUG_ECHOLNPAIR("... wait:", wait, " mode:", mode);

  #if ENABLED(PREVENT_COLD_EXTRUSION)
    if (!DEBUGGING(DRYRUN) && thermalManager.targetTooColdToExtrude(active_extruder))
      thermalManager.setTargetHotend(thermalManager.extrude_min_temp, active_extruder);
  #endif

-  #if HAS_LCD_MENU
-    lcd_pause_show_message(PAUSE_MESSAGE_HEATING, mode);
-  #endif
-  UNUSED(mode);
+  ui.pause_show_message(PAUSE_MESSAGE_HEATING, mode); UNUSED(mode);

  if (wait) return thermalManager.wait_for_hotend(active_extruder);

@@ -179,21 +176,15 @@ bool load_filament(const float &slow_load_length/*=0*/, const float &fast_load_l
                   DXC_ARGS
 ) {
  DEBUG_SECTION(lf, "load_filament", true);
-  DEBUG_ECHOLNPAIR("... slowlen:", slow_load_length, " fastlen:", fast_load_length, " purgelen:", purge_length, " maxbeep:", int(max_beep_count), " showlcd:", int(show_lcd), " pauseforuser:", int(pause_for_user), " pausemode:", int(mode) DXC_SAY);
-
-  UNUSED(show_lcd);
+  DEBUG_ECHOLNPAIR("... slowlen:", slow_load_length, " fastlen:", fast_load_length, " purgelen:", purge_length, " maxbeep:", max_beep_count, " showlcd:", show_lcd, " pauseforuser:", pause_for_user, " pausemode:", mode DXC_SAY);

  if (!ensure_safe_temperature(false, mode)) {
-    #if HAS_LCD_MENU
-      if (show_lcd) lcd_pause_show_message(PAUSE_MESSAGE_STATUS, mode);
-    #endif
+    if (show_lcd) ui.pause_show_message(PAUSE_MESSAGE_STATUS, mode);
    return false;
  }

  if (pause_for_user) {
-    #if HAS_LCD_MENU
-      if (show_lcd) lcd_pause_show_message(PAUSE_MESSAGE_INSERT, mode);
-    #endif
+    if (show_lcd) ui.pause_show_message(PAUSE_MESSAGE_INSERT, mode);
    SERIAL_ECHO_MSG(_PMSG(STR_FILAMENT_CHANGE_INSERT));

    first_impatient_beep(max_beep_count);
@@ -217,9 +208,7 @@ bool load_filament(const float &slow_load_length/*=0*/, const float &fast_load_l
    }
  }

-  #if HAS_LCD_MENU
-    if (show_lcd) lcd_pause_show_message(PAUSE_MESSAGE_LOAD, mode);
-  #endif
+  if (show_lcd) ui.pause_show_message(PAUSE_MESSAGE_LOAD, mode);

  #if ENABLED(DUAL_X_CARRIAGE)
    const int8_t saved_ext        = active_extruder;
@@ -250,9 +239,7 @@ bool load_filament(const float &slow_load_length/*=0*/, const float &fast_load_l

  #if ENABLED(ADVANCED_PAUSE_CONTINUOUS_PURGE)

-    #if HAS_LCD_MENU
-      if (show_lcd) lcd_pause_show_message(PAUSE_MESSAGE_PURGE);
-    #endif
+    if (show_lcd) ui.pause_show_message(PAUSE_MESSAGE_PURGE);

    TERN_(HOST_PROMPT_SUPPORT, host_prompt_do(PROMPT_USER_CONTINUE, PSTR("Filament Purging..."), CONTINUE_STR));
    TERN_(EXTENSIBLE_UI, ExtUI::onUserConfirmRequired_P(PSTR("Filament Purging...")));
@@ -266,9 +253,7 @@ bool load_filament(const float &slow_load_length/*=0*/, const float &fast_load_l
    do {
      if (purge_length > 0) {
        // "Wait for filament purge"
-        #if HAS_LCD_MENU
-          if (show_lcd) lcd_pause_show_message(PAUSE_MESSAGE_PURGE);
-        #endif
+        if (show_lcd) ui.pause_show_message(PAUSE_MESSAGE_PURGE);

        // Extrude filament to get into hotend
        unscaled_e_move(purge_length, ADVANCED_PAUSE_PURGE_FEEDRATE);
@@ -281,7 +266,7 @@ bool load_filament(const float &slow_load_length/*=0*/, const float &fast_load_l
          // Show "Purge More" / "Resume" menu and wait for reply
          KEEPALIVE_STATE(PAUSED_FOR_USER);
          wait_for_user = false;
-          lcd_pause_show_message(PAUSE_MESSAGE_OPTION);
+          ui.pause_show_message(PAUSE_MESSAGE_OPTION);
          while (pause_menu_response == PAUSE_RESPONSE_WAIT_FOR) idle_no_sleep();
        }
      #endif
@@ -324,28 +309,22 @@ bool unload_filament(const float &unload_length, const bool show_lcd/*=false*/,
                     #endif
 ) {
  DEBUG_SECTION(uf, "unload_filament", true);
-  DEBUG_ECHOLNPAIR("... unloadlen:", unload_length, " showlcd:", int(show_lcd), " mode:", int(mode)
+  DEBUG_ECHOLNPAIR("... unloadlen:", unload_length, " showlcd:", show_lcd, " mode:", mode
    #if BOTH(FILAMENT_UNLOAD_ALL_EXTRUDERS, MIXING_EXTRUDER)
      , " mixmult:", mix_multiplier
    #endif
  );

-  UNUSED(show_lcd);
-
  #if !BOTH(FILAMENT_UNLOAD_ALL_EXTRUDERS, MIXING_EXTRUDER)
    constexpr float mix_multiplier = 1.0;
  #endif

  if (!ensure_safe_temperature(false, mode)) {
-    #if HAS_LCD_MENU
-      if (show_lcd) lcd_pause_show_message(PAUSE_MESSAGE_STATUS);
-    #endif
+    if (show_lcd) ui.pause_show_message(PAUSE_MESSAGE_STATUS);
    return false;
  }

-  #if HAS_LCD_MENU
-    if (show_lcd) lcd_pause_show_message(PAUSE_MESSAGE_UNLOAD, mode);
-  #endif
+  if (show_lcd) ui.pause_show_message(PAUSE_MESSAGE_UNLOAD, mode);

  // Retract filament
  unscaled_e_move(-(FILAMENT_UNLOAD_PURGE_RETRACT) * mix_multiplier, (PAUSE_PARK_RETRACT_FEEDRATE) * mix_multiplier);
@@ -394,7 +373,7 @@ uint8_t did_pause_print = 0;

 bool pause_print(const float &retract, const xyz_pos_t &park_point, const float &unload_length/*=0*/, const bool show_lcd/*=false*/ DXC_ARGS) {
  DEBUG_SECTION(pp, "pause_print", true);
-  DEBUG_ECHOLNPAIR("... park.x:", park_point.x, " y:", park_point.y, " z:", park_point.z, " unloadlen:", unload_length, " showlcd:", int(show_lcd) DXC_SAY);
+  DEBUG_ECHOLNPAIR("... park.x:", park_point.x, " y:", park_point.y, " z:", park_point.z, " unloadlen:", unload_length, " showlcd:", show_lcd DXC_SAY);

  UNUSED(show_lcd);

@@ -477,16 +456,16 @@ bool pause_print(const float &retract, const xyz_pos_t &park_point, const float

 void show_continue_prompt(const bool is_reload) {
  DEBUG_SECTION(scp, "pause_print", true);
-  DEBUG_ECHOLNPAIR("... is_reload:", int(is_reload));
+  DEBUG_ECHOLNPAIR("... is_reload:", is_reload);

-  TERN_(HAS_LCD_MENU, lcd_pause_show_message(is_reload ? PAUSE_MESSAGE_INSERT : PAUSE_MESSAGE_WAITING));
+  ui.pause_show_message(is_reload ? PAUSE_MESSAGE_INSERT : PAUSE_MESSAGE_WAITING);
  SERIAL_ECHO_START();
  serialprintPGM(is_reload ? PSTR(_PMSG(STR_FILAMENT_CHANGE_INSERT) "\n") : PSTR(_PMSG(STR_FILAMENT_CHANGE_WAIT) "\n"));
 }

 void wait_for_confirmation(const bool is_reload/*=false*/, const int8_t max_beep_count/*=0*/ DXC_ARGS) {
  DEBUG_SECTION(wfc, "wait_for_confirmation", true);
-  DEBUG_ECHOLNPAIR("... is_reload:", is_reload, " maxbeep:", int(max_beep_count) DXC_SAY);
+  DEBUG_ECHOLNPAIR("... is_reload:", is_reload, " maxbeep:", max_beep_count DXC_SAY);

  bool nozzle_timed_out = false;

@@ -520,7 +499,7 @@ void wait_for_confirmation(const bool is_reload/*=false*/, const int8_t max_beep
    // Wait for the user to press the button to re-heat the nozzle, then
    // re-heat the nozzle, re-show the continue prompt, restart idle timers, start over
    if (nozzle_timed_out) {
-      TERN_(HAS_LCD_MENU, lcd_pause_show_message(PAUSE_MESSAGE_HEAT));
+      ui.pause_show_message(PAUSE_MESSAGE_HEAT);
      SERIAL_ECHO_MSG(_PMSG(STR_FILAMENT_CHANGE_HEAT));

      TERN_(HOST_PROMPT_SUPPORT, host_prompt_do(PROMPT_USER_CONTINUE, GET_TEXT(MSG_HEATER_TIMEOUT), GET_TEXT(MSG_REHEAT)));
@@ -582,7 +561,7 @@ void wait_for_confirmation(const bool is_reload/*=false*/, const int8_t max_beep
 */
 void resume_print(const float &slow_load_length/*=0*/, const float &fast_load_length/*=0*/, const float &purge_length/*=ADVANCED_PAUSE_PURGE_LENGTH*/, const int8_t max_beep_count/*=0*/, int16_t targetTemp/*=0*/ DXC_ARGS) {
  DEBUG_SECTION(rp, "resume_print", true);
-  DEBUG_ECHOLNPAIR("... slowlen:", slow_load_length, " fastlen:", fast_load_length, " purgelen:", purge_length, " maxbeep:", int(max_beep_count), " targetTemp:", targetTemp DXC_SAY);
+  DEBUG_ECHOLNPAIR("... slowlen:", slow_load_length, " fastlen:", fast_load_length, " purgelen:", purge_length, " maxbeep:", max_beep_count, " targetTemp:", targetTemp DXC_SAY);

  /*
  SERIAL_ECHOLNPAIR(
@@ -614,7 +593,7 @@ void resume_print(const float &slow_load_length/*=0*/, const float &fast_load_le
    thermalManager.wait_for_hotend(active_extruder, false);
  }

-  TERN_(HAS_LCD_MENU, lcd_pause_show_message(PAUSE_MESSAGE_RESUME));
+  ui.pause_show_message(PAUSE_MESSAGE_RESUME);

  // Check Temperature before moving hotend
  ensure_safe_temperature();
@@ -653,7 +632,7 @@ void resume_print(const float &slow_load_length/*=0*/, const float &fast_load_le
  // Write PLR now to update the z axis value
  TERN_(POWER_LOSS_RECOVERY, if (recovery.enabled) recovery.save(true));

-  TERN_(HAS_LCD_MENU, lcd_pause_show_message(PAUSE_MESSAGE_STATUS));
+  ui.pause_show_message(PAUSE_MESSAGE_STATUS);

  #ifdef ACTION_ON_RESUMED
    host_action_resumed();
--- a/Show More
+++ b/Show More