Initial commit

83 files changed, 31318 insertions, 0 deletions

diff --git a/Marlin/src/module/delta.cpp b/Marlin/src/module/delta.cpp
new file mode 100644
index 0000000..93238a6
--- /dev/null
+++ b/Marlin/src/module/delta.cpp
@@ -0,0 +1,287 @@
+/**
+ * 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/>.
+ *
+ */
+
+/**
+ * delta.cpp
+ */
+
+#include "../inc/MarlinConfig.h"
+
+#if ENABLED(DELTA)
+
+#include "delta.h"
+#include "motion.h"
+
+// For homing:
+#include "planner.h"
+#include "endstops.h"
+#include "../lcd/marlinui.h"
+#include "../MarlinCore.h"
+
+#if HAS_BED_PROBE
+  #include "probe.h"
+#endif
+
+#if ENABLED(SENSORLESS_HOMING)
+  #include "../feature/tmc_util.h"
+  #include "stepper/indirection.h"
+#endif
+
+#define DEBUG_OUT ENABLED(DEBUG_LEVELING_FEATURE)
+#include "../core/debug_out.h"
+
+// Initialized by settings.load()
+float delta_height;
+abc_float_t delta_endstop_adj{0};
+float delta_radius,
+      delta_diagonal_rod,
+      delta_segments_per_second;
+abc_float_t delta_tower_angle_trim;
+xy_float_t delta_tower[ABC];
+abc_float_t delta_diagonal_rod_2_tower;
+float delta_clip_start_height = Z_MAX_POS;
+abc_float_t delta_diagonal_rod_trim;
+
+float delta_safe_distance_from_top();
+
+/**
+ * Recalculate factors used for delta kinematics whenever
+ * settings have been changed (e.g., by M665).
+ */
+void recalc_delta_settings() {
+  constexpr abc_float_t trt = DELTA_RADIUS_TRIM_TOWER;
+  delta_tower[A_AXIS].set(cos(RADIANS(210 + delta_tower_angle_trim.a)) * (delta_radius + trt.a), // front left tower
+                          sin(RADIANS(210 + delta_tower_angle_trim.a)) * (delta_radius + trt.a));
+  delta_tower[B_AXIS].set(cos(RADIANS(330 + delta_tower_angle_trim.b)) * (delta_radius + trt.b), // front right tower
+                          sin(RADIANS(330 + delta_tower_angle_trim.b)) * (delta_radius + trt.b));
+  delta_tower[C_AXIS].set(cos(RADIANS( 90 + delta_tower_angle_trim.c)) * (delta_radius + trt.c), // back middle tower
+                          sin(RADIANS( 90 + delta_tower_angle_trim.c)) * (delta_radius + trt.c));
+  delta_diagonal_rod_2_tower.set(sq(delta_diagonal_rod + delta_diagonal_rod_trim.a),
+                                 sq(delta_diagonal_rod + delta_diagonal_rod_trim.b),
+                                 sq(delta_diagonal_rod + delta_diagonal_rod_trim.c));
+  update_software_endstops(Z_AXIS);
+  set_all_unhomed();
+}
+
+/**
+ * Get a safe radius for calibration
+ */
+
+#if EITHER(DELTA_AUTO_CALIBRATION, DELTA_CALIBRATION_MENU)
+
+  #if ENABLED(DELTA_AUTO_CALIBRATION)
+    float calibration_radius_factor = 1;
+  #endif
+
+  float delta_calibration_radius() {
+    return calibration_radius_factor * (
+      #if HAS_BED_PROBE
+        FLOOR((DELTA_PRINTABLE_RADIUS) - _MAX(HYPOT(probe.offset_xy.x, probe.offset_xy.y), PROBING_MARGIN))
+      #else
+        DELTA_PRINTABLE_RADIUS
+      #endif
+    );
+  }
+
+#endif
+
+/**
+ * Delta Inverse Kinematics
+ *
+ * Calculate the tower positions for a given machine
+ * position, storing the result in the delta[] array.
+ *
+ * This is an expensive calculation, requiring 3 square
+ * roots per segmented linear move, and strains the limits
+ * of a Mega2560 with a Graphical Display.
+ *
+ * Suggested optimizations include:
+ *
+ * - Disable the home_offset (M206) and/or position_shift (G92)
+ *   features to remove up to 12 float additions.
+ */
+
+#define DELTA_DEBUG(VAR) do { \
+    SERIAL_ECHOLNPAIR_P(PSTR("Cartesian X"), VAR.x, SP_Y_STR, VAR.y, SP_Z_STR, VAR.z); \
+    SERIAL_ECHOLNPAIR_P(PSTR("Delta A"), delta.a, SP_B_STR, delta.b, SP_C_STR, delta.c); \
+  }while(0)
+
+void inverse_kinematics(const xyz_pos_t &raw) {
+  #if HAS_HOTEND_OFFSET
+    // Delta hotend offsets must be applied in Cartesian space with no "spoofing"
+    xyz_pos_t pos = { raw.x - hotend_offset[active_extruder].x,
+                      raw.y - hotend_offset[active_extruder].y,
+                      raw.z };
+    DELTA_IK(pos);
+    //DELTA_DEBUG(pos);
+  #else
+    DELTA_IK(raw);
+    //DELTA_DEBUG(raw);
+  #endif
+}
+
+/**
+ * Calculate the highest Z position where the
+ * effector has the full range of XY motion.
+ */
+float delta_safe_distance_from_top() {
+  xyz_pos_t cartesian{0};
+  inverse_kinematics(cartesian);
+  const float centered_extent = delta.a;
+  cartesian.y = DELTA_PRINTABLE_RADIUS;
+  inverse_kinematics(cartesian);
+  return ABS(centered_extent - delta.a);
+}
+
+/**
+ * Delta Forward Kinematics
+ *
+ * See the Wikipedia article "Trilateration"
+ * https://en.wikipedia.org/wiki/Trilateration
+ *
+ * Establish a new coordinate system in the plane of the
+ * three carriage points. This system has its origin at
+ * tower1, with tower2 on the X axis. Tower3 is in the X-Y
+ * plane with a Z component of zero.
+ * We will define unit vectors in this coordinate system
+ * in our original coordinate system. Then when we calculate
+ * the Xnew, Ynew and Znew values, we can translate back into
+ * the original system by moving along those unit vectors
+ * by the corresponding values.
+ *
+ * Variable names matched to Marlin, c-version, and avoid the
+ * use of any vector library.
+ *
+ * by Andreas Hardtung 2016-06-07
+ * based on a Java function from "Delta Robot Kinematics V3"
+ * by Steve Graves
+ *
+ * The result is stored in the cartes[] array.
+ */
+void forward_kinematics_DELTA(const float &z1, const float &z2, const float &z3) {
+  // Create a vector in old coordinates along x axis of new coordinate
+  const float p12[3] = { delta_tower[B_AXIS].x - delta_tower[A_AXIS].x, delta_tower[B_AXIS].y - delta_tower[A_AXIS].y, z2 - z1 },
+
+  // Get the reciprocal of Magnitude of vector.
+  d2 = sq(p12[0]) + sq(p12[1]) + sq(p12[2]), inv_d = RSQRT(d2),
+
+  // Create unit vector by multiplying by the inverse of the magnitude.
+  ex[3] = { p12[0] * inv_d, p12[1] * inv_d, p12[2] * inv_d },
+
+  // Get the vector from the origin of the new system to the third point.
+  p13[3] = { delta_tower[C_AXIS].x - delta_tower[A_AXIS].x, delta_tower[C_AXIS].y - delta_tower[A_AXIS].y, z3 - z1 },
+
+  // Use the dot product to find the component of this vector on the X axis.
+  i = ex[0] * p13[0] + ex[1] * p13[1] + ex[2] * p13[2],
+
+  // Create a vector along the x axis that represents the x component of p13.
+  iex[3] = { ex[0] * i, ex[1] * i, ex[2] * i };
+
+  // Subtract the X component from the original vector leaving only Y. We use the
+  // variable that will be the unit vector after we scale it.
+  float ey[3] = { p13[0] - iex[0], p13[1] - iex[1], p13[2] - iex[2] };
+
+  // The magnitude and the inverse of the magnitude of Y component
+  const float j2 = sq(ey[0]) + sq(ey[1]) + sq(ey[2]), inv_j = RSQRT(j2);
+
+  // Convert to a unit vector
+  ey[0] *= inv_j; ey[1] *= inv_j; ey[2] *= inv_j;
+
+  // The cross product of the unit x and y is the unit z
+  // float[] ez = vectorCrossProd(ex, ey);
+  const float ez[3] = {
+    ex[1] * ey[2] - ex[2] * ey[1],
+    ex[2] * ey[0] - ex[0] * ey[2],
+    ex[0] * ey[1] - ex[1] * ey[0]
+  },
+
+  // We now have the d, i and j values defined in Wikipedia.
+  // Plug them into the equations defined in Wikipedia for Xnew, Ynew and Znew
+  Xnew = (delta_diagonal_rod_2_tower.a - delta_diagonal_rod_2_tower.b + d2) * inv_d * 0.5,
+  Ynew = ((delta_diagonal_rod_2_tower.a - delta_diagonal_rod_2_tower.c + sq(i) + j2) * 0.5 - i * Xnew) * inv_j,
+  Znew = SQRT(delta_diagonal_rod_2_tower.a - HYPOT2(Xnew, Ynew));
+
+  // Start from the origin of the old coordinates and add vectors in the
+  // old coords that represent the Xnew, Ynew and Znew to find the point
+  // in the old system.
+  cartes.set(delta_tower[A_AXIS].x + ex[0] * Xnew + ey[0] * Ynew - ez[0] * Znew,
+             delta_tower[A_AXIS].y + ex[1] * Xnew + ey[1] * Ynew - ez[1] * Znew,
+                                z1 + ex[2] * Xnew + ey[2] * Ynew - ez[2] * Znew);
+}
+
+/**
+ * A delta can only safely home all axes at the same time
+ * This is like quick_home_xy() but for 3 towers.
+ */
+void home_delta() {
+  DEBUG_SECTION(log_home_delta, "home_delta", DEBUGGING(LEVELING));
+
+  // Init the current position of all carriages to 0,0,0
+  current_position.reset();
+  destination.reset();
+  sync_plan_position();
+
+  // Disable stealthChop if used. Enable diag1 pin on driver.
+  #if ENABLED(SENSORLESS_HOMING)
+  TERN_(X_SENSORLESS, sensorless_t stealth_states_x = start_sensorless_homing_per_axis(X_AXIS));
+  TERN_(Y_SENSORLESS, sensorless_t stealth_states_y = start_sensorless_homing_per_axis(Y_AXIS));
+  TERN_(Z_SENSORLESS, sensorless_t stealth_states_z = start_sensorless_homing_per_axis(Z_AXIS));
+  #endif
+
+  // Move all carriages together linearly until an endstop is hit.
+  current_position.z = (delta_height + 10 - TERN0(HAS_BED_PROBE, probe.offset.z));
+  line_to_current_position(homing_feedrate(Z_AXIS));
+  planner.synchronize();
+
+  // Re-enable stealthChop if used. Disable diag1 pin on driver.
+  #if ENABLED(SENSORLESS_HOMING)
+  TERN_(X_SENSORLESS, end_sensorless_homing_per_axis(X_AXIS, stealth_states_x));
+  TERN_(Y_SENSORLESS, end_sensorless_homing_per_axis(Y_AXIS, stealth_states_y));
+  TERN_(Z_SENSORLESS, end_sensorless_homing_per_axis(Z_AXIS, stealth_states_z));
+  #endif
+
+  endstops.validate_homing_move();
+
+  // At least one carriage has reached the top.
+  // Now re-home each carriage separately.
+  homeaxis(A_AXIS);
+  homeaxis(B_AXIS);
+  homeaxis(C_AXIS);
+
+  // Set all carriages to their home positions
+  // Do this here all at once for Delta, because
+  // XYZ isn't ABC. Applying this per-tower would
+  // give the impression that they are the same.
+  LOOP_XYZ(i) set_axis_is_at_home((AxisEnum)i);
+
+  sync_plan_position();
+
+  #if DISABLED(DELTA_HOME_TO_SAFE_ZONE) && defined(HOMING_BACKOFF_POST_MM)
+    constexpr xyz_float_t endstop_backoff = HOMING_BACKOFF_POST_MM;
+    if (endstop_backoff.z) {
+      current_position.z -= ABS(endstop_backoff.z) * Z_HOME_DIR;
+      line_to_current_position(homing_feedrate(Z_AXIS));
+    }
+  #endif
+}
+
+#endif // DELTA
diff --git a/Marlin/src/module/delta.h b/Marlin/src/module/delta.h
new file mode 100644
index 0000000..5e9a78b
--- /dev/null
+++ b/Marlin/src/module/delta.h
@@ -0,0 +1,129 @@
+/**
+ * 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
+
+/**
+ * delta.h - Delta-specific functions
+ */
+
+#include "../core/types.h"
+#include "../core/macros.h"
+
+extern float delta_height;
+extern abc_float_t delta_endstop_adj;
+extern float delta_radius,
+             delta_diagonal_rod,
+             delta_segments_per_second;
+extern abc_float_t delta_tower_angle_trim;
+extern xy_float_t delta_tower[ABC];
+extern abc_float_t delta_diagonal_rod_2_tower;
+extern float delta_clip_start_height;
+extern abc_float_t delta_diagonal_rod_trim;
+
+/**
+ * Recalculate factors used for delta kinematics whenever
+ * settings have been changed (e.g., by M665).
+ */
+void recalc_delta_settings();
+
+/**
+ * Get a safe radius for calibration
+ */
+#if ENABLED(DELTA_AUTO_CALIBRATION)
+  extern float calibration_radius_factor;
+#else
+  constexpr float calibration_radius_factor = 1;
+#endif
+
+#if EITHER(DELTA_AUTO_CALIBRATION, DELTA_CALIBRATION_MENU)
+  float delta_calibration_radius();
+#endif
+
+/**
+ * Delta Inverse Kinematics
+ *
+ * Calculate the tower positions for a given machine
+ * position, storing the result in the delta[] array.
+ *
+ * This is an expensive calculation, requiring 3 square
+ * roots per segmented linear move, and strains the limits
+ * of a Mega2560 with a Graphical Display.
+ *
+ * Suggested optimizations include:
+ *
+ * - Disable the home_offset (M206) and/or position_shift (G92)
+ *   features to remove up to 12 float additions.
+ *
+ * - Use a fast-inverse-sqrt function and add the reciprocal.
+ *   (see above)
+ */
+
+// Macro to obtain the Z position of an individual tower
+#define DELTA_Z(V,T) V.z + SQRT(          \
+  delta_diagonal_rod_2_tower[T] - HYPOT2( \
+      delta_tower[T].x - V.x,             \
+      delta_tower[T].y - V.y              \
+    )                                     \
+  )
+
+#define DELTA_IK(V) delta.set(DELTA_Z(V, A_AXIS), DELTA_Z(V, B_AXIS), DELTA_Z(V, C_AXIS))
+
+void inverse_kinematics(const xyz_pos_t &raw);
+
+/**
+ * Calculate the highest Z position where the
+ * effector has the full range of XY motion.
+ */
+float delta_safe_distance_from_top();
+
+/**
+ * Delta Forward Kinematics
+ *
+ * See the Wikipedia article "Trilateration"
+ * https://en.wikipedia.org/wiki/Trilateration
+ *
+ * Establish a new coordinate system in the plane of the
+ * three carriage points. This system has its origin at
+ * tower1, with tower2 on the X axis. Tower3 is in the X-Y
+ * plane with a Z component of zero.
+ * We will define unit vectors in this coordinate system
+ * in our original coordinate system. Then when we calculate
+ * the Xnew, Ynew and Znew values, we can translate back into
+ * the original system by moving along those unit vectors
+ * by the corresponding values.
+ *
+ * Variable names matched to Marlin, c-version, and avoid the
+ * use of any vector library.
+ *
+ * by Andreas Hardtung 2016-06-07
+ * based on a Java function from "Delta Robot Kinematics V3"
+ * by Steve Graves
+ *
+ * The result is stored in the cartes[] array.
+ */
+void forward_kinematics_DELTA(const float &z1, const float &z2, const float &z3);
+
+FORCE_INLINE void forward_kinematics_DELTA(const abc_float_t &point) {
+  forward_kinematics_DELTA(point.a, point.b, point.c);
+}
+
+void home_delta();
diff --git a/Marlin/src/module/endstops.cpp b/Marlin/src/module/endstops.cpp
new file mode 100644
index 0000000..b9d2c1c
--- /dev/null
+++ b/Marlin/src/module/endstops.cpp
@@ -0,0 +1,1065 @@
+/**
+ * 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/>.
+ *
+ */
+
+/**
+ * endstops.cpp - A singleton object to manage endstops
+ */
+
+#include "endstops.h"
+#include "stepper.h"
+
+#include "../sd/cardreader.h"
+#include "temperature.h"
+#include "../lcd/marlinui.h"
+
+#if ENABLED(ENDSTOP_INTERRUPTS_FEATURE)
+  #include HAL_PATH(../HAL, endstop_interrupts.h)
+#endif
+
+#if BOTH(SD_ABORT_ON_ENDSTOP_HIT, SDSUPPORT)
+  #include "printcounter.h" // for print_job_timer
+#endif
+
+#if ENABLED(BLTOUCH)
+  #include "../feature/bltouch.h"
+#endif
+
+#if ENABLED(JOYSTICK)
+  #include "../feature/joystick.h"
+#endif
+
+#if HAS_BED_PROBE
+  #include "probe.h"
+#endif
+
+Endstops endstops;
+
+// private:
+
+bool Endstops::enabled, Endstops::enabled_globally; // Initialized by settings.load()
+volatile uint8_t Endstops::hit_state;
+
+Endstops::esbits_t Endstops::live_state = 0;
+
+#if ENDSTOP_NOISE_THRESHOLD
+  Endstops::esbits_t Endstops::validated_live_state;
+  uint8_t Endstops::endstop_poll_count;
+#endif
+
+#if HAS_BED_PROBE
+  volatile bool Endstops::z_probe_enabled = false;
+#endif
+
+// Initialized by settings.load()
+#if ENABLED(X_DUAL_ENDSTOPS)
+  float Endstops::x2_endstop_adj;
+#endif
+#if ENABLED(Y_DUAL_ENDSTOPS)
+  float Endstops::y2_endstop_adj;
+#endif
+#if ENABLED(Z_MULTI_ENDSTOPS)
+  float Endstops::z2_endstop_adj;
+  #if NUM_Z_STEPPER_DRIVERS >= 3
+    float Endstops::z3_endstop_adj;
+    #if NUM_Z_STEPPER_DRIVERS >= 4
+      float Endstops::z4_endstop_adj;
+    #endif
+  #endif
+#endif
+
+#if ENABLED(SPI_ENDSTOPS)
+  Endstops::tmc_spi_homing_t Endstops::tmc_spi_homing; // = 0
+#endif
+#if ENABLED(IMPROVE_HOMING_RELIABILITY)
+  millis_t sg_guard_period; // = 0
+#endif
+
+/**
+ * Class and Instance Methods
+ */
+
+void Endstops::init() {
+
+  #if HAS_X_MIN
+    #if ENABLED(ENDSTOPPULLUP_XMIN)
+      SET_INPUT_PULLUP(X_MIN_PIN);
+    #elif ENABLED(ENDSTOPPULLDOWN_XMIN)
+      SET_INPUT_PULLDOWN(X_MIN_PIN);
+    #else
+      SET_INPUT(X_MIN_PIN);
+    #endif
+  #endif
+
+  #if HAS_X2_MIN
+    #if ENABLED(ENDSTOPPULLUP_XMIN)
+      SET_INPUT_PULLUP(X2_MIN_PIN);
+    #elif ENABLED(ENDSTOPPULLDOWN_XMIN)
+      SET_INPUT_PULLDOWN(X2_MIN_PIN);
+    #else
+      SET_INPUT(X2_MIN_PIN);
+    #endif
+  #endif
+
+  #if HAS_Y_MIN
+    #if ENABLED(ENDSTOPPULLUP_YMIN)
+      SET_INPUT_PULLUP(Y_MIN_PIN);
+    #elif ENABLED(ENDSTOPPULLDOWN_YMIN)
+      SET_INPUT_PULLDOWN(Y_MIN_PIN);
+    #else
+      SET_INPUT(Y_MIN_PIN);
+    #endif
+  #endif
+
+  #if HAS_Y2_MIN
+    #if ENABLED(ENDSTOPPULLUP_YMIN)
+      SET_INPUT_PULLUP(Y2_MIN_PIN);
+    #elif ENABLED(ENDSTOPPULLDOWN_YMIN)
+      SET_INPUT_PULLDOWN(Y2_MIN_PIN);
+    #else
+      SET_INPUT(Y2_MIN_PIN);
+    #endif
+  #endif
+
+  #if HAS_Z_MIN
+    #if ENABLED(ENDSTOPPULLUP_ZMIN)
+      SET_INPUT_PULLUP(Z_MIN_PIN);
+    #elif ENABLED(ENDSTOPPULLDOWN_ZMIN)
+      SET_INPUT_PULLDOWN(Z_MIN_PIN);
+    #else
+      SET_INPUT(Z_MIN_PIN);
+    #endif
+  #endif
+
+  #if HAS_Z2_MIN
+    #if ENABLED(ENDSTOPPULLUP_ZMIN)
+      SET_INPUT_PULLUP(Z2_MIN_PIN);
+    #elif ENABLED(ENDSTOPPULLDOWN_ZMIN)
+      SET_INPUT_PULLDOWN(Z2_MIN_PIN);
+    #else
+      SET_INPUT(Z2_MIN_PIN);
+    #endif
+  #endif
+
+  #if HAS_Z3_MIN
+    #if ENABLED(ENDSTOPPULLUP_ZMIN)
+      SET_INPUT_PULLUP(Z3_MIN_PIN);
+    #elif ENABLED(ENDSTOPPULLDOWN_ZMIN)
+      SET_INPUT_PULLDOWN(Z3_MIN_PIN);
+    #else
+      SET_INPUT(Z3_MIN_PIN);
+    #endif
+  #endif
+
+  #if HAS_Z4_MIN
+    #if ENABLED(ENDSTOPPULLUP_ZMIN)
+      SET_INPUT_PULLUP(Z4_MIN_PIN);
+    #elif ENABLED(ENDSTOPPULLDOWN_ZMIN)
+      SET_INPUT_PULLDOWN(Z4_MIN_PIN);
+    #else
+      SET_INPUT(Z4_MIN_PIN);
+    #endif
+  #endif
+
+  #if HAS_X_MAX
+    #if ENABLED(ENDSTOPPULLUP_XMAX)
+      SET_INPUT_PULLUP(X_MAX_PIN);
+    #elif ENABLED(ENDSTOPPULLDOWN_XMAX)
+      SET_INPUT_PULLDOWN(X_MAX_PIN);
+    #else
+      SET_INPUT(X_MAX_PIN);
+    #endif
+  #endif
+
+  #if HAS_X2_MAX
+    #if ENABLED(ENDSTOPPULLUP_XMAX)
+      SET_INPUT_PULLUP(X2_MAX_PIN);
+    #elif ENABLED(ENDSTOPPULLDOWN_XMAX)
+      SET_INPUT_PULLDOWN(X2_MAX_PIN);
+    #else
+      SET_INPUT(X2_MAX_PIN);
+    #endif
+  #endif
+
+  #if HAS_Y_MAX
+    #if ENABLED(ENDSTOPPULLUP_YMAX)
+      SET_INPUT_PULLUP(Y_MAX_PIN);
+    #elif ENABLED(ENDSTOPPULLDOWN_YMAX)
+      SET_INPUT_PULLDOWN(Y_MAX_PIN);
+    #else
+      SET_INPUT(Y_MAX_PIN);
+    #endif
+  #endif
+
+  #if HAS_Y2_MAX
+    #if ENABLED(ENDSTOPPULLUP_YMAX)
+      SET_INPUT_PULLUP(Y2_MAX_PIN);
+    #elif ENABLED(ENDSTOPPULLDOWN_YMAX)
+      SET_INPUT_PULLDOWN(Y2_MAX_PIN);
+    #else
+      SET_INPUT(Y2_MAX_PIN);
+    #endif
+  #endif
+
+  #if HAS_Z_MAX
+    #if ENABLED(ENDSTOPPULLUP_ZMAX)
+      SET_INPUT_PULLUP(Z_MAX_PIN);
+    #elif ENABLED(ENDSTOPPULLDOWN_ZMAX)
+      SET_INPUT_PULLDOWN(Z_MAX_PIN);
+    #else
+      SET_INPUT(Z_MAX_PIN);
+    #endif
+  #endif
+
+  #if HAS_Z2_MAX
+    #if ENABLED(ENDSTOPPULLUP_ZMAX)
+      SET_INPUT_PULLUP(Z2_MAX_PIN);
+    #elif ENABLED(ENDSTOPPULLDOWN_ZMAX)
+      SET_INPUT_PULLDOWN(Z2_MAX_PIN);
+    #else
+      SET_INPUT(Z2_MAX_PIN);
+    #endif
+  #endif
+
+  #if HAS_Z3_MAX
+    #if ENABLED(ENDSTOPPULLUP_ZMAX)
+      SET_INPUT_PULLUP(Z3_MAX_PIN);
+    #elif ENABLED(ENDSTOPPULLDOWN_ZMAX)
+      SET_INPUT_PULLDOWN(Z3_MAX_PIN);
+    #else
+      SET_INPUT(Z3_MAX_PIN);
+    #endif
+  #endif
+
+  #if HAS_Z4_MAX
+    #if ENABLED(ENDSTOPPULLUP_ZMAX)
+      SET_INPUT_PULLUP(Z4_MAX_PIN);
+    #elif ENABLED(ENDSTOPPULLDOWN_ZMAX)
+      SET_INPUT_PULLDOWN(Z4_MAX_PIN);
+    #else
+      SET_INPUT(Z4_MAX_PIN);
+    #endif
+  #endif
+
+  #if PIN_EXISTS(CALIBRATION)
+    #if ENABLED(CALIBRATION_PIN_PULLUP)
+      SET_INPUT_PULLUP(CALIBRATION_PIN);
+    #elif ENABLED(CALIBRATION_PIN_PULLDOWN)
+      SET_INPUT_PULLDOWN(CALIBRATION_PIN);
+    #else
+      SET_INPUT(CALIBRATION_PIN);
+    #endif
+  #endif
+
+  #if HAS_CUSTOM_PROBE_PIN
+    #if ENABLED(ENDSTOPPULLUP_ZMIN_PROBE)
+      SET_INPUT_PULLUP(Z_MIN_PROBE_PIN);
+    #elif ENABLED(ENDSTOPPULLDOWN_ZMIN_PROBE)
+      SET_INPUT_PULLDOWN(Z_MIN_PROBE_PIN);
+    #else
+      SET_INPUT(Z_MIN_PROBE_PIN);
+    #endif
+  #endif
+
+  #if ENABLED(PROBE_ACTIVATION_SWITCH)
+    SET_INPUT(PROBE_ACTIVATION_SWITCH_PIN);
+  #endif
+
+  TERN_(PROBE_TARE, probe.tare());
+
+  TERN_(ENDSTOP_INTERRUPTS_FEATURE, setup_endstop_interrupts());
+
+  // Enable endstops
+  enable_globally(ENABLED(ENDSTOPS_ALWAYS_ON_DEFAULT));
+
+} // Endstops::init
+
+// Called at ~1KHz from Temperature ISR: Poll endstop state if required
+void Endstops::poll() {
+
+  TERN_(PINS_DEBUGGING, run_monitor()); // Report changes in endstop status
+
+  #if DISABLED(ENDSTOP_INTERRUPTS_FEATURE)
+    update();
+  #elif ENDSTOP_NOISE_THRESHOLD
+    if (endstop_poll_count) update();
+  #endif
+}
+
+void Endstops::enable_globally(const bool onoff) {
+  enabled_globally = enabled = onoff;
+  resync();
+}
+
+// Enable / disable endstop checking
+void Endstops::enable(const bool onoff) {
+  enabled = onoff;
+  resync();
+}
+
+// Disable / Enable endstops based on ENSTOPS_ONLY_FOR_HOMING and global enable
+void Endstops::not_homing() {
+  enabled = enabled_globally;
+}
+
+#if ENABLED(VALIDATE_HOMING_ENDSTOPS)
+  // If the last move failed to trigger an endstop, call kill
+  void Endstops::validate_homing_move() {
+    if (trigger_state()) hit_on_purpose();
+    else kill(GET_TEXT(MSG_KILL_HOMING_FAILED));
+  }
+#endif
+
+// Enable / disable endstop z-probe checking
+#if HAS_BED_PROBE
+  void Endstops::enable_z_probe(const bool onoff) {
+    z_probe_enabled = onoff;
+    resync();
+  }
+#endif
+
+// Get the stable endstop states when enabled
+void Endstops::resync() {
+  if (!abort_enabled()) return;     // If endstops/probes are disabled the loop below can hang
+
+  // Wait for Temperature ISR to run at least once (runs at 1KHz)
+  TERN(ENDSTOP_INTERRUPTS_FEATURE, update(), safe_delay(2));
+  while (TERN0(ENDSTOP_NOISE_THRESHOLD, endstop_poll_count)) safe_delay(1);
+}
+
+#if ENABLED(PINS_DEBUGGING)
+  void Endstops::run_monitor() {
+    if (!monitor_flag) return;
+    static uint8_t monitor_count = 16;  // offset this check from the others
+    monitor_count += _BV(1);            //  15 Hz
+    monitor_count &= 0x7F;
+    if (!monitor_count) monitor();      // report changes in endstop status
+  }
+#endif
+
+void Endstops::event_handler() {
+  static uint8_t prev_hit_state; // = 0
+  if (hit_state == prev_hit_state) return;
+  prev_hit_state = hit_state;
+  if (hit_state) {
+    #if HAS_WIRED_LCD
+      char chrX = ' ', chrY = ' ', chrZ = ' ', chrP = ' ';
+      #define _SET_STOP_CHAR(A,C) (chr## A = C)
+    #else
+      #define _SET_STOP_CHAR(A,C) ;
+    #endif
+
+    #define _ENDSTOP_HIT_ECHO(A,C) do{ \
+      SERIAL_ECHOPAIR(" " STRINGIFY(A) ":", planner.triggered_position_mm(_AXIS(A))); \
+      _SET_STOP_CHAR(A,C); }while(0)
+
+    #define _ENDSTOP_HIT_TEST(A,C) \
+      if (TEST(hit_state, A ##_MIN) || TEST(hit_state, A ##_MAX)) \
+        _ENDSTOP_HIT_ECHO(A,C)
+
+    #define ENDSTOP_HIT_TEST_X() _ENDSTOP_HIT_TEST(X,'X')
+    #define ENDSTOP_HIT_TEST_Y() _ENDSTOP_HIT_TEST(Y,'Y')
+    #define ENDSTOP_HIT_TEST_Z() _ENDSTOP_HIT_TEST(Z,'Z')
+
+    SERIAL_ECHO_START();
+    SERIAL_ECHOPGM(STR_ENDSTOPS_HIT);
+    ENDSTOP_HIT_TEST_X();
+    ENDSTOP_HIT_TEST_Y();
+    ENDSTOP_HIT_TEST_Z();
+
+    #if HAS_CUSTOM_PROBE_PIN
+      #define P_AXIS Z_AXIS
+      if (TEST(hit_state, Z_MIN_PROBE)) _ENDSTOP_HIT_ECHO(P, 'P');
+    #endif
+    SERIAL_EOL();
+
+    TERN_(HAS_WIRED_LCD, ui.status_printf_P(0, PSTR(S_FMT " %c %c %c %c"), GET_TEXT(MSG_LCD_ENDSTOPS), chrX, chrY, chrZ, chrP));
+
+    #if BOTH(SD_ABORT_ON_ENDSTOP_HIT, SDSUPPORT)
+      if (planner.abort_on_endstop_hit) {
+        card.endFilePrint();
+        quickstop_stepper();
+        thermalManager.disable_all_heaters();
+        print_job_timer.stop();
+      }
+    #endif
+  }
+}
+
+static void print_es_state(const bool is_hit, PGM_P const label=nullptr) {
+  if (label) serialprintPGM(label);
+  SERIAL_ECHOPGM(": ");
+  serialprintPGM(is_hit ? PSTR(STR_ENDSTOP_HIT) : PSTR(STR_ENDSTOP_OPEN));
+  SERIAL_EOL();
+}
+
+void _O2 Endstops::report_states() {
+  TERN_(BLTOUCH, bltouch._set_SW_mode());
+  SERIAL_ECHOLNPGM(STR_M119_REPORT);
+  #define ES_REPORT(S) print_es_state(READ(S##_PIN) != S##_ENDSTOP_INVERTING, PSTR(STR_##S))
+  #if HAS_X_MIN
+    ES_REPORT(X_MIN);
+  #endif
+  #if HAS_X2_MIN
+    ES_REPORT(X2_MIN);
+  #endif
+  #if HAS_X_MAX
+    ES_REPORT(X_MAX);
+  #endif
+  #if HAS_X2_MAX
+    ES_REPORT(X2_MAX);
+  #endif
+  #if HAS_Y_MIN
+    ES_REPORT(Y_MIN);
+  #endif
+  #if HAS_Y2_MIN
+    ES_REPORT(Y2_MIN);
+  #endif
+  #if HAS_Y_MAX
+    ES_REPORT(Y_MAX);
+  #endif
+  #if HAS_Y2_MAX
+    ES_REPORT(Y2_MAX);
+  #endif
+  #if HAS_Z_MIN
+    ES_REPORT(Z_MIN);
+  #endif
+  #if HAS_Z2_MIN
+    ES_REPORT(Z2_MIN);
+  #endif
+  #if HAS_Z3_MIN
+    ES_REPORT(Z3_MIN);
+  #endif
+  #if HAS_Z4_MIN
+    ES_REPORT(Z4_MIN);
+  #endif
+  #if HAS_Z_MAX
+    ES_REPORT(Z_MAX);
+  #endif
+  #if HAS_Z2_MAX
+    ES_REPORT(Z2_MAX);
+  #endif
+  #if HAS_Z3_MAX
+    ES_REPORT(Z3_MAX);
+  #endif
+  #if HAS_Z4_MAX
+    ES_REPORT(Z4_MAX);
+  #endif
+  #if BOTH(MARLIN_DEV_MODE, PROBE_ACTIVATION_SWITCH)
+    print_es_state(probe_switch_activated(), PSTR(STR_PROBE_EN));
+  #endif
+  #if HAS_CUSTOM_PROBE_PIN
+    print_es_state(PROBE_TRIGGERED(), PSTR(STR_Z_PROBE));
+  #endif
+  #if HAS_FILAMENT_SENSOR
+    #if NUM_RUNOUT_SENSORS == 1
+      print_es_state(READ(FIL_RUNOUT1_PIN) != FIL_RUNOUT1_STATE, PSTR(STR_FILAMENT_RUNOUT_SENSOR));
+    #else
+      #define _CASE_RUNOUT(N) case N: pin = FIL_RUNOUT##N##_PIN; state = FIL_RUNOUT##N##_STATE; break;
+      LOOP_S_LE_N(i, 1, NUM_RUNOUT_SENSORS) {
+        pin_t pin;
+        uint8_t state;
+        switch (i) {
+          default: continue;
+          REPEAT_S(1, INCREMENT(NUM_RUNOUT_SENSORS), _CASE_RUNOUT)
+        }
+        SERIAL_ECHOPGM(STR_FILAMENT_RUNOUT_SENSOR);
+        if (i > 1) SERIAL_CHAR(' ', '0' + i);
+        print_es_state(extDigitalRead(pin) != state);
+      }
+      #undef _CASE_RUNOUT
+    #endif
+  #endif
+
+  TERN_(BLTOUCH, bltouch._reset_SW_mode());
+  TERN_(JOYSTICK_DEBUG, joystick.report());
+
+} // Endstops::report_states
+
+// The following routines are called from an ISR context. It could be the temperature ISR, the
+// endstop ISR or the Stepper ISR.
+
+#define _ENDSTOP(AXIS, MINMAX) AXIS ##_## MINMAX
+#define _ENDSTOP_PIN(AXIS, MINMAX) AXIS ##_## MINMAX ##_PIN
+#define _ENDSTOP_INVERTING(AXIS, MINMAX) AXIS ##_## MINMAX ##_ENDSTOP_INVERTING
+
+// Check endstops - Could be called from Temperature ISR!
+void Endstops::update() {
+
+  #if !ENDSTOP_NOISE_THRESHOLD
+    if (!abort_enabled()) return;
+  #endif
+
+  #define UPDATE_ENDSTOP_BIT(AXIS, MINMAX) SET_BIT_TO(live_state, _ENDSTOP(AXIS, MINMAX), (READ(_ENDSTOP_PIN(AXIS, MINMAX)) != _ENDSTOP_INVERTING(AXIS, MINMAX)))
+  #define COPY_LIVE_STATE(SRC_BIT, DST_BIT) SET_BIT_TO(live_state, DST_BIT, TEST(live_state, SRC_BIT))
+
+  #if ENABLED(G38_PROBE_TARGET) && PIN_EXISTS(Z_MIN_PROBE) && NONE(CORE_IS_XY, CORE_IS_XZ, MARKFORGED_XY)
+    // If G38 command is active check Z_MIN_PROBE for ALL movement
+    if (G38_move) UPDATE_ENDSTOP_BIT(Z, MIN_PROBE);
+  #endif
+
+  // With Dual X, endstops are only checked in the homing direction for the active extruder
+  #if ENABLED(DUAL_X_CARRIAGE)
+    #define E0_ACTIVE stepper.last_moved_extruder == 0
+    #define X_MIN_TEST() ((X_HOME_DIR < 0 && E0_ACTIVE) || (X2_HOME_DIR < 0 && !E0_ACTIVE))
+    #define X_MAX_TEST() ((X_HOME_DIR > 0 && E0_ACTIVE) || (X2_HOME_DIR > 0 && !E0_ACTIVE))
+  #else
+    #define X_MIN_TEST() true
+    #define X_MAX_TEST() true
+  #endif
+
+  // Use HEAD for core axes, AXIS for others
+  #if ANY(CORE_IS_XY, CORE_IS_XZ, MARKFORGED_XY)
+    #define X_AXIS_HEAD X_HEAD
+  #else
+    #define X_AXIS_HEAD X_AXIS
+  #endif
+  #if ANY(CORE_IS_XY, CORE_IS_YZ, MARKFORGED_XY)
+    #define Y_AXIS_HEAD Y_HEAD
+  #else
+    #define Y_AXIS_HEAD Y_AXIS
+  #endif
+  #if CORE_IS_XZ || CORE_IS_YZ
+    #define Z_AXIS_HEAD Z_HEAD
+  #else
+    #define Z_AXIS_HEAD Z_AXIS
+  #endif
+
+  /**
+   * Check and update endstops
+   */
+  #if HAS_X_MIN && !X_SPI_SENSORLESS
+    UPDATE_ENDSTOP_BIT(X, MIN);
+    #if ENABLED(X_DUAL_ENDSTOPS)
+      #if HAS_X2_MIN
+        UPDATE_ENDSTOP_BIT(X2, MIN);
+      #else
+        COPY_LIVE_STATE(X_MIN, X2_MIN);
+      #endif
+    #endif
+  #endif
+
+  #if HAS_X_MAX && !X_SPI_SENSORLESS
+    UPDATE_ENDSTOP_BIT(X, MAX);
+    #if ENABLED(X_DUAL_ENDSTOPS)
+      #if HAS_X2_MAX
+        UPDATE_ENDSTOP_BIT(X2, MAX);
+      #else
+        COPY_LIVE_STATE(X_MAX, X2_MAX);
+      #endif
+    #endif
+  #endif
+
+  #if HAS_Y_MIN && !Y_SPI_SENSORLESS
+    UPDATE_ENDSTOP_BIT(Y, MIN);
+    #if ENABLED(Y_DUAL_ENDSTOPS)
+      #if HAS_Y2_MIN
+        UPDATE_ENDSTOP_BIT(Y2, MIN);
+      #else
+        COPY_LIVE_STATE(Y_MIN, Y2_MIN);
+      #endif
+    #endif
+  #endif
+
+  #if HAS_Y_MAX && !Y_SPI_SENSORLESS
+    UPDATE_ENDSTOP_BIT(Y, MAX);
+    #if ENABLED(Y_DUAL_ENDSTOPS)
+      #if HAS_Y2_MAX
+        UPDATE_ENDSTOP_BIT(Y2, MAX);
+      #else
+        COPY_LIVE_STATE(Y_MAX, Y2_MAX);
+      #endif
+    #endif
+  #endif
+
+  #if HAS_Z_MIN && NONE(Z_SPI_SENSORLESS, Z_MIN_PROBE_USES_Z_MIN_ENDSTOP_PIN)
+    UPDATE_ENDSTOP_BIT(Z, MIN);
+    #if ENABLED(Z_MULTI_ENDSTOPS)
+      #if HAS_Z2_MIN
+        UPDATE_ENDSTOP_BIT(Z2, MIN);
+      #else
+        COPY_LIVE_STATE(Z_MIN, Z2_MIN);
+      #endif
+      #if NUM_Z_STEPPER_DRIVERS >= 3
+        #if HAS_Z3_MIN
+          UPDATE_ENDSTOP_BIT(Z3, MIN);
+        #else
+          COPY_LIVE_STATE(Z_MIN, Z3_MIN);
+        #endif
+      #endif
+      #if NUM_Z_STEPPER_DRIVERS >= 4
+        #if HAS_Z4_MIN
+          UPDATE_ENDSTOP_BIT(Z4, MIN);
+        #else
+          COPY_LIVE_STATE(Z_MIN, Z4_MIN);
+        #endif
+      #endif
+    #endif
+  #endif
+
+  #if HAS_BED_PROBE
+    // When closing the gap check the enabled probe
+    if (probe_switch_activated())
+      UPDATE_ENDSTOP_BIT(Z, TERN(HAS_CUSTOM_PROBE_PIN, MIN_PROBE, MIN));
+  #endif
+
+  #if HAS_Z_MAX && !Z_SPI_SENSORLESS
+    // Check both Z dual endstops
+    #if ENABLED(Z_MULTI_ENDSTOPS)
+      UPDATE_ENDSTOP_BIT(Z, MAX);
+      #if HAS_Z2_MAX
+        UPDATE_ENDSTOP_BIT(Z2, MAX);
+      #else
+        COPY_LIVE_STATE(Z_MAX, Z2_MAX);
+      #endif
+      #if NUM_Z_STEPPER_DRIVERS >= 3
+        #if HAS_Z3_MAX
+          UPDATE_ENDSTOP_BIT(Z3, MAX);
+        #else
+          COPY_LIVE_STATE(Z_MAX, Z3_MAX);
+        #endif
+      #endif
+      #if NUM_Z_STEPPER_DRIVERS >= 4
+        #if HAS_Z4_MAX
+          UPDATE_ENDSTOP_BIT(Z4, MAX);
+        #else
+          COPY_LIVE_STATE(Z_MAX, Z4_MAX);
+        #endif
+      #endif
+    #elif !HAS_CUSTOM_PROBE_PIN || Z_MAX_PIN != Z_MIN_PROBE_PIN
+      // If this pin isn't the bed probe it's the Z endstop
+      UPDATE_ENDSTOP_BIT(Z, MAX);
+    #endif
+  #endif
+
+  #if ENDSTOP_NOISE_THRESHOLD
+
+    /**
+     * Filtering out noise on endstops requires a delayed decision. Let's assume, due to noise,
+     * that 50% of endstop signal samples are good and 50% are bad (assuming normal distribution
+     * of random noise). Then the first sample has a 50% chance to be good or bad. The 2nd sample
+     * also has a 50% chance to be good or bad. The chances of 2 samples both being bad becomes
+     * 50% of 50%, or 25%. That was the previous implementation of Marlin endstop handling. It
+     * reduces chances of bad readings in half, at the cost of 1 extra sample period, but chances
+     * still exist. The only way to reduce them further is to increase the number of samples.
+     * To reduce the chance to 1% (1/128th) requires 7 samples (adding 7ms of delay).
+     */
+    static esbits_t old_live_state;
+    if (old_live_state != live_state) {
+      endstop_poll_count = ENDSTOP_NOISE_THRESHOLD;
+      old_live_state = live_state;
+    }
+    else if (endstop_poll_count && !--endstop_poll_count)
+      validated_live_state = live_state;
+
+    if (!abort_enabled()) return;
+
+  #endif
+
+  // Test the current status of an endstop
+  #define TEST_ENDSTOP(ENDSTOP) (TEST(state(), ENDSTOP))
+
+  // Record endstop was hit
+  #define _ENDSTOP_HIT(AXIS, MINMAX) SBI(hit_state, _ENDSTOP(AXIS, MINMAX))
+
+  // Call the endstop triggered routine for single endstops
+  #define PROCESS_ENDSTOP(AXIS, MINMAX) do { \
+    if (TEST_ENDSTOP(_ENDSTOP(AXIS, MINMAX))) { \
+      _ENDSTOP_HIT(AXIS, MINMAX); \
+      planner.endstop_triggered(_AXIS(AXIS)); \
+    } \
+  }while(0)
+
+  // Core Sensorless Homing needs to test an Extra Pin
+  #define CORE_DIAG(QQ,A,MM) (CORE_IS_##QQ && A##_SENSORLESS && !A##_SPI_SENSORLESS && HAS_##A##_##MM)
+  #define PROCESS_CORE_ENDSTOP(A1,M1,A2,M2) do { \
+    if (TEST_ENDSTOP(_ENDSTOP(A1,M1))) { \
+      _ENDSTOP_HIT(A2,M2); \
+      planner.endstop_triggered(_AXIS(A2)); \
+    } \
+  }while(0)
+
+  // Call the endstop triggered routine for dual endstops
+  #define PROCESS_DUAL_ENDSTOP(A, MINMAX) do { \
+    const byte dual_hit = TEST_ENDSTOP(_ENDSTOP(A, MINMAX)) | (TEST_ENDSTOP(_ENDSTOP(A##2, MINMAX)) << 1); \
+    if (dual_hit) { \
+      _ENDSTOP_HIT(A, MINMAX); \
+      /* if not performing home or if both endstops were trigged during homing... */ \
+      if (!stepper.separate_multi_axis || dual_hit == 0b11) \
+        planner.endstop_triggered(_AXIS(A)); \
+    } \
+  }while(0)
+
+  #define PROCESS_TRIPLE_ENDSTOP(A, MINMAX) do { \
+    const byte triple_hit = TEST_ENDSTOP(_ENDSTOP(A, MINMAX)) | (TEST_ENDSTOP(_ENDSTOP(A##2, MINMAX)) << 1) | (TEST_ENDSTOP(_ENDSTOP(A##3, MINMAX)) << 2); \
+    if (triple_hit) { \
+      _ENDSTOP_HIT(A, MINMAX); \
+      /* if not performing home or if both endstops were trigged during homing... */ \
+      if (!stepper.separate_multi_axis || triple_hit == 0b111) \
+        planner.endstop_triggered(_AXIS(A)); \
+    } \
+  }while(0)
+
+  #define PROCESS_QUAD_ENDSTOP(A, MINMAX) do { \
+    const byte quad_hit = TEST_ENDSTOP(_ENDSTOP(A, MINMAX)) | (TEST_ENDSTOP(_ENDSTOP(A##2, MINMAX)) << 1) | (TEST_ENDSTOP(_ENDSTOP(A##3, MINMAX)) << 2) | (TEST_ENDSTOP(_ENDSTOP(A##4, MINMAX)) << 3); \
+    if (quad_hit) { \
+      _ENDSTOP_HIT(A, MINMAX); \
+      /* if not performing home or if both endstops were trigged during homing... */ \
+      if (!stepper.separate_multi_axis || quad_hit == 0b1111) \
+        planner.endstop_triggered(_AXIS(A)); \
+    } \
+  }while(0)
+
+  #if ENABLED(X_DUAL_ENDSTOPS)
+    #define PROCESS_ENDSTOP_X(MINMAX) PROCESS_DUAL_ENDSTOP(X, MINMAX)
+  #else
+    #define PROCESS_ENDSTOP_X(MINMAX) if (X_##MINMAX##_TEST()) PROCESS_ENDSTOP(X, MINMAX)
+  #endif
+
+  #if ENABLED(Y_DUAL_ENDSTOPS)
+    #define PROCESS_ENDSTOP_Y(MINMAX) PROCESS_DUAL_ENDSTOP(Y, MINMAX)
+  #else
+    #define PROCESS_ENDSTOP_Y(MINMAX) PROCESS_ENDSTOP(Y, MINMAX)
+  #endif
+
+  #if DISABLED(Z_MULTI_ENDSTOPS)
+    #define PROCESS_ENDSTOP_Z(MINMAX) PROCESS_ENDSTOP(Z, MINMAX)
+  #elif NUM_Z_STEPPER_DRIVERS == 4
+    #define PROCESS_ENDSTOP_Z(MINMAX) PROCESS_QUAD_ENDSTOP(Z, MINMAX)
+  #elif NUM_Z_STEPPER_DRIVERS == 3
+    #define PROCESS_ENDSTOP_Z(MINMAX) PROCESS_TRIPLE_ENDSTOP(Z, MINMAX)
+  #else
+    #define PROCESS_ENDSTOP_Z(MINMAX) PROCESS_DUAL_ENDSTOP(Z, MINMAX)
+  #endif
+
+  #if ENABLED(G38_PROBE_TARGET) && PIN_EXISTS(Z_MIN_PROBE) && NONE(CORE_IS_XY, CORE_IS_XZ, MARKFORGED_XY)
+    #if ENABLED(G38_PROBE_AWAY)
+      #define _G38_OPEN_STATE (G38_move >= 4)
+    #else
+      #define _G38_OPEN_STATE LOW
+    #endif
+    // If G38 command is active check Z_MIN_PROBE for ALL movement
+    if (G38_move && TEST_ENDSTOP(_ENDSTOP(Z, MIN_PROBE)) != _G38_OPEN_STATE) {
+           if (stepper.axis_is_moving(X_AXIS)) { _ENDSTOP_HIT(X, MIN); planner.endstop_triggered(X_AXIS); }
+      else if (stepper.axis_is_moving(Y_AXIS)) { _ENDSTOP_HIT(Y, MIN); planner.endstop_triggered(Y_AXIS); }
+      else if (stepper.axis_is_moving(Z_AXIS)) { _ENDSTOP_HIT(Z, MIN); planner.endstop_triggered(Z_AXIS); }
+      G38_did_trigger = true;
+    }
+  #endif
+
+  // Signal, after validation, if an endstop limit is pressed or not
+
+  if (stepper.axis_is_moving(X_AXIS)) {
+    if (stepper.motor_direction(X_AXIS_HEAD)) { // -direction
+      #if HAS_X_MIN || (X_SPI_SENSORLESS && X_HOME_DIR < 0)
+        PROCESS_ENDSTOP_X(MIN);
+        #if   CORE_DIAG(XY, Y, MIN)
+          PROCESS_CORE_ENDSTOP(Y,MIN,X,MIN);
+        #elif CORE_DIAG(XY, Y, MAX)
+          PROCESS_CORE_ENDSTOP(Y,MAX,X,MIN);
+        #elif CORE_DIAG(XZ, Z, MIN)
+          PROCESS_CORE_ENDSTOP(Z,MIN,X,MIN);
+        #elif CORE_DIAG(XZ, Z, MAX)
+          PROCESS_CORE_ENDSTOP(Z,MAX,X,MIN);
+        #endif
+      #endif
+    }
+    else { // +direction
+      #if HAS_X_MAX || (X_SPI_SENSORLESS && X_HOME_DIR > 0)
+        PROCESS_ENDSTOP_X(MAX);
+        #if   CORE_DIAG(XY, Y, MIN)
+          PROCESS_CORE_ENDSTOP(Y,MIN,X,MAX);
+        #elif CORE_DIAG(XY, Y, MAX)
+          PROCESS_CORE_ENDSTOP(Y,MAX,X,MAX);
+        #elif CORE_DIAG(XZ, Z, MIN)
+          PROCESS_CORE_ENDSTOP(Z,MIN,X,MAX);
+        #elif CORE_DIAG(XZ, Z, MAX)
+          PROCESS_CORE_ENDSTOP(Z,MAX,X,MAX);
+        #endif
+      #endif
+    }
+  }
+
+  if (stepper.axis_is_moving(Y_AXIS)) {
+    if (stepper.motor_direction(Y_AXIS_HEAD)) { // -direction
+      #if HAS_Y_MIN || (Y_SPI_SENSORLESS && Y_HOME_DIR < 0)
+        PROCESS_ENDSTOP_Y(MIN);
+        #if   CORE_DIAG(XY, X, MIN)
+          PROCESS_CORE_ENDSTOP(X,MIN,Y,MIN);
+        #elif CORE_DIAG(XY, X, MAX)
+          PROCESS_CORE_ENDSTOP(X,MAX,Y,MIN);
+        #elif CORE_DIAG(YZ, Z, MIN)
+          PROCESS_CORE_ENDSTOP(Z,MIN,Y,MIN);
+        #elif CORE_DIAG(YZ, Z, MAX)
+          PROCESS_CORE_ENDSTOP(Z,MAX,Y,MIN);
+        #endif
+      #endif
+    }
+    else { // +direction
+      #if HAS_Y_MAX || (Y_SPI_SENSORLESS && Y_HOME_DIR > 0)
+        PROCESS_ENDSTOP_Y(MAX);
+        #if   CORE_DIAG(XY, X, MIN)
+          PROCESS_CORE_ENDSTOP(X,MIN,Y,MAX);
+        #elif CORE_DIAG(XY, X, MAX)
+          PROCESS_CORE_ENDSTOP(X,MAX,Y,MAX);
+        #elif CORE_DIAG(YZ, Z, MIN)
+          PROCESS_CORE_ENDSTOP(Z,MIN,Y,MAX);
+        #elif CORE_DIAG(YZ, Z, MAX)
+          PROCESS_CORE_ENDSTOP(Z,MAX,Y,MAX);
+        #endif
+      #endif
+    }
+  }
+
+  if (stepper.axis_is_moving(Z_AXIS)) {
+    if (stepper.motor_direction(Z_AXIS_HEAD)) { // Z -direction. Gantry down, bed up.
+
+      #if HAS_Z_MIN || (Z_SPI_SENSORLESS && Z_HOME_DIR < 0)
+        if ( TERN1(Z_MIN_PROBE_USES_Z_MIN_ENDSTOP_PIN, z_probe_enabled)
+          && TERN1(HAS_CUSTOM_PROBE_PIN, !z_probe_enabled)
+        ) PROCESS_ENDSTOP_Z(MIN);
+        #if   CORE_DIAG(XZ, X, MIN)
+          PROCESS_CORE_ENDSTOP(X,MIN,Z,MIN);
+        #elif CORE_DIAG(XZ, X, MAX)
+          PROCESS_CORE_ENDSTOP(X,MAX,Z,MIN);
+        #elif CORE_DIAG(YZ, Y, MIN)
+          PROCESS_CORE_ENDSTOP(Y,MIN,Z,MIN);
+        #elif CORE_DIAG(YZ, Y, MAX)
+          PROCESS_CORE_ENDSTOP(Y,MAX,Z,MIN);
+        #endif
+      #endif
+
+      // When closing the gap check the enabled probe
+      #if HAS_CUSTOM_PROBE_PIN
+        if (z_probe_enabled) PROCESS_ENDSTOP(Z, MIN_PROBE);
+      #endif
+    }
+    else { // Z +direction. Gantry up, bed down.
+      #if HAS_Z_MAX || (Z_SPI_SENSORLESS && Z_HOME_DIR > 0)
+        #if ENABLED(Z_MULTI_ENDSTOPS)
+          PROCESS_ENDSTOP_Z(MAX);
+        #elif !HAS_CUSTOM_PROBE_PIN || Z_MAX_PIN != Z_MIN_PROBE_PIN  // No probe or probe is Z_MIN || Probe is not Z_MAX
+          PROCESS_ENDSTOP(Z, MAX);
+        #endif
+        #if   CORE_DIAG(XZ, X, MIN)
+          PROCESS_CORE_ENDSTOP(X,MIN,Z,MAX);
+        #elif CORE_DIAG(XZ, X, MAX)
+          PROCESS_CORE_ENDSTOP(X,MAX,Z,MAX);
+        #elif CORE_DIAG(YZ, Y, MIN)
+          PROCESS_CORE_ENDSTOP(Y,MIN,Z,MAX);
+        #elif CORE_DIAG(YZ, Y, MAX)
+          PROCESS_CORE_ENDSTOP(Y,MAX,Z,MAX);
+        #endif
+      #endif
+    }
+  }
+} // Endstops::update()
+
+#if ENABLED(SPI_ENDSTOPS)
+
+  bool Endstops::tmc_spi_homing_check() {
+    bool hit = false;
+    #if X_SPI_SENSORLESS
+      if (tmc_spi_homing.x && (stepperX.test_stall_status()
+        #if ANY(CORE_IS_XY, MARKFORGED_XY) && Y_SPI_SENSORLESS
+          || stepperY.test_stall_status()
+        #elif CORE_IS_XZ && Z_SPI_SENSORLESS
+          || stepperZ.test_stall_status()
+        #endif
+      )) {
+        SBI(live_state, X_ENDSTOP);
+        hit = true;
+      }
+    #endif
+    #if Y_SPI_SENSORLESS
+      if (tmc_spi_homing.y && (stepperY.test_stall_status()
+        #if ANY(CORE_IS_XY, MARKFORGED_XY) && X_SPI_SENSORLESS
+          || stepperX.test_stall_status()
+        #elif CORE_IS_YZ && Z_SPI_SENSORLESS
+          || stepperZ.test_stall_status()
+        #endif
+      )) {
+        SBI(live_state, Y_ENDSTOP);
+        hit = true;
+      }
+    #endif
+    #if Z_SPI_SENSORLESS
+      if (tmc_spi_homing.z && (stepperZ.test_stall_status()
+        #if CORE_IS_XZ && X_SPI_SENSORLESS
+          || stepperX.test_stall_status()
+        #elif CORE_IS_YZ && Y_SPI_SENSORLESS
+          || stepperY.test_stall_status()
+        #endif
+      )) {
+        SBI(live_state, Z_ENDSTOP);
+        hit = true;
+      }
+    #endif
+
+    if (TERN0(ENDSTOP_INTERRUPTS_FEATURE, hit)) update();
+
+    return hit;
+  }
+
+  void Endstops::clear_endstop_state() {
+    TERN_(X_SPI_SENSORLESS, CBI(live_state, X_ENDSTOP));
+    TERN_(Y_SPI_SENSORLESS, CBI(live_state, Y_ENDSTOP));
+    TERN_(Z_SPI_SENSORLESS, CBI(live_state, Z_ENDSTOP));
+  }
+
+#endif // SPI_ENDSTOPS
+
+#if ENABLED(PINS_DEBUGGING)
+
+  bool Endstops::monitor_flag = false;
+
+  /**
+   * Monitor Endstops and Z Probe for changes
+   *
+   * If a change is detected then the LED is toggled and
+   * a message is sent out the serial port.
+   *
+   * Yes, we could miss a rapid back & forth change but
+   * that won't matter because this is all manual.
+   */
+  void Endstops::monitor() {
+
+    static uint16_t old_live_state_local = 0;
+    static uint8_t local_LED_status = 0;
+    uint16_t live_state_local = 0;
+
+    #define ES_GET_STATE(S) if (READ(S##_PIN)) SBI(live_state_local, S)
+
+    #if HAS_X_MIN
+      ES_GET_STATE(X_MIN);
+    #endif
+    #if HAS_X_MAX
+      ES_GET_STATE(X_MAX);
+    #endif
+    #if HAS_Y_MIN
+      ES_GET_STATE(Y_MIN);
+    #endif
+    #if HAS_Y_MAX
+      ES_GET_STATE(Y_MAX);
+    #endif
+    #if HAS_Z_MIN
+      ES_GET_STATE(Z_MIN);
+    #endif
+    #if HAS_Z_MAX
+      ES_GET_STATE(Z_MAX);
+    #endif
+    #if HAS_Z_MIN_PROBE_PIN
+      ES_GET_STATE(Z_MIN_PROBE);
+    #endif
+    #if HAS_X2_MIN
+      ES_GET_STATE(X2_MIN);
+    #endif
+    #if HAS_X2_MAX
+      ES_GET_STATE(X2_MAX);
+    #endif
+    #if HAS_Y2_MIN
+      ES_GET_STATE(Y2_MIN);
+    #endif
+    #if HAS_Y2_MAX
+      ES_GET_STATE(Y2_MAX);
+    #endif
+    #if HAS_Z2_MIN
+      ES_GET_STATE(Z2_MIN);
+    #endif
+    #if HAS_Z2_MAX
+      ES_GET_STATE(Z2_MAX);
+    #endif
+    #if HAS_Z3_MIN
+      ES_GET_STATE(Z3_MIN);
+    #endif
+    #if HAS_Z3_MAX
+      ES_GET_STATE(Z3_MAX);
+    #endif
+    #if HAS_Z4_MIN
+      ES_GET_STATE(Z4_MIN);
+    #endif
+    #if HAS_Z4_MAX
+      ES_GET_STATE(Z4_MAX);
+    #endif
+
+    uint16_t endstop_change = live_state_local ^ old_live_state_local;
+    #define ES_REPORT_CHANGE(S) if (TEST(endstop_change, S)) SERIAL_ECHOPAIR("  " STRINGIFY(S) ":", TEST(live_state_local, S))
+
+    if (endstop_change) {
+      #if HAS_X_MIN
+        ES_REPORT_CHANGE(X_MIN);
+      #endif
+      #if HAS_X_MAX
+        ES_REPORT_CHANGE(X_MAX);
+      #endif
+      #if HAS_Y_MIN
+        ES_REPORT_CHANGE(Y_MIN);
+      #endif
+      #if HAS_Y_MAX
+        ES_REPORT_CHANGE(Y_MAX);
+      #endif
+      #if HAS_Z_MIN
+        ES_REPORT_CHANGE(Z_MIN);
+      #endif
+      #if HAS_Z_MAX
+        ES_REPORT_CHANGE(Z_MAX);
+      #endif
+      #if HAS_Z_MIN_PROBE_PIN
+        ES_REPORT_CHANGE(Z_MIN_PROBE);
+      #endif
+      #if HAS_X2_MIN
+        ES_REPORT_CHANGE(X2_MIN);
+      #endif
+      #if HAS_X2_MAX
+        ES_REPORT_CHANGE(X2_MAX);
+      #endif
+      #if HAS_Y2_MIN
+        ES_REPORT_CHANGE(Y2_MIN);
+      #endif
+      #if HAS_Y2_MAX
+        ES_REPORT_CHANGE(Y2_MAX);
+      #endif
+      #if HAS_Z2_MIN
+        ES_REPORT_CHANGE(Z2_MIN);
+      #endif
+      #if HAS_Z2_MAX
+        ES_REPORT_CHANGE(Z2_MAX);
+      #endif
+      #if HAS_Z3_MIN
+        ES_REPORT_CHANGE(Z3_MIN);
+      #endif
+      #if HAS_Z3_MAX
+        ES_REPORT_CHANGE(Z3_MAX);
+      #endif
+      #if HAS_Z4_MIN
+        ES_REPORT_CHANGE(Z4_MIN);
+      #endif
+      #if HAS_Z4_MAX
+        ES_REPORT_CHANGE(Z4_MAX);
+      #endif
+      SERIAL_ECHOLNPGM("\n");
+      analogWrite(pin_t(LED_PIN), local_LED_status);
+      local_LED_status ^= 255;
+      old_live_state_local = live_state_local;
+    }
+  }
+
+#endif // PINS_DEBUGGING
diff --git a/Marlin/src/module/endstops.h b/Marlin/src/module/endstops.h
new file mode 100644
index 0000000..05936a6
--- /dev/null
+++ b/Marlin/src/module/endstops.h
@@ -0,0 +1,198 @@
+/**
+ * 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
+
+/**
+ * endstops.h - manages endstops
+ */
+
+#include "../inc/MarlinConfig.h"
+#include <stdint.h>
+
+enum EndstopEnum : char {
+  X_MIN,  Y_MIN,  Z_MIN,  Z_MIN_PROBE,
+  X_MAX,  Y_MAX,  Z_MAX,
+  X2_MIN, X2_MAX,
+  Y2_MIN, Y2_MAX,
+  Z2_MIN, Z2_MAX,
+  Z3_MIN, Z3_MAX,
+  Z4_MIN, Z4_MAX
+};
+
+#define X_ENDSTOP (X_HOME_DIR < 0 ? X_MIN : X_MAX)
+#define Y_ENDSTOP (Y_HOME_DIR < 0 ? Y_MIN : Y_MAX)
+#define Z_ENDSTOP (Z_HOME_DIR < 0 ? TERN(HOMING_Z_WITH_PROBE, Z_MIN, Z_MIN_PROBE) : Z_MAX)
+
+class Endstops {
+  public:
+    #if HAS_EXTRA_ENDSTOPS
+      typedef uint16_t esbits_t;
+      TERN_(X_DUAL_ENDSTOPS, static float x2_endstop_adj);
+      TERN_(Y_DUAL_ENDSTOPS, static float y2_endstop_adj);
+      TERN_(Z_MULTI_ENDSTOPS, static float z2_endstop_adj);
+      #if ENABLED(Z_MULTI_ENDSTOPS) && NUM_Z_STEPPER_DRIVERS >= 3
+        static float z3_endstop_adj;
+      #endif
+      #if ENABLED(Z_MULTI_ENDSTOPS) && NUM_Z_STEPPER_DRIVERS >= 4
+        static float z4_endstop_adj;
+      #endif
+    #else
+      typedef uint8_t esbits_t;
+    #endif
+
+  private:
+    static bool enabled, enabled_globally;
+    static esbits_t live_state;
+    static volatile uint8_t hit_state;      // Use X_MIN, Y_MIN, Z_MIN and Z_MIN_PROBE as BIT index
+
+    #if ENDSTOP_NOISE_THRESHOLD
+      static esbits_t validated_live_state;
+      static uint8_t endstop_poll_count;    // Countdown from threshold for polling
+    #endif
+
+  public:
+    Endstops() {};
+
+    /**
+     * Initialize the endstop pins
+     */
+    static void init();
+
+    /**
+     * Are endstops or the probe set to abort the move?
+     */
+    FORCE_INLINE static bool abort_enabled() {
+      return enabled || TERN0(HAS_BED_PROBE, z_probe_enabled);
+    }
+
+    static inline bool global_enabled() { return enabled_globally; }
+
+    /**
+     * Periodic call to poll endstops if required. Called from temperature ISR
+     */
+    static void poll();
+
+    /**
+     * Update endstops bits from the pins. Apply filtering to get a verified state.
+     * If abort_enabled() and moving towards a triggered switch, abort the current move.
+     * Called from ISR contexts.
+     */
+    static void update();
+
+    /**
+     * Get Endstop hit state.
+     */
+    FORCE_INLINE static uint8_t trigger_state() { return hit_state; }
+
+    /**
+     * Get current endstops state
+     */
+    FORCE_INLINE static esbits_t state() {
+      return
+        #if ENDSTOP_NOISE_THRESHOLD
+          validated_live_state
+        #else
+          live_state
+        #endif
+      ;
+    }
+
+    static inline bool probe_switch_activated() {
+      return (true
+        #if ENABLED(PROBE_ACTIVATION_SWITCH)
+          && READ(PROBE_ACTIVATION_SWITCH_PIN) == PROBE_ACTIVATION_SWITCH_STATE
+        #endif
+      );
+    }
+
+    /**
+     * Report endstop hits to serial. Called from loop().
+     */
+    static void event_handler();
+
+    /**
+     * Report endstop states in response to M119
+     */
+    static void report_states();
+
+    // Enable / disable endstop checking globally
+    static void enable_globally(const bool onoff=true);
+
+    // Enable / disable endstop checking
+    static void enable(const bool onoff=true);
+
+    // Disable / Enable endstops based on ENSTOPS_ONLY_FOR_HOMING and global enable
+    static void not_homing();
+
+    #if ENABLED(VALIDATE_HOMING_ENDSTOPS)
+      // If the last move failed to trigger an endstop, call kill
+      static void validate_homing_move();
+    #else
+      FORCE_INLINE static void validate_homing_move() { hit_on_purpose(); }
+    #endif
+
+    // Clear endstops (i.e., they were hit intentionally) to suppress the report
+    FORCE_INLINE static void hit_on_purpose() { hit_state = 0; }
+
+    // Enable / disable endstop z-probe checking
+    #if HAS_BED_PROBE
+      static volatile bool z_probe_enabled;
+      static void enable_z_probe(const bool onoff=true);
+    #endif
+
+    static void resync();
+
+    // Debugging of endstops
+    #if ENABLED(PINS_DEBUGGING)
+      static bool monitor_flag;
+      static void monitor();
+      static void run_monitor();
+    #endif
+
+    #if ENABLED(SPI_ENDSTOPS)
+      typedef struct {
+        union {
+          bool any;
+          struct { bool x:1, y:1, z:1; };
+        };
+      } tmc_spi_homing_t;
+      static tmc_spi_homing_t tmc_spi_homing;
+      static void clear_endstop_state();
+      static bool tmc_spi_homing_check();
+    #endif
+};
+
+extern Endstops endstops;
+
+/**
+ * A class to save and change the endstop state,
+ * then restore it when it goes out of scope.
+ */
+class TemporaryGlobalEndstopsState {
+  bool saved;
+
+  public:
+    TemporaryGlobalEndstopsState(const bool enable) : saved(endstops.global_enabled()) {
+      endstops.enable_globally(enable);
+    }
+    ~TemporaryGlobalEndstopsState() { endstops.enable_globally(saved); }
+};
diff --git a/Marlin/src/module/motion.cpp b/Marlin/src/module/motion.cpp
new file mode 100644
index 0000000..f7fc66b
--- /dev/null
+++ b/Marlin/src/module/motion.cpp
@@ -0,0 +1,1896 @@
+/**
+ * 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/>.
+ *
+ */
+
+/**
+ * motion.cpp
+ */
+
+#include "motion.h"
+#include "endstops.h"
+#include "stepper.h"
+#include "planner.h"
+#include "temperature.h"
+
+#include "../gcode/gcode.h"
+
+#include "../inc/MarlinConfig.h"
+
+#if IS_SCARA
+  #include "../libs/buzzer.h"
+  #include "../lcd/marlinui.h"
+#endif
+
+#if HAS_BED_PROBE
+  #include "probe.h"
+#endif
+
+#if HAS_LEVELING
+  #include "../feature/bedlevel/bedlevel.h"
+#endif
+
+#if ENABLED(BLTOUCH)
+  #include "../feature/bltouch.h"
+#endif
+
+#if HAS_DISPLAY
+  #include "../lcd/marlinui.h"
+#endif
+
+#if HAS_FILAMENT_SENSOR
+  #include "../feature/runout.h"
+#endif
+
+#if ENABLED(SENSORLESS_HOMING)
+  #include "../feature/tmc_util.h"
+#endif
+
+#if ENABLED(FWRETRACT)
+  #include "../feature/fwretract.h"
+#endif
+
+#if ENABLED(BABYSTEP_DISPLAY_TOTAL)
+  #include "../feature/babystep.h"
+#endif
+
+#define DEBUG_OUT ENABLED(DEBUG_LEVELING_FEATURE)
+#include "../core/debug_out.h"
+
+/**
+ * axis_homed
+ *   Flags that each linear axis was homed.
+ *   XYZ on cartesian, ABC on delta, ABZ on SCARA.
+ *
+ * axis_trusted
+ *   Flags that the position is trusted in each linear axis. Set when homed.
+ *   Cleared whenever a stepper powers off, potentially losing its position.
+ */
+uint8_t axis_homed, axis_trusted; // = 0
+
+// Relative Mode. Enable with G91, disable with G90.
+bool relative_mode; // = false;
+
+/**
+ * Cartesian Current Position
+ *   Used to track the native machine position as moves are queued.
+ *   Used by 'line_to_current_position' to do a move after changing it.
+ *   Used by 'sync_plan_position' to update 'planner.position'.
+ */
+xyze_pos_t current_position = { X_HOME_POS, Y_HOME_POS, Z_HOME_POS };
+
+/**
+ * Cartesian Destination
+ *   The destination for a move, filled in by G-code movement commands,
+ *   and expected by functions like 'prepare_line_to_destination'.
+ *   G-codes can set destination using 'get_destination_from_command'
+ */
+xyze_pos_t destination; // {0}
+
+// G60/G61 Position Save and Return
+#if SAVED_POSITIONS
+  uint8_t saved_slots[(SAVED_POSITIONS + 7) >> 3];
+  xyz_pos_t stored_position[SAVED_POSITIONS];
+#endif
+
+// The active extruder (tool). Set with T<extruder> command.
+#if HAS_MULTI_EXTRUDER
+  uint8_t active_extruder = 0; // = 0
+#endif
+
+#if ENABLED(LCD_SHOW_E_TOTAL)
+  float e_move_accumulator; // = 0
+#endif
+
+// Extruder offsets
+#if HAS_HOTEND_OFFSET
+  xyz_pos_t hotend_offset[HOTENDS]; // Initialized by settings.load()
+  void reset_hotend_offsets() {
+    constexpr float tmp[XYZ][HOTENDS] = { HOTEND_OFFSET_X, HOTEND_OFFSET_Y, HOTEND_OFFSET_Z };
+    static_assert(
+      !tmp[X_AXIS][0] && !tmp[Y_AXIS][0] && !tmp[Z_AXIS][0],
+      "Offsets for the first hotend must be 0.0."
+    );
+    // Transpose from [XYZ][HOTENDS] to [HOTENDS][XYZ]
+    HOTEND_LOOP() LOOP_XYZ(a) hotend_offset[e][a] = tmp[a][e];
+    #if ENABLED(DUAL_X_CARRIAGE)
+      hotend_offset[1].x = _MAX(X2_HOME_POS, X2_MAX_POS);
+    #endif
+  }
+#endif
+
+// The feedrate for the current move, often used as the default if
+// no other feedrate is specified. Overridden for special moves.
+// Set by the last G0 through G5 command's "F" parameter.
+// Functions that override this for custom moves *must always* restore it!
+feedRate_t feedrate_mm_s = MMM_TO_MMS(1500);
+int16_t feedrate_percentage = 100;
+
+// Cartesian conversion result goes here:
+xyz_pos_t cartes;
+
+#if IS_KINEMATIC
+
+  abc_pos_t delta;
+
+  #if HAS_SCARA_OFFSET
+    abc_pos_t scara_home_offset;
+  #endif
+
+  #if HAS_SOFTWARE_ENDSTOPS
+    float delta_max_radius, delta_max_radius_2;
+  #elif IS_SCARA
+    constexpr float delta_max_radius = SCARA_PRINTABLE_RADIUS,
+                    delta_max_radius_2 = sq(SCARA_PRINTABLE_RADIUS);
+  #else // DELTA
+    constexpr float delta_max_radius = DELTA_PRINTABLE_RADIUS,
+                    delta_max_radius_2 = sq(DELTA_PRINTABLE_RADIUS);
+  #endif
+
+#endif
+
+/**
+ * The workspace can be offset by some commands, or
+ * these offsets may be omitted to save on computation.
+ */
+#if HAS_POSITION_SHIFT
+  // The distance that XYZ has been offset by G92. Reset by G28.
+  xyz_pos_t position_shift{0};
+#endif
+#if HAS_HOME_OFFSET
+  // This offset is added to the configured home position.
+  // Set by M206, M428, or menu item. Saved to EEPROM.
+  xyz_pos_t home_offset{0};
+#endif
+#if HAS_HOME_OFFSET && HAS_POSITION_SHIFT
+  // The above two are combined to save on computes
+  xyz_pos_t workspace_offset{0};
+#endif
+
+#if HAS_ABL_NOT_UBL
+  feedRate_t xy_probe_feedrate_mm_s = MMM_TO_MMS(XY_PROBE_SPEED);
+#endif
+
+/**
+ * Output the current position to serial
+ */
+
+inline void report_more_positions() {
+  stepper.report_positions();
+  TERN_(IS_SCARA, scara_report_positions());
+}
+
+// Report the logical position for a given machine position
+inline void report_logical_position(const xyze_pos_t &rpos) {
+  const xyze_pos_t lpos = rpos.asLogical();
+  SERIAL_ECHOPAIR_P(X_LBL, lpos.x, SP_Y_LBL, lpos.y, SP_Z_LBL, lpos.z, SP_E_LBL, lpos.e);
+}
+
+// Report the real current position according to the steppers.
+// Forward kinematics and un-leveling are applied.
+void report_real_position() {
+  get_cartesian_from_steppers();
+  xyze_pos_t npos = cartes;
+  npos.e = planner.get_axis_position_mm(E_AXIS);
+
+  #if HAS_POSITION_MODIFIERS
+    planner.unapply_modifiers(npos, true);
+  #endif
+
+  report_logical_position(npos);
+  report_more_positions();
+}
+
+// Report the logical current position according to the most recent G-code command
+void report_current_position() {
+  report_logical_position(current_position);
+  report_more_positions();
+}
+
+/**
+ * Report the logical current position according to the most recent G-code command.
+ * The planner.position always corresponds to the last G-code too. This makes M114
+ * suitable for debugging kinematics and leveling while avoiding planner sync that
+ * definitively interrupts the printing flow.
+ */
+void report_current_position_projected() {
+  report_logical_position(current_position);
+  stepper.report_a_position(planner.position);
+}
+
+/**
+ * Run out the planner buffer and re-sync the current
+ * position from the last-updated stepper positions.
+ */
+void quickstop_stepper() {
+  planner.quick_stop();
+  planner.synchronize();
+  set_current_from_steppers_for_axis(ALL_AXES);
+  sync_plan_position();
+}
+
+/**
+ * Set the planner/stepper positions directly from current_position with
+ * no kinematic translation. Used for homing axes and cartesian/core syncing.
+ */
+void sync_plan_position() {
+  if (DEBUGGING(LEVELING)) DEBUG_POS("sync_plan_position", current_position);
+  planner.set_position_mm(current_position);
+}
+
+void sync_plan_position_e() { planner.set_e_position_mm(current_position.e); }
+
+/**
+ * Get the stepper positions in the cartes[] array.
+ * Forward kinematics are applied for DELTA and SCARA.
+ *
+ * The result is in the current coordinate space with
+ * leveling applied. The coordinates need to be run through
+ * unapply_leveling to obtain the "ideal" coordinates
+ * suitable for current_position, etc.
+ */
+void get_cartesian_from_steppers() {
+  #if ENABLED(DELTA)
+    forward_kinematics_DELTA(planner.get_axis_positions_mm());
+  #else
+    #if IS_SCARA
+      forward_kinematics_SCARA(
+        planner.get_axis_position_degrees(A_AXIS),
+        planner.get_axis_position_degrees(B_AXIS)
+      );
+    #else
+      cartes.set(planner.get_axis_position_mm(X_AXIS), planner.get_axis_position_mm(Y_AXIS));
+    #endif
+    cartes.z = planner.get_axis_position_mm(Z_AXIS);
+  #endif
+}
+
+/**
+ * Set the current_position for an axis based on
+ * the stepper positions, removing any leveling that
+ * may have been applied.
+ *
+ * To prevent small shifts in axis position always call
+ * sync_plan_position after updating axes with this.
+ *
+ * To keep hosts in sync, always call report_current_position
+ * after updating the current_position.
+ */
+void set_current_from_steppers_for_axis(const AxisEnum axis) {
+  get_cartesian_from_steppers();
+  xyze_pos_t pos = cartes;
+  pos.e = planner.get_axis_position_mm(E_AXIS);
+
+  #if HAS_POSITION_MODIFIERS
+    planner.unapply_modifiers(pos, true);
+  #endif
+
+  if (axis == ALL_AXES)
+    current_position = pos;
+  else
+    current_position[axis] = pos[axis];
+}
+
+/**
+ * Move the planner to the current position from wherever it last moved
+ * (or from wherever it has been told it is located).
+ */
+void line_to_current_position(const feedRate_t &fr_mm_s/*=feedrate_mm_s*/) {
+  planner.buffer_line(current_position, fr_mm_s, active_extruder);
+}
+
+#if EXTRUDERS
+  void unscaled_e_move(const float &length, const feedRate_t &fr_mm_s) {
+    TERN_(HAS_FILAMENT_SENSOR, runout.reset());
+    current_position.e += length / planner.e_factor[active_extruder];
+    line_to_current_position(fr_mm_s);
+    planner.synchronize();
+  }
+#endif
+
+#if IS_KINEMATIC
+
+  /**
+   * Buffer a fast move without interpolation. Set current_position to destination
+   */
+  void prepare_fast_move_to_destination(const feedRate_t &scaled_fr_mm_s/*=MMS_SCALED(feedrate_mm_s)*/) {
+    if (DEBUGGING(LEVELING)) DEBUG_POS("prepare_fast_move_to_destination", destination);
+
+    #if UBL_SEGMENTED
+      // UBL segmented line will do Z-only moves in single segment
+      ubl.line_to_destination_segmented(scaled_fr_mm_s);
+    #else
+      if (current_position == destination) return;
+
+      planner.buffer_line(destination, scaled_fr_mm_s, active_extruder);
+    #endif
+
+    current_position = destination;
+  }
+
+#endif // IS_KINEMATIC
+
+/**
+ * Do a fast or normal move to 'destination' with an optional FR.
+ *  - Move at normal speed regardless of feedrate percentage.
+ *  - Extrude the specified length regardless of flow percentage.
+ */
+void _internal_move_to_destination(const feedRate_t &fr_mm_s/*=0.0f*/
+  #if IS_KINEMATIC
+    , const bool is_fast/*=false*/
+  #endif
+) {
+  const feedRate_t old_feedrate = feedrate_mm_s;
+  if (fr_mm_s) feedrate_mm_s = fr_mm_s;
+
+  const uint16_t old_pct = feedrate_percentage;
+  feedrate_percentage = 100;
+
+  #if EXTRUDERS
+    const float old_fac = planner.e_factor[active_extruder];
+    planner.e_factor[active_extruder] = 1.0f;
+  #endif
+
+  #if IS_KINEMATIC
+    if (is_fast)
+      prepare_fast_move_to_destination();
+    else
+  #endif
+      prepare_line_to_destination();
+
+  feedrate_mm_s = old_feedrate;
+  feedrate_percentage = old_pct;
+  #if EXTRUDERS
+    planner.e_factor[active_extruder] = old_fac;
+  #endif
+}
+
+/**
+ * Plan a move to (X, Y, Z) and set the current_position
+ */
+void do_blocking_move_to(const float rx, const float ry, const float rz, const feedRate_t &fr_mm_s/*=0.0*/) {
+  DEBUG_SECTION(log_move, "do_blocking_move_to", DEBUGGING(LEVELING));
+  if (DEBUGGING(LEVELING)) DEBUG_XYZ("> ", rx, ry, rz);
+
+  const feedRate_t z_feedrate = fr_mm_s ?: homing_feedrate(Z_AXIS),
+                  xy_feedrate = fr_mm_s ?: feedRate_t(XY_PROBE_FEEDRATE_MM_S);
+
+  #if ENABLED(DELTA)
+
+    if (!position_is_reachable(rx, ry)) return;
+
+    REMEMBER(fr, feedrate_mm_s, xy_feedrate);
+
+    destination = current_position;          // sync destination at the start
+
+    if (DEBUGGING(LEVELING)) DEBUG_POS("destination = current_position", destination);
+
+    // when in the danger zone
+    if (current_position.z > delta_clip_start_height) {
+      if (rz > delta_clip_start_height) {   // staying in the danger zone
+        destination.set(rx, ry, rz);        // move directly (uninterpolated)
+        prepare_internal_fast_move_to_destination();          // set current_position from destination
+        if (DEBUGGING(LEVELING)) DEBUG_POS("danger zone move", current_position);
+        return;
+      }
+      destination.z = delta_clip_start_height;
+      prepare_internal_fast_move_to_destination();            // set current_position from destination
+      if (DEBUGGING(LEVELING)) DEBUG_POS("zone border move", current_position);
+    }
+
+    if (rz > current_position.z) {                            // raising?
+      destination.z = rz;
+      prepare_internal_fast_move_to_destination(z_feedrate);  // set current_position from destination
+      if (DEBUGGING(LEVELING)) DEBUG_POS("z raise move", current_position);
+    }
+
+    destination.set(rx, ry);
+    prepare_internal_move_to_destination();                   // set current_position from destination
+    if (DEBUGGING(LEVELING)) DEBUG_POS("xy move", current_position);
+
+    if (rz < current_position.z) {                            // lowering?
+      destination.z = rz;
+      prepare_internal_fast_move_to_destination(z_feedrate);  // set current_position from destination
+      if (DEBUGGING(LEVELING)) DEBUG_POS("z lower move", current_position);
+    }
+
+  #elif IS_SCARA
+
+    if (!position_is_reachable(rx, ry)) return;
+
+    destination = current_position;
+
+    // If Z needs to raise, do it before moving XY
+    if (destination.z < rz) {
+      destination.z = rz;
+      prepare_internal_fast_move_to_destination(z_feedrate);
+    }
+
+    destination.set(rx, ry);
+    prepare_internal_fast_move_to_destination(xy_feedrate);
+
+    // If Z needs to lower, do it after moving XY
+    if (destination.z > rz) {
+      destination.z = rz;
+      prepare_internal_fast_move_to_destination(z_feedrate);
+    }
+
+  #else
+
+    // If Z needs to raise, do it before moving XY
+    if (current_position.z < rz) {
+      current_position.z = rz;
+      line_to_current_position(z_feedrate);
+    }
+
+    current_position.set(rx, ry);
+    line_to_current_position(xy_feedrate);
+
+    // If Z needs to lower, do it after moving XY
+    if (current_position.z > rz) {
+      current_position.z = rz;
+      line_to_current_position(z_feedrate);
+    }
+
+  #endif
+
+  planner.synchronize();
+}
+
+void do_blocking_move_to(const xy_pos_t &raw, const feedRate_t &fr_mm_s/*=0.0f*/) {
+  do_blocking_move_to(raw.x, raw.y, current_position.z, fr_mm_s);
+}
+void do_blocking_move_to(const xyz_pos_t &raw, const feedRate_t &fr_mm_s/*=0.0f*/) {
+  do_blocking_move_to(raw.x, raw.y, raw.z, fr_mm_s);
+}
+void do_blocking_move_to(const xyze_pos_t &raw, const feedRate_t &fr_mm_s/*=0.0f*/) {
+  do_blocking_move_to(raw.x, raw.y, raw.z, fr_mm_s);
+}
+
+void do_blocking_move_to_x(const float &rx, const feedRate_t &fr_mm_s/*=0.0*/) {
+  do_blocking_move_to(rx, current_position.y, current_position.z, fr_mm_s);
+}
+void do_blocking_move_to_y(const float &ry, const feedRate_t &fr_mm_s/*=0.0*/) {
+  do_blocking_move_to(current_position.x, ry, current_position.z, fr_mm_s);
+}
+void do_blocking_move_to_z(const float &rz, const feedRate_t &fr_mm_s/*=0.0*/) {
+  do_blocking_move_to_xy_z(current_position, rz, fr_mm_s);
+}
+
+void do_blocking_move_to_xy(const float &rx, const float &ry, const feedRate_t &fr_mm_s/*=0.0*/) {
+  do_blocking_move_to(rx, ry, current_position.z, fr_mm_s);
+}
+void do_blocking_move_to_xy(const xy_pos_t &raw, const feedRate_t &fr_mm_s/*=0.0f*/) {
+  do_blocking_move_to_xy(raw.x, raw.y, fr_mm_s);
+}
+
+void do_blocking_move_to_xy_z(const xy_pos_t &raw, const float &z, const feedRate_t &fr_mm_s/*=0.0f*/) {
+  do_blocking_move_to(raw.x, raw.y, z, fr_mm_s);
+}
+
+void do_z_clearance(const float &zclear, const bool z_trusted/*=true*/, const bool raise_on_untrusted/*=true*/, const bool lower_allowed/*=false*/) {
+  const bool rel = raise_on_untrusted && !z_trusted;
+  float zdest = zclear + (rel ? current_position.z : 0.0f);
+  if (!lower_allowed) NOLESS(zdest, current_position.z);
+  do_blocking_move_to_z(_MIN(zdest, Z_MAX_POS), TERN(HAS_BED_PROBE, z_probe_fast_mm_s, homing_feedrate(Z_AXIS)));
+}
+
+//
+// Prepare to do endstop or probe moves with custom feedrates.
+//  - Save / restore current feedrate and multiplier
+//
+static float saved_feedrate_mm_s;
+static int16_t saved_feedrate_percentage;
+void remember_feedrate_and_scaling() {
+  saved_feedrate_mm_s = feedrate_mm_s;
+  saved_feedrate_percentage = feedrate_percentage;
+}
+void remember_feedrate_scaling_off() {
+  remember_feedrate_and_scaling();
+  feedrate_percentage = 100;
+}
+void restore_feedrate_and_scaling() {
+  feedrate_mm_s = saved_feedrate_mm_s;
+  feedrate_percentage = saved_feedrate_percentage;
+}
+
+#if HAS_SOFTWARE_ENDSTOPS
+
+  // Software Endstops are based on the configured limits.
+  soft_endstops_t soft_endstop = {
+    true, false,
+    { X_MIN_POS, Y_MIN_POS, Z_MIN_POS },
+    { X_MAX_POS, Y_MAX_POS, Z_MAX_POS }
+  };
+
+  /**
+   * Software endstops can be used to monitor the open end of
+   * an axis that has a hardware endstop on the other end. Or
+   * they can prevent axes from moving past endstops and grinding.
+   *
+   * To keep doing their job as the coordinate system changes,
+   * the software endstop positions must be refreshed to remain
+   * at the same positions relative to the machine.
+   */
+  void update_software_endstops(const AxisEnum axis
+    #if HAS_HOTEND_OFFSET
+      , const uint8_t old_tool_index/*=0*/
+      , const uint8_t new_tool_index/*=0*/
+    #endif
+  ) {
+
+    #if ENABLED(DUAL_X_CARRIAGE)
+
+      if (axis == X_AXIS) {
+
+        // In Dual X mode hotend_offset[X] is T1's home position
+        const float dual_max_x = _MAX(hotend_offset[1].x, X2_MAX_POS);
+
+        if (new_tool_index != 0) {
+          // T1 can move from X2_MIN_POS to X2_MAX_POS or X2 home position (whichever is larger)
+          soft_endstop.min.x = X2_MIN_POS;
+          soft_endstop.max.x = dual_max_x;
+        }
+        else if (idex_is_duplicating()) {
+          // In Duplication Mode, T0 can move as far left as X1_MIN_POS
+          // but not so far to the right that T1 would move past the end
+          soft_endstop.min.x = X1_MIN_POS;
+          soft_endstop.max.x = _MIN(X1_MAX_POS, dual_max_x - duplicate_extruder_x_offset);
+        }
+        else {
+          // In other modes, T0 can move from X1_MIN_POS to X1_MAX_POS
+          soft_endstop.min.x = X1_MIN_POS;
+          soft_endstop.max.x = X1_MAX_POS;
+        }
+
+      }
+
+    #elif ENABLED(DELTA)
+
+      soft_endstop.min[axis] = base_min_pos(axis);
+      soft_endstop.max[axis] = (axis == Z_AXIS) ? delta_height - TERN0(HAS_BED_PROBE, probe.offset.z) : base_max_pos(axis);
+
+      switch (axis) {
+        case X_AXIS:
+        case Y_AXIS:
+          // Get a minimum radius for clamping
+          delta_max_radius = _MIN(ABS(_MAX(soft_endstop.min.x, soft_endstop.min.y)), soft_endstop.max.x, soft_endstop.max.y);
+          delta_max_radius_2 = sq(delta_max_radius);
+          break;
+        case Z_AXIS:
+          delta_clip_start_height = soft_endstop.max[axis] - delta_safe_distance_from_top();
+        default: break;
+      }
+
+    #elif HAS_HOTEND_OFFSET
+
+      // Software endstops are relative to the tool 0 workspace, so
+      // the movement limits must be shifted by the tool offset to
+      // retain the same physical limit when other tools are selected.
+
+      if (new_tool_index == old_tool_index || axis == Z_AXIS) { // The Z axis is "special" and shouldn't be modified
+        const float offs = (axis == Z_AXIS) ? 0 : hotend_offset[active_extruder][axis];
+        soft_endstop.min[axis] = base_min_pos(axis) + offs;
+        soft_endstop.max[axis] = base_max_pos(axis) + offs;
+      }
+      else {
+        const float diff = hotend_offset[new_tool_index][axis] - hotend_offset[old_tool_index][axis];
+        soft_endstop.min[axis] += diff;
+        soft_endstop.max[axis] += diff;
+      }
+
+    #else
+
+      soft_endstop.min[axis] = base_min_pos(axis);
+      soft_endstop.max[axis] = base_max_pos(axis);
+
+    #endif
+
+    if (DEBUGGING(LEVELING))
+      SERIAL_ECHOLNPAIR("Axis ", XYZ_CHAR(axis), " min:", soft_endstop.min[axis], " max:", soft_endstop.max[axis]);
+  }
+
+  /**
+   * Constrain the given coordinates to the software endstops.
+   *
+   * For DELTA/SCARA the XY constraint is based on the smallest
+   * radius within the set software endstops.
+   */
+  void apply_motion_limits(xyz_pos_t &target) {
+
+    if (!soft_endstop._enabled) return;
+
+    #if IS_KINEMATIC
+
+      if (TERN0(DELTA, !all_axes_homed())) return;
+
+      #if BOTH(HAS_HOTEND_OFFSET, DELTA)
+        // The effector center position will be the target minus the hotend offset.
+        const xy_pos_t offs = hotend_offset[active_extruder];
+      #else
+        // SCARA needs to consider the angle of the arm through the entire move, so for now use no tool offset.
+        constexpr xy_pos_t offs{0};
+      #endif
+
+      if (TERN1(IS_SCARA, axis_was_homed(X_AXIS) && axis_was_homed(Y_AXIS))) {
+        const float dist_2 = HYPOT2(target.x - offs.x, target.y - offs.y);
+        if (dist_2 > delta_max_radius_2)
+          target *= float(delta_max_radius / SQRT(dist_2)); // 200 / 300 = 0.66
+      }
+
+    #else
+
+      if (axis_was_homed(X_AXIS)) {
+        #if !HAS_SOFTWARE_ENDSTOPS || ENABLED(MIN_SOFTWARE_ENDSTOP_X)
+          NOLESS(target.x, soft_endstop.min.x);
+        #endif
+        #if !HAS_SOFTWARE_ENDSTOPS || ENABLED(MAX_SOFTWARE_ENDSTOP_X)
+          NOMORE(target.x, soft_endstop.max.x);
+        #endif
+      }
+
+      if (axis_was_homed(Y_AXIS)) {
+        #if !HAS_SOFTWARE_ENDSTOPS || ENABLED(MIN_SOFTWARE_ENDSTOP_Y)
+          NOLESS(target.y, soft_endstop.min.y);
+        #endif
+        #if !HAS_SOFTWARE_ENDSTOPS || ENABLED(MAX_SOFTWARE_ENDSTOP_Y)
+          NOMORE(target.y, soft_endstop.max.y);
+        #endif
+      }
+
+    #endif
+
+    if (axis_was_homed(Z_AXIS)) {
+      #if !HAS_SOFTWARE_ENDSTOPS || ENABLED(MIN_SOFTWARE_ENDSTOP_Z)
+        NOLESS(target.z, soft_endstop.min.z);
+      #endif
+      #if !HAS_SOFTWARE_ENDSTOPS || ENABLED(MAX_SOFTWARE_ENDSTOP_Z)
+        NOMORE(target.z, soft_endstop.max.z);
+      #endif
+    }
+  }
+
+#else // !HAS_SOFTWARE_ENDSTOPS
+
+  soft_endstops_t soft_endstop;
+
+#endif // !HAS_SOFTWARE_ENDSTOPS
+
+#if !UBL_SEGMENTED
+
+FORCE_INLINE void segment_idle(millis_t &next_idle_ms) {
+  const millis_t ms = millis();
+  if (ELAPSED(ms, next_idle_ms)) {
+    next_idle_ms = ms + 200UL;
+    return idle();
+  }
+  thermalManager.manage_heater();  // Returns immediately on most calls
+}
+
+#if IS_KINEMATIC
+
+  #if IS_SCARA
+    /**
+     * Before raising this value, use M665 S[seg_per_sec] to decrease
+     * the number of segments-per-second. Default is 200. Some deltas
+     * do better with 160 or lower. It would be good to know how many
+     * segments-per-second are actually possible for SCARA on AVR.
+     *
+     * Longer segments result in less kinematic overhead
+     * but may produce jagged lines. Try 0.5mm, 1.0mm, and 2.0mm
+     * and compare the difference.
+     */
+    #define SCARA_MIN_SEGMENT_LENGTH 0.5f
+  #endif
+
+  /**
+   * Prepare a linear move in a DELTA or SCARA setup.
+   *
+   * Called from prepare_line_to_destination as the
+   * default Delta/SCARA segmenter.
+   *
+   * This calls planner.buffer_line several times, adding
+   * small incremental moves for DELTA or SCARA.
+   *
+   * For Unified Bed Leveling (Delta or Segmented Cartesian)
+   * the ubl.line_to_destination_segmented method replaces this.
+   *
+   * For Auto Bed Leveling (Bilinear) with SEGMENT_LEVELED_MOVES
+   * this is replaced by segmented_line_to_destination below.
+   */
+  inline bool line_to_destination_kinematic() {
+
+    // Get the top feedrate of the move in the XY plane
+    const float scaled_fr_mm_s = MMS_SCALED(feedrate_mm_s);
+
+    const xyze_float_t diff = destination - current_position;
+
+    // If the move is only in Z/E don't split up the move
+    if (!diff.x && !diff.y) {
+      planner.buffer_line(destination, scaled_fr_mm_s, active_extruder);
+      return false; // caller will update current_position
+    }
+
+    // Fail if attempting move outside printable radius
+    if (!position_is_reachable(destination)) return true;
+
+    // Get the linear distance in XYZ
+    float cartesian_mm = diff.magnitude();
+
+    // If the move is very short, check the E move distance
+    if (UNEAR_ZERO(cartesian_mm)) cartesian_mm = ABS(diff.e);
+
+    // No E move either? Game over.
+    if (UNEAR_ZERO(cartesian_mm)) return true;
+
+    // Minimum number of seconds to move the given distance
+    const float seconds = cartesian_mm / scaled_fr_mm_s;
+
+    // The number of segments-per-second times the duration
+    // gives the number of segments
+    uint16_t segments = delta_segments_per_second * seconds;
+
+    // For SCARA enforce a minimum segment size
+    #if IS_SCARA
+      NOMORE(segments, cartesian_mm * RECIPROCAL(SCARA_MIN_SEGMENT_LENGTH));
+    #endif
+
+    // At least one segment is required
+    NOLESS(segments, 1U);
+
+    // The approximate length of each segment
+    const float inv_segments = 1.0f / float(segments),
+                cartesian_segment_mm = cartesian_mm * inv_segments;
+    const xyze_float_t segment_distance = diff * inv_segments;
+
+    #if ENABLED(SCARA_FEEDRATE_SCALING)
+      const float inv_duration = scaled_fr_mm_s / cartesian_segment_mm;
+    #endif
+
+    /*
+    SERIAL_ECHOPAIR("mm=", cartesian_mm);
+    SERIAL_ECHOPAIR(" seconds=", seconds);
+    SERIAL_ECHOPAIR(" segments=", segments);
+    SERIAL_ECHOPAIR(" segment_mm=", cartesian_segment_mm);
+    SERIAL_EOL();
+    //*/
+
+    // Get the current position as starting point
+    xyze_pos_t raw = current_position;
+
+    // Calculate and execute the segments
+    millis_t next_idle_ms = millis() + 200UL;
+    while (--segments) {
+      segment_idle(next_idle_ms);
+      raw += segment_distance;
+      if (!planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, cartesian_segment_mm
+        #if ENABLED(SCARA_FEEDRATE_SCALING)
+          , inv_duration
+        #endif
+      )) break;
+    }
+
+    // Ensure last segment arrives at target location.
+    planner.buffer_line(destination, scaled_fr_mm_s, active_extruder, cartesian_segment_mm
+      #if ENABLED(SCARA_FEEDRATE_SCALING)
+        , inv_duration
+      #endif
+    );
+
+    return false; // caller will update current_position
+  }
+
+#else // !IS_KINEMATIC
+
+  #if ENABLED(SEGMENT_LEVELED_MOVES)
+
+    /**
+     * Prepare a segmented move on a CARTESIAN setup.
+     *
+     * This calls planner.buffer_line several times, adding
+     * small incremental moves. This allows the planner to
+     * apply more detailed bed leveling to the full move.
+     */
+    inline void segmented_line_to_destination(const feedRate_t &fr_mm_s, const float segment_size=LEVELED_SEGMENT_LENGTH) {
+
+      const xyze_float_t diff = destination - current_position;
+
+      // If the move is only in Z/E don't split up the move
+      if (!diff.x && !diff.y) {
+        planner.buffer_line(destination, fr_mm_s, active_extruder);
+        return;
+      }
+
+      // Get the linear distance in XYZ
+      // If the move is very short, check the E move distance
+      // No E move either? Game over.
+      float cartesian_mm = diff.magnitude();
+      if (UNEAR_ZERO(cartesian_mm)) cartesian_mm = ABS(diff.e);
+      if (UNEAR_ZERO(cartesian_mm)) return;
+
+      // The length divided by the segment size
+      // At least one segment is required
+      uint16_t segments = cartesian_mm / segment_size;
+      NOLESS(segments, 1U);
+
+      // The approximate length of each segment
+      const float inv_segments = 1.0f / float(segments),
+                  cartesian_segment_mm = cartesian_mm * inv_segments;
+      const xyze_float_t segment_distance = diff * inv_segments;
+
+      #if ENABLED(SCARA_FEEDRATE_SCALING)
+        const float inv_duration = scaled_fr_mm_s / cartesian_segment_mm;
+      #endif
+
+      // SERIAL_ECHOPAIR("mm=", cartesian_mm);
+      // SERIAL_ECHOLNPAIR(" segments=", segments);
+      // SERIAL_ECHOLNPAIR(" segment_mm=", cartesian_segment_mm);
+
+      // Get the raw current position as starting point
+      xyze_pos_t raw = current_position;
+
+      // Calculate and execute the segments
+      millis_t next_idle_ms = millis() + 200UL;
+      while (--segments) {
+        segment_idle(next_idle_ms);
+        raw += segment_distance;
+        if (!planner.buffer_line(raw, fr_mm_s, active_extruder, cartesian_segment_mm
+          #if ENABLED(SCARA_FEEDRATE_SCALING)
+            , inv_duration
+          #endif
+        )) break;
+      }
+
+      // Since segment_distance is only approximate,
+      // the final move must be to the exact destination.
+      planner.buffer_line(destination, fr_mm_s, active_extruder, cartesian_segment_mm
+        #if ENABLED(SCARA_FEEDRATE_SCALING)
+          , inv_duration
+        #endif
+      );
+    }
+
+  #endif // SEGMENT_LEVELED_MOVES
+
+  /**
+   * Prepare a linear move in a Cartesian setup.
+   *
+   * When a mesh-based leveling system is active, moves are segmented
+   * according to the configuration of the leveling system.
+   *
+   * Return true if 'current_position' was set to 'destination'
+   */
+  inline bool line_to_destination_cartesian() {
+    const float scaled_fr_mm_s = MMS_SCALED(feedrate_mm_s);
+    #if HAS_MESH
+      if (planner.leveling_active && planner.leveling_active_at_z(destination.z)) {
+        #if ENABLED(AUTO_BED_LEVELING_UBL)
+          ubl.line_to_destination_cartesian(scaled_fr_mm_s, active_extruder); // UBL's motion routine needs to know about
+          return true;                                                        // all moves, including Z-only moves.
+        #elif ENABLED(SEGMENT_LEVELED_MOVES)
+          segmented_line_to_destination(scaled_fr_mm_s);
+          return false; // caller will update current_position
+        #else
+          /**
+           * For MBL and ABL-BILINEAR only segment moves when X or Y are involved.
+           * Otherwise fall through to do a direct single move.
+           */
+          if (xy_pos_t(current_position) != xy_pos_t(destination)) {
+            #if ENABLED(MESH_BED_LEVELING)
+              mbl.line_to_destination(scaled_fr_mm_s);
+            #elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
+              bilinear_line_to_destination(scaled_fr_mm_s);
+            #endif
+            return true;
+          }
+        #endif
+      }
+    #endif // HAS_MESH
+
+    planner.buffer_line(destination, scaled_fr_mm_s, active_extruder);
+    return false; // caller will update current_position
+  }
+
+#endif // !IS_KINEMATIC
+#endif // !UBL_SEGMENTED
+
+#if HAS_DUPLICATION_MODE
+  bool extruder_duplication_enabled;
+  #if ENABLED(MULTI_NOZZLE_DUPLICATION)
+    uint8_t duplication_e_mask; // = 0
+  #endif
+#endif
+
+#if ENABLED(DUAL_X_CARRIAGE)
+
+  DualXMode dual_x_carriage_mode         = DEFAULT_DUAL_X_CARRIAGE_MODE;
+  float inactive_extruder_x              = X2_MAX_POS,                    // Used in mode 0 & 1
+        duplicate_extruder_x_offset      = DEFAULT_DUPLICATION_X_OFFSET;  // Used in mode 2
+  xyz_pos_t raised_parked_position;                                       // Used in mode 1
+  bool active_extruder_parked            = false;                         // Used in mode 1 & 2
+  millis_t delayed_move_time             = 0;                             // Used in mode 1
+  int16_t duplicate_extruder_temp_offset = 0;                             // Used in mode 2
+  bool idex_mirrored_mode                = false;                         // Used in mode 3
+
+  float x_home_pos(const uint8_t extruder) {
+    if (extruder == 0)
+      return base_home_pos(X_AXIS);
+    else
+      /**
+       * In dual carriage mode the extruder offset provides an override of the
+       * second X-carriage position when homed - otherwise X2_HOME_POS is used.
+       * This allows soft recalibration of the second extruder home position
+       * without firmware reflash (through the M218 command).
+       */
+      return hotend_offset[1].x > 0 ? hotend_offset[1].x : X2_HOME_POS;
+  }
+
+  void idex_set_mirrored_mode(const bool mirr) {
+    idex_mirrored_mode = mirr;
+    stepper.set_directions();
+  }
+
+  void set_duplication_enabled(const bool dupe, const int8_t tool_index/*=-1*/) {
+    extruder_duplication_enabled = dupe;
+    if (tool_index >= 0) active_extruder = tool_index;
+    stepper.set_directions();
+  }
+
+  void idex_set_parked(const bool park/*=true*/) {
+    delayed_move_time = 0;
+    active_extruder_parked = park;
+    if (park) raised_parked_position = current_position;  // Remember current raised toolhead position for use by unpark
+  }
+
+  /**
+   * Prepare a linear move in a dual X axis setup
+   *
+   * Return true if current_position[] was set to destination[]
+   */
+  inline bool dual_x_carriage_unpark() {
+    if (active_extruder_parked) {
+      switch (dual_x_carriage_mode) {
+
+        case DXC_FULL_CONTROL_MODE: break;
+
+        case DXC_AUTO_PARK_MODE: {
+          if (current_position.e == destination.e) {
+            // This is a travel move (with no extrusion)
+            // Skip it, but keep track of the current position
+            // (so it can be used as the start of the next non-travel move)
+            if (delayed_move_time != 0xFFFFFFFFUL) {
+              current_position = destination;
+              NOLESS(raised_parked_position.z, destination.z);
+              delayed_move_time = millis() + 1000UL;
+              return true;
+            }
+          }
+          //
+          // Un-park the active extruder
+          //
+          const feedRate_t fr_zfast = planner.settings.max_feedrate_mm_s[Z_AXIS];
+          #define CURPOS current_position
+          #define RAISED raised_parked_position
+          //  1. Move to the raised parked XYZ. Presumably the tool is already at XY.
+          if (planner.buffer_line(RAISED.x, RAISED.y, RAISED.z, CURPOS.e, fr_zfast, active_extruder)) {
+            //  2. Move to the current native XY and raised Z. Presumably this is a null move.
+            if (planner.buffer_line(CURPOS.x, CURPOS.y, RAISED.z, CURPOS.e, PLANNER_XY_FEEDRATE(), active_extruder)) {
+              //  3. Lower Z back down
+              line_to_current_position(fr_zfast);
+            }
+          }
+          stepper.set_directions();
+
+          idex_set_parked(false);
+          if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("idex_set_parked(false)");
+        } break;
+
+        case DXC_MIRRORED_MODE:
+        case DXC_DUPLICATION_MODE:
+          if (active_extruder == 0) {
+            xyze_pos_t new_pos = current_position;
+            if (dual_x_carriage_mode == DXC_DUPLICATION_MODE)
+              new_pos.x += duplicate_extruder_x_offset;
+            else
+              new_pos.x = inactive_extruder_x;
+            // Move duplicate extruder into correct duplication position.
+            if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("Set planner X", inactive_extruder_x, " ... Line to X", new_pos.x);
+            planner.set_position_mm(inactive_extruder_x, current_position.y, current_position.z, current_position.e);
+            if (!planner.buffer_line(new_pos, planner.settings.max_feedrate_mm_s[X_AXIS], 1)) break;
+
+            planner.synchronize();
+            sync_plan_position();
+
+            set_duplication_enabled(true);
+            idex_set_parked(false);
+            if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("set_duplication_enabled(true)\nidex_set_parked(false)");
+          }
+          else if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Active extruder not 0");
+          break;
+      }
+    }
+    return false;
+  }
+
+#endif // DUAL_X_CARRIAGE
+
+/**
+ * Prepare a single move and get ready for the next one
+ *
+ * This may result in several calls to planner.buffer_line to
+ * do smaller moves for DELTA, SCARA, mesh moves, etc.
+ *
+ * Make sure current_position.e and destination.e are good
+ * before calling or cold/lengthy extrusion may get missed.
+ *
+ * Before exit, current_position is set to destination.
+ */
+void prepare_line_to_destination() {
+  apply_motion_limits(destination);
+
+  #if EITHER(PREVENT_COLD_EXTRUSION, PREVENT_LENGTHY_EXTRUDE)
+
+    if (!DEBUGGING(DRYRUN) && destination.e != current_position.e) {
+      bool ignore_e = false;
+
+      #if ENABLED(PREVENT_COLD_EXTRUSION)
+        ignore_e = thermalManager.tooColdToExtrude(active_extruder);
+        if (ignore_e) SERIAL_ECHO_MSG(STR_ERR_COLD_EXTRUDE_STOP);
+      #endif
+
+      #if ENABLED(PREVENT_LENGTHY_EXTRUDE)
+        const float e_delta = ABS(destination.e - current_position.e) * planner.e_factor[active_extruder];
+        if (e_delta > (EXTRUDE_MAXLENGTH)) {
+          #if ENABLED(MIXING_EXTRUDER)
+            float collector[MIXING_STEPPERS];
+            mixer.refresh_collector(1.0, mixer.get_current_vtool(), collector);
+            MIXER_STEPPER_LOOP(e) {
+              if (e_delta * collector[e] > (EXTRUDE_MAXLENGTH)) {
+                ignore_e = true;
+                SERIAL_ECHO_MSG(STR_ERR_LONG_EXTRUDE_STOP);
+                break;
+              }
+            }
+          #else
+            ignore_e = true;
+            SERIAL_ECHO_MSG(STR_ERR_LONG_EXTRUDE_STOP);
+          #endif
+        }
+      #endif
+
+      if (ignore_e) {
+        current_position.e = destination.e;       // Behave as if the E move really took place
+        planner.set_e_position_mm(destination.e); // Prevent the planner from complaining too
+      }
+    }
+
+  #endif // PREVENT_COLD_EXTRUSION || PREVENT_LENGTHY_EXTRUDE
+
+  if (TERN0(DUAL_X_CARRIAGE, dual_x_carriage_unpark())) return;
+
+  if (
+    #if UBL_SEGMENTED
+      #if IS_KINEMATIC // UBL using Kinematic / Cartesian cases as a workaround for now.
+        ubl.line_to_destination_segmented(MMS_SCALED(feedrate_mm_s))
+      #else
+        line_to_destination_cartesian()
+      #endif
+    #elif IS_KINEMATIC
+      line_to_destination_kinematic()
+    #else
+      line_to_destination_cartesian()
+    #endif
+  ) return;
+
+  current_position = destination;
+}
+
+uint8_t axes_should_home(uint8_t axis_bits/*=0x07*/) {
+  #define SHOULD_HOME(A) TERN(HOME_AFTER_DEACTIVATE, axis_is_trusted, axis_was_homed)(A)
+  // Clear test bits that are trusted
+  if (TEST(axis_bits, X_AXIS) && SHOULD_HOME(X_AXIS)) CBI(axis_bits, X_AXIS);
+  if (TEST(axis_bits, Y_AXIS) && SHOULD_HOME(Y_AXIS)) CBI(axis_bits, Y_AXIS);
+  if (TEST(axis_bits, Z_AXIS) && SHOULD_HOME(Z_AXIS)) CBI(axis_bits, Z_AXIS);
+  return axis_bits;
+}
+
+bool homing_needed_error(uint8_t axis_bits/*=0x07*/) {
+  if ((axis_bits = axes_should_home(axis_bits))) {
+    PGM_P home_first = GET_TEXT(MSG_HOME_FIRST);
+    char msg[strlen_P(home_first)+1];
+    sprintf_P(msg, home_first,
+      TEST(axis_bits, X_AXIS) ? "X" : "",
+      TEST(axis_bits, Y_AXIS) ? "Y" : "",
+      TEST(axis_bits, Z_AXIS) ? "Z" : ""
+    );
+    SERIAL_ECHO_START();
+    SERIAL_ECHOLN(msg);
+    TERN_(HAS_DISPLAY, ui.set_status(msg));
+    return true;
+  }
+  return false;
+}
+
+/**
+ * Homing bump feedrate (mm/s)
+ */
+feedRate_t get_homing_bump_feedrate(const AxisEnum axis) {
+  #if HOMING_Z_WITH_PROBE
+    if (axis == Z_AXIS) return MMM_TO_MMS(Z_PROBE_SPEED_SLOW);
+  #endif
+  static const uint8_t homing_bump_divisor[] PROGMEM = HOMING_BUMP_DIVISOR;
+  uint8_t hbd = pgm_read_byte(&homing_bump_divisor[axis]);
+  if (hbd < 1) {
+    hbd = 10;
+    SERIAL_ECHO_MSG("Warning: Homing Bump Divisor < 1");
+  }
+  return homing_feedrate(axis) / float(hbd);
+}
+
+#if ENABLED(SENSORLESS_HOMING)
+  /**
+   * Set sensorless homing if the axis has it, accounting for Core Kinematics.
+   */
+  sensorless_t start_sensorless_homing_per_axis(const AxisEnum axis) {
+    sensorless_t stealth_states { false };
+
+    switch (axis) {
+      default: break;
+      #if X_SENSORLESS
+        case X_AXIS:
+          stealth_states.x = tmc_enable_stallguard(stepperX);
+          #if AXIS_HAS_STALLGUARD(X2)
+            stealth_states.x2 = tmc_enable_stallguard(stepperX2);
+          #endif
+          #if EITHER(CORE_IS_XY, MARKFORGED_XY) && Y_SENSORLESS
+            stealth_states.y = tmc_enable_stallguard(stepperY);
+          #elif CORE_IS_XZ && Z_SENSORLESS
+            stealth_states.z = tmc_enable_stallguard(stepperZ);
+          #endif
+          break;
+      #endif
+      #if Y_SENSORLESS
+        case Y_AXIS:
+          stealth_states.y = tmc_enable_stallguard(stepperY);
+          #if AXIS_HAS_STALLGUARD(Y2)
+            stealth_states.y2 = tmc_enable_stallguard(stepperY2);
+          #endif
+          #if EITHER(CORE_IS_XY, MARKFORGED_XY) && X_SENSORLESS
+            stealth_states.x = tmc_enable_stallguard(stepperX);
+          #elif CORE_IS_YZ && Z_SENSORLESS
+            stealth_states.z = tmc_enable_stallguard(stepperZ);
+          #endif
+          break;
+      #endif
+      #if Z_SENSORLESS
+        case Z_AXIS:
+          stealth_states.z = tmc_enable_stallguard(stepperZ);
+          #if AXIS_HAS_STALLGUARD(Z2)
+            stealth_states.z2 = tmc_enable_stallguard(stepperZ2);
+          #endif
+          #if AXIS_HAS_STALLGUARD(Z3)
+            stealth_states.z3 = tmc_enable_stallguard(stepperZ3);
+          #endif
+          #if AXIS_HAS_STALLGUARD(Z4)
+            stealth_states.z4 = tmc_enable_stallguard(stepperZ4);
+          #endif
+          #if CORE_IS_XZ && X_SENSORLESS
+            stealth_states.x = tmc_enable_stallguard(stepperX);
+          #elif CORE_IS_YZ && Y_SENSORLESS
+            stealth_states.y = tmc_enable_stallguard(stepperY);
+          #endif
+          break;
+      #endif
+    }
+
+    #if ENABLED(SPI_ENDSTOPS)
+      switch (axis) {
+        case X_AXIS: if (ENABLED(X_SPI_SENSORLESS)) endstops.tmc_spi_homing.x = true; break;
+        case Y_AXIS: if (ENABLED(Y_SPI_SENSORLESS)) endstops.tmc_spi_homing.y = true; break;
+        case Z_AXIS: if (ENABLED(Z_SPI_SENSORLESS)) endstops.tmc_spi_homing.z = true; break;
+        default: break;
+      }
+    #endif
+
+    TERN_(IMPROVE_HOMING_RELIABILITY, sg_guard_period = millis() + default_sg_guard_duration);
+
+    return stealth_states;
+  }
+
+  void end_sensorless_homing_per_axis(const AxisEnum axis, sensorless_t enable_stealth) {
+    switch (axis) {
+      default: break;
+      #if X_SENSORLESS
+        case X_AXIS:
+          tmc_disable_stallguard(stepperX, enable_stealth.x);
+          #if AXIS_HAS_STALLGUARD(X2)
+            tmc_disable_stallguard(stepperX2, enable_stealth.x2);
+          #endif
+          #if EITHER(CORE_IS_XY, MARKFORGED_XY) && Y_SENSORLESS
+            tmc_disable_stallguard(stepperY, enable_stealth.y);
+          #elif CORE_IS_XZ && Z_SENSORLESS
+            tmc_disable_stallguard(stepperZ, enable_stealth.z);
+          #endif
+          break;
+      #endif
+      #if Y_SENSORLESS
+        case Y_AXIS:
+          tmc_disable_stallguard(stepperY, enable_stealth.y);
+          #if AXIS_HAS_STALLGUARD(Y2)
+            tmc_disable_stallguard(stepperY2, enable_stealth.y2);
+          #endif
+          #if EITHER(CORE_IS_XY, MARKFORGED_XY) && X_SENSORLESS
+            tmc_disable_stallguard(stepperX, enable_stealth.x);
+          #elif CORE_IS_YZ && Z_SENSORLESS
+            tmc_disable_stallguard(stepperZ, enable_stealth.z);
+          #endif
+          break;
+      #endif
+      #if Z_SENSORLESS
+        case Z_AXIS:
+          tmc_disable_stallguard(stepperZ, enable_stealth.z);
+          #if AXIS_HAS_STALLGUARD(Z2)
+            tmc_disable_stallguard(stepperZ2, enable_stealth.z2);
+          #endif
+          #if AXIS_HAS_STALLGUARD(Z3)
+            tmc_disable_stallguard(stepperZ3, enable_stealth.z3);
+          #endif
+          #if AXIS_HAS_STALLGUARD(Z4)
+            tmc_disable_stallguard(stepperZ4, enable_stealth.z4);
+          #endif
+          #if CORE_IS_XZ && X_SENSORLESS
+            tmc_disable_stallguard(stepperX, enable_stealth.x);
+          #elif CORE_IS_YZ && Y_SENSORLESS
+            tmc_disable_stallguard(stepperY, enable_stealth.y);
+          #endif
+          break;
+      #endif
+    }
+
+    #if ENABLED(SPI_ENDSTOPS)
+      switch (axis) {
+        case X_AXIS: if (ENABLED(X_SPI_SENSORLESS)) endstops.tmc_spi_homing.x = false; break;
+        case Y_AXIS: if (ENABLED(Y_SPI_SENSORLESS)) endstops.tmc_spi_homing.y = false; break;
+        case Z_AXIS: if (ENABLED(Z_SPI_SENSORLESS)) endstops.tmc_spi_homing.z = false; break;
+        default: break;
+      }
+    #endif
+  }
+
+#endif // SENSORLESS_HOMING
+
+/**
+ * Home an individual linear axis
+ */
+void do_homing_move(const AxisEnum axis, const float distance, const feedRate_t fr_mm_s=0.0, const bool final_approach=true) {
+  DEBUG_SECTION(log_move, "do_homing_move", DEBUGGING(LEVELING));
+
+  const feedRate_t home_fr_mm_s = fr_mm_s ?: homing_feedrate(axis);
+
+  if (DEBUGGING(LEVELING)) {
+    DEBUG_ECHOPAIR("...(", axis_codes[axis], ", ", distance, ", ");
+    if (fr_mm_s)
+      DEBUG_ECHO(fr_mm_s);
+    else
+      DEBUG_ECHOPAIR("[", home_fr_mm_s, "]");
+    DEBUG_ECHOLNPGM(")");
+  }
+
+  // Only do some things when moving towards an endstop
+  const int8_t axis_home_dir = TERN0(DUAL_X_CARRIAGE, axis == X_AXIS)
+                ? x_home_dir(active_extruder) : home_dir(axis);
+  const bool is_home_dir = (axis_home_dir > 0) == (distance > 0);
+
+  #if ENABLED(SENSORLESS_HOMING)
+    sensorless_t stealth_states;
+  #endif
+
+  if (is_home_dir) {
+
+    if (TERN0(HOMING_Z_WITH_PROBE, axis == Z_AXIS)) {
+      #if ALL(HAS_HEATED_BED, WAIT_FOR_BED_HEATER)
+        // Wait for bed to heat back up between probing points
+        thermalManager.wait_for_bed_heating();
+      #endif
+
+      TERN_(HAS_QUIET_PROBING, if (final_approach) probe.set_probing_paused(true));
+    }
+
+    // Disable stealthChop if used. Enable diag1 pin on driver.
+    TERN_(SENSORLESS_HOMING, stealth_states = start_sensorless_homing_per_axis(axis));
+  }
+
+  #if IS_SCARA
+    // Tell the planner the axis is at 0
+    current_position[axis] = 0;
+    sync_plan_position();
+    current_position[axis] = distance;
+    line_to_current_position(home_fr_mm_s);
+  #else
+    // Get the ABC or XYZ positions in mm
+    abce_pos_t target = planner.get_axis_positions_mm();
+
+    target[axis] = 0;                         // Set the single homing axis to 0
+    planner.set_machine_position_mm(target);  // Update the machine position
+
+    #if HAS_DIST_MM_ARG
+      const xyze_float_t cart_dist_mm{0};
+    #endif
+
+    // Set delta/cartesian axes directly
+    target[axis] = distance;                  // The move will be towards the endstop
+    planner.buffer_segment(target
+      #if HAS_DIST_MM_ARG
+        , cart_dist_mm
+      #endif
+      , home_fr_mm_s, active_extruder
+    );
+  #endif
+
+  planner.synchronize();
+
+  if (is_home_dir) {
+
+    #if HOMING_Z_WITH_PROBE && HAS_QUIET_PROBING
+      if (axis == Z_AXIS && final_approach) probe.set_probing_paused(false);
+    #endif
+
+    endstops.validate_homing_move();
+
+    // Re-enable stealthChop if used. Disable diag1 pin on driver.
+    TERN_(SENSORLESS_HOMING, end_sensorless_homing_per_axis(axis, stealth_states));
+  }
+}
+
+/**
+ * Set an axis' current position to its home position (after homing).
+ *
+ * For Core and Cartesian robots this applies one-to-one when an
+ * individual axis has been homed.
+ *
+ * DELTA should wait until all homing is done before setting the XYZ
+ * current_position to home, because homing is a single operation.
+ * In the case where the axis positions are trusted and previously
+ * homed, DELTA could home to X or Y individually by moving either one
+ * to the center. However, homing Z always homes XY and Z.
+ *
+ * SCARA should wait until all XY homing is done before setting the XY
+ * current_position to home, because neither X nor Y is at home until
+ * both are at home. Z can however be homed individually.
+ *
+ * Callers must sync the planner position after calling this!
+ */
+void set_axis_is_at_home(const AxisEnum axis) {
+  if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR(">>> set_axis_is_at_home(", axis_codes[axis], ")");
+
+  set_axis_trusted(axis);
+  set_axis_homed(axis);
+
+  #if ENABLED(DUAL_X_CARRIAGE)
+    if (axis == X_AXIS && (active_extruder == 1 || dual_x_carriage_mode == DXC_DUPLICATION_MODE)) {
+      current_position.x = x_home_pos(active_extruder);
+      return;
+    }
+  #endif
+
+  #if ENABLED(MORGAN_SCARA)
+    scara_set_axis_is_at_home(axis);
+  #elif ENABLED(DELTA)
+    current_position[axis] = (axis == Z_AXIS) ? delta_height - TERN0(HAS_BED_PROBE, probe.offset.z) : base_home_pos(axis);
+  #else
+    current_position[axis] = base_home_pos(axis);
+  #endif
+
+  /**
+   * Z Probe Z Homing? Account for the probe's Z offset.
+   */
+  #if HAS_BED_PROBE && Z_HOME_DIR < 0
+    if (axis == Z_AXIS) {
+      #if HOMING_Z_WITH_PROBE
+
+        current_position.z -= probe.offset.z;
+
+        if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("*** Z HOMED WITH PROBE (Z_MIN_PROBE_USES_Z_MIN_ENDSTOP_PIN) ***\n> probe.offset.z = ", probe.offset.z);
+
+      #else
+
+        if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("*** Z HOMED TO ENDSTOP ***");
+
+      #endif
+    }
+  #endif
+
+  TERN_(I2C_POSITION_ENCODERS, I2CPEM.homed(axis));
+
+  TERN_(BABYSTEP_DISPLAY_TOTAL, babystep.reset_total(axis));
+
+  #if HAS_POSITION_SHIFT
+    position_shift[axis] = 0;
+    update_workspace_offset(axis);
+  #endif
+
+  if (DEBUGGING(LEVELING)) {
+    #if HAS_HOME_OFFSET
+      DEBUG_ECHOLNPAIR("> home_offset[", axis_codes[axis], "] = ", home_offset[axis]);
+    #endif
+    DEBUG_POS("", current_position);
+    DEBUG_ECHOLNPAIR("<<< set_axis_is_at_home(", axis_codes[axis], ")");
+  }
+}
+
+/**
+ * Set an axis to be unhomed.
+ */
+void set_axis_never_homed(const AxisEnum axis) {
+  if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR(">>> set_axis_never_homed(", axis_codes[axis], ")");
+
+  set_axis_untrusted(axis);
+  set_axis_unhomed(axis);
+
+  if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("<<< set_axis_never_homed(", axis_codes[axis], ")");
+
+  TERN_(I2C_POSITION_ENCODERS, I2CPEM.unhomed(axis));
+}
+
+#ifdef TMC_HOME_PHASE
+  /**
+   * Move the axis back to its home_phase if set and driver is capable (TMC)
+   *
+   * Improves homing repeatability by homing to stepper coil's nearest absolute
+   * phase position. Trinamic drivers use a stepper phase table with 1024 values
+   * spanning 4 full steps with 256 positions each (ergo, 1024 positions).
+   */
+  void backout_to_tmc_homing_phase(const AxisEnum axis) {
+    const xyz_long_t home_phase = TMC_HOME_PHASE;
+
+    // check if home phase is disabled for this axis.
+    if (home_phase[axis] < 0) return;
+
+    int16_t phasePerUStep,      // TMC µsteps(phase) per Marlin µsteps
+            phaseCurrent,       // The TMC µsteps(phase) count of the current position
+            effectorBackoutDir, // Direction in which the effector mm coordinates move away from endstop.
+            stepperBackoutDir;  // Direction in which the TMC µstep count(phase) move away from endstop.
+
+    #define PHASE_PER_MICROSTEP(N) (256 / _MAX(1, N##_MICROSTEPS))
+
+    switch (axis) {
+      #ifdef X_MICROSTEPS
+        case X_AXIS:
+          phasePerUStep = PHASE_PER_MICROSTEP(X);
+          phaseCurrent = stepperX.get_microstep_counter();
+          effectorBackoutDir = -X_HOME_DIR;
+          stepperBackoutDir = INVERT_X_DIR ? effectorBackoutDir : -effectorBackoutDir;
+          break;
+      #endif
+      #ifdef Y_MICROSTEPS
+        case Y_AXIS:
+          phasePerUStep = PHASE_PER_MICROSTEP(Y);
+          phaseCurrent = stepperY.get_microstep_counter();
+          effectorBackoutDir = -Y_HOME_DIR;
+          stepperBackoutDir = INVERT_Y_DIR ? effectorBackoutDir : -effectorBackoutDir;
+          break;
+      #endif
+      #ifdef Z_MICROSTEPS
+        case Z_AXIS:
+          phasePerUStep = PHASE_PER_MICROSTEP(Z);
+          phaseCurrent = stepperZ.get_microstep_counter();
+          effectorBackoutDir = -Z_HOME_DIR;
+          stepperBackoutDir = INVERT_Z_DIR ? effectorBackoutDir : -effectorBackoutDir;
+          break;
+      #endif
+      default: return;
+    }
+
+    // Phase distance to nearest home phase position when moving in the backout direction from endstop(may be negative).
+    int16_t phaseDelta = (home_phase[axis] - phaseCurrent) * stepperBackoutDir;
+
+    // Check if home distance within endstop assumed repeatability noise of .05mm and warn.
+    if (ABS(phaseDelta) * planner.steps_to_mm[axis] / phasePerUStep < 0.05f)
+      SERIAL_ECHOLNPAIR("Selected home phase ", home_phase[axis],
+                       " too close to endstop trigger phase ", phaseCurrent,
+                       ". Pick a different phase for ", axis_codes[axis]);
+
+    // Skip to next if target position is behind current. So it only moves away from endstop.
+    if (phaseDelta < 0) phaseDelta += 1024;
+
+    // Convert TMC µsteps(phase) to whole Marlin µsteps to effector backout direction to mm
+    const float mmDelta = int16_t(phaseDelta / phasePerUStep) * effectorBackoutDir * planner.steps_to_mm[axis];
+
+    // Optional debug messages
+    if (DEBUGGING(LEVELING)) {
+      DEBUG_ECHOLNPAIR(
+        "Endstop ", axis_codes[axis], " hit at Phase:", phaseCurrent,
+        " Delta:", phaseDelta, " Distance:", mmDelta
+      );
+    }
+
+    if (mmDelta != 0) {
+      // Retrace by the amount computed in mmDelta.
+      do_homing_move(axis, mmDelta, get_homing_bump_feedrate(axis));
+    }
+  }
+#endif
+
+/**
+ * Home an individual "raw axis" to its endstop.
+ * This applies to XYZ on Cartesian and Core robots, and
+ * to the individual ABC steppers on DELTA and SCARA.
+ *
+ * At the end of the procedure the axis is marked as
+ * homed and the current position of that axis is updated.
+ * Kinematic robots should wait till all axes are homed
+ * before updating the current position.
+ */
+
+void homeaxis(const AxisEnum axis) {
+
+  #if IS_SCARA
+    // Only Z homing (with probe) is permitted
+    if (axis != Z_AXIS) { BUZZ(100, 880); return; }
+  #else
+    #define _CAN_HOME(A) (axis == _AXIS(A) && ( \
+         ENABLED(A##_SPI_SENSORLESS) \
+      || (_AXIS(A) == Z_AXIS && ENABLED(HOMING_Z_WITH_PROBE)) \
+      || (A##_MIN_PIN > -1 && A##_HOME_DIR < 0) \
+      || (A##_MAX_PIN > -1 && A##_HOME_DIR > 0) \
+    ))
+    if (!_CAN_HOME(X) && !_CAN_HOME(Y) && !_CAN_HOME(Z)) return;
+  #endif
+
+  if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR(">>> homeaxis(", axis_codes[axis], ")");
+
+  const int axis_home_dir = TERN0(DUAL_X_CARRIAGE, axis == X_AXIS)
+              ? x_home_dir(active_extruder) : home_dir(axis);
+
+  //
+  // Homing Z with a probe? Raise Z (maybe) and deploy the Z probe.
+  //
+  if (TERN0(HOMING_Z_WITH_PROBE, axis == Z_AXIS && probe.deploy()))
+    return;
+
+  // Set flags for X, Y, Z motor locking
+  #if HAS_EXTRA_ENDSTOPS
+    switch (axis) {
+      TERN_(X_DUAL_ENDSTOPS, case X_AXIS:)
+      TERN_(Y_DUAL_ENDSTOPS, case Y_AXIS:)
+      TERN_(Z_MULTI_ENDSTOPS, case Z_AXIS:)
+        stepper.set_separate_multi_axis(true);
+      default: break;
+    }
+  #endif
+
+  //
+  // Deploy BLTouch or tare the probe just before probing
+  //
+  #if HOMING_Z_WITH_PROBE
+    if (axis == Z_AXIS) {
+      if (TERN0(BLTOUCH, bltouch.deploy())) return;   // BLTouch was deployed above, but get the alarm state.
+      if (TERN0(PROBE_TARE, probe.tare())) return;
+    }
+  #endif
+
+  //
+  // Back away to prevent an early X/Y sensorless trigger
+  //
+  #if DISABLED(DELTA) && defined(SENSORLESS_BACKOFF_MM)
+    const xy_float_t backoff = SENSORLESS_BACKOFF_MM;
+    if ((TERN0(X_SENSORLESS, axis == X_AXIS) || TERN0(Y_SENSORLESS, axis == Y_AXIS)) && backoff[axis]) {
+      const float backoff_length = -ABS(backoff[axis]) * axis_home_dir;
+      if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("Sensorless backoff: ", backoff_length, "mm");
+      do_homing_move(axis, backoff_length, homing_feedrate(axis));
+    }
+  #endif
+
+  // Determine if a homing bump will be done and the bumps distance
+  // When homing Z with probe respect probe clearance
+  const bool use_probe_bump = TERN0(HOMING_Z_WITH_PROBE, axis == Z_AXIS && home_bump_mm(Z_AXIS));
+  const float bump = axis_home_dir * (
+    use_probe_bump ? _MAX(TERN0(HOMING_Z_WITH_PROBE, Z_CLEARANCE_BETWEEN_PROBES), home_bump_mm(Z_AXIS)) : home_bump_mm(axis)
+  );
+
+  //
+  // Fast move towards endstop until triggered
+  //
+  const float move_length = 1.5f * max_length(TERN(DELTA, Z_AXIS, axis)) * axis_home_dir;
+  if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("Home Fast: ", move_length, "mm");
+  do_homing_move(axis, move_length, 0.0, !use_probe_bump);
+
+  #if BOTH(HOMING_Z_WITH_PROBE, BLTOUCH_SLOW_MODE)
+    if (axis == Z_AXIS) bltouch.stow(); // Intermediate STOW (in LOW SPEED MODE)
+  #endif
+
+  // If a second homing move is configured...
+  if (bump) {
+    // Move away from the endstop by the axis HOMING_BUMP_MM
+    if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("Move Away: ", -bump, "mm");
+    do_homing_move(axis, -bump, TERN0(HOMING_Z_WITH_PROBE, axis == Z_AXIS) ? MMM_TO_MMS(Z_PROBE_SPEED_FAST) : 0, false);
+
+    #if ENABLED(DETECT_BROKEN_ENDSTOP)
+      // Check for a broken endstop
+      EndstopEnum es;
+      switch (axis) {
+        default:
+        case X_AXIS: es = X_ENDSTOP; break;
+        case Y_AXIS: es = Y_ENDSTOP; break;
+        case Z_AXIS: es = Z_ENDSTOP; break;
+      }
+      if (TEST(endstops.state(), es)) {
+        SERIAL_ECHO_MSG("Bad ", axis_codes[axis], " Endstop?");
+        kill(GET_TEXT(MSG_KILL_HOMING_FAILED));
+      }
+    #endif
+
+    #if BOTH(HOMING_Z_WITH_PROBE, BLTOUCH_SLOW_MODE)
+      if (axis == Z_AXIS && bltouch.deploy()) return; // Intermediate DEPLOY (in LOW SPEED MODE)
+    #endif
+
+    // Slow move towards endstop until triggered
+    const float rebump = bump * 2;
+    if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("Re-bump: ", rebump, "mm");
+    do_homing_move(axis, rebump, get_homing_bump_feedrate(axis), true);
+
+    #if BOTH(HOMING_Z_WITH_PROBE, BLTOUCH)
+      if (axis == Z_AXIS) bltouch.stow(); // The final STOW
+    #endif
+  }
+
+  #if HAS_EXTRA_ENDSTOPS
+    const bool pos_dir = axis_home_dir > 0;
+    #if ENABLED(X_DUAL_ENDSTOPS)
+      if (axis == X_AXIS) {
+        const float adj = ABS(endstops.x2_endstop_adj);
+        if (adj) {
+          if (pos_dir ? (endstops.x2_endstop_adj > 0) : (endstops.x2_endstop_adj < 0)) stepper.set_x_lock(true); else stepper.set_x2_lock(true);
+          do_homing_move(axis, pos_dir ? -adj : adj);
+          stepper.set_x_lock(false);
+          stepper.set_x2_lock(false);
+        }
+      }
+    #endif
+    #if ENABLED(Y_DUAL_ENDSTOPS)
+      if (axis == Y_AXIS) {
+        const float adj = ABS(endstops.y2_endstop_adj);
+        if (adj) {
+          if (pos_dir ? (endstops.y2_endstop_adj > 0) : (endstops.y2_endstop_adj < 0)) stepper.set_y_lock(true); else stepper.set_y2_lock(true);
+          do_homing_move(axis, pos_dir ? -adj : adj);
+          stepper.set_y_lock(false);
+          stepper.set_y2_lock(false);
+        }
+      }
+    #endif
+
+    #if ENABLED(Z_MULTI_ENDSTOPS)
+      if (axis == Z_AXIS) {
+
+        #if NUM_Z_STEPPER_DRIVERS == 2
+
+          const float adj = ABS(endstops.z2_endstop_adj);
+          if (adj) {
+            if (pos_dir ? (endstops.z2_endstop_adj > 0) : (endstops.z2_endstop_adj < 0)) stepper.set_z1_lock(true); else stepper.set_z2_lock(true);
+            do_homing_move(axis, pos_dir ? -adj : adj);
+            stepper.set_z1_lock(false);
+            stepper.set_z2_lock(false);
+          }
+
+        #else
+
+          // Handy arrays of stepper lock function pointers
+
+          typedef void (*adjustFunc_t)(const bool);
+
+          adjustFunc_t lock[] = {
+            stepper.set_z1_lock, stepper.set_z2_lock, stepper.set_z3_lock
+            #if NUM_Z_STEPPER_DRIVERS >= 4
+              , stepper.set_z4_lock
+            #endif
+          };
+          float adj[] = {
+            0, endstops.z2_endstop_adj, endstops.z3_endstop_adj
+            #if NUM_Z_STEPPER_DRIVERS >= 4
+              , endstops.z4_endstop_adj
+            #endif
+          };
+
+          adjustFunc_t tempLock;
+          float tempAdj;
+
+          // Manual bubble sort by adjust value
+          if (adj[1] < adj[0]) {
+            tempLock = lock[0], tempAdj = adj[0];
+            lock[0] = lock[1], adj[0] = adj[1];
+            lock[1] = tempLock, adj[1] = tempAdj;
+          }
+          if (adj[2] < adj[1]) {
+            tempLock = lock[1], tempAdj = adj[1];
+            lock[1] = lock[2], adj[1] = adj[2];
+            lock[2] = tempLock, adj[2] = tempAdj;
+          }
+          #if NUM_Z_STEPPER_DRIVERS >= 4
+            if (adj[3] < adj[2]) {
+              tempLock = lock[2], tempAdj = adj[2];
+              lock[2] = lock[3], adj[2] = adj[3];
+              lock[3] = tempLock, adj[3] = tempAdj;
+            }
+            if (adj[2] < adj[1]) {
+              tempLock = lock[1], tempAdj = adj[1];
+              lock[1] = lock[2], adj[1] = adj[2];
+              lock[2] = tempLock, adj[2] = tempAdj;
+            }
+          #endif
+          if (adj[1] < adj[0]) {
+            tempLock = lock[0], tempAdj = adj[0];
+            lock[0] = lock[1], adj[0] = adj[1];
+            lock[1] = tempLock, adj[1] = tempAdj;
+          }
+
+          if (pos_dir) {
+            // normalize adj to smallest value and do the first move
+            (*lock[0])(true);
+            do_homing_move(axis, adj[1] - adj[0]);
+            // lock the second stepper for the final correction
+            (*lock[1])(true);
+            do_homing_move(axis, adj[2] - adj[1]);
+            #if NUM_Z_STEPPER_DRIVERS >= 4
+              // lock the third stepper for the final correction
+              (*lock[2])(true);
+              do_homing_move(axis, adj[3] - adj[2]);
+            #endif
+          }
+          else {
+            #if NUM_Z_STEPPER_DRIVERS >= 4
+              (*lock[3])(true);
+              do_homing_move(axis, adj[2] - adj[3]);
+            #endif
+            (*lock[2])(true);
+            do_homing_move(axis, adj[1] - adj[2]);
+            (*lock[1])(true);
+            do_homing_move(axis, adj[0] - adj[1]);
+          }
+
+          stepper.set_z1_lock(false);
+          stepper.set_z2_lock(false);
+          stepper.set_z3_lock(false);
+          #if NUM_Z_STEPPER_DRIVERS >= 4
+            stepper.set_z4_lock(false);
+          #endif
+
+        #endif
+      }
+    #endif
+
+    // Reset flags for X, Y, Z motor locking
+    switch (axis) {
+      default: break;
+      TERN_(X_DUAL_ENDSTOPS, case X_AXIS:)
+      TERN_(Y_DUAL_ENDSTOPS, case Y_AXIS:)
+      TERN_(Z_MULTI_ENDSTOPS, case Z_AXIS:)
+        stepper.set_separate_multi_axis(false);
+    }
+  #endif
+
+  #ifdef TMC_HOME_PHASE
+    // move back to homing phase if configured and capable
+    backout_to_tmc_homing_phase(axis);
+  #endif
+
+  #if IS_SCARA
+
+    set_axis_is_at_home(axis);
+    sync_plan_position();
+
+  #elif ENABLED(DELTA)
+
+    // Delta has already moved all three towers up in G28
+    // so here it re-homes each tower in turn.
+    // Delta homing treats the axes as normal linear axes.
+
+    const float adjDistance = delta_endstop_adj[axis],
+                minDistance = (MIN_STEPS_PER_SEGMENT) * planner.steps_to_mm[axis];
+
+    // Retrace by the amount specified in delta_endstop_adj if more than min steps.
+    if (adjDistance * (Z_HOME_DIR) < 0 && ABS(adjDistance) > minDistance) { // away from endstop, more than min distance
+      if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("adjDistance:", adjDistance);
+      do_homing_move(axis, adjDistance, get_homing_bump_feedrate(axis));
+    }
+
+  #else // CARTESIAN / CORE / MARKFORGED_XY
+
+    set_axis_is_at_home(axis);
+    sync_plan_position();
+
+    destination[axis] = current_position[axis];
+
+    if (DEBUGGING(LEVELING)) DEBUG_POS("> AFTER set_axis_is_at_home", current_position);
+
+  #endif
+
+  // Put away the Z probe
+  #if HOMING_Z_WITH_PROBE
+    if (axis == Z_AXIS && probe.stow()) return;
+  #endif
+
+  #if DISABLED(DELTA) && defined(HOMING_BACKOFF_POST_MM)
+    const xyz_float_t endstop_backoff = HOMING_BACKOFF_POST_MM;
+    if (endstop_backoff[axis]) {
+      current_position[axis] -= ABS(endstop_backoff[axis]) * axis_home_dir;
+      line_to_current_position(
+        #if HOMING_Z_WITH_PROBE
+          (axis == Z_AXIS) ? z_probe_fast_mm_s :
+        #endif
+        homing_feedrate(axis)
+      );
+
+      #if ENABLED(SENSORLESS_HOMING)
+        planner.synchronize();
+        if (false
+          #if EITHER(IS_CORE, MARKFORGED_XY)
+            || axis != NORMAL_AXIS
+          #endif
+        ) safe_delay(200);  // Short delay to allow belts to spring back
+      #endif
+    }
+  #endif
+
+  // Clear retracted status if homing the Z axis
+  #if ENABLED(FWRETRACT)
+    if (axis == Z_AXIS) fwretract.current_hop = 0.0;
+  #endif
+
+  if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("<<< homeaxis(", axis_codes[axis], ")");
+
+} // homeaxis()
+
+#if HAS_WORKSPACE_OFFSET
+  void update_workspace_offset(const AxisEnum axis) {
+    workspace_offset[axis] = home_offset[axis] + position_shift[axis];
+    if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("Axis ", XYZ_CHAR(axis), " home_offset = ", home_offset[axis], " position_shift = ", position_shift[axis]);
+  }
+#endif
+
+#if HAS_M206_COMMAND
+  /**
+   * Change the home offset for an axis.
+   * Also refreshes the workspace offset.
+   */
+  void set_home_offset(const AxisEnum axis, const float v) {
+    home_offset[axis] = v;
+    update_workspace_offset(axis);
+  }
+#endif // HAS_M206_COMMAND
diff --git a/Marlin/src/module/motion.h b/Marlin/src/module/motion.h
new file mode 100644
index 0000000..887da1a
--- /dev/null
+++ b/Marlin/src/module/motion.h
@@ -0,0 +1,465 @@
+/**
+ * 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
+
+/**
+ * motion.h
+ *
+ * High-level motion commands to feed the planner
+ * Some of these methods may migrate to the planner class.
+ */
+
+#include "../inc/MarlinConfig.h"
+
+#if IS_SCARA
+  #include "scara.h"
+#endif
+
+// Error margin to work around float imprecision
+constexpr float fslop = 0.0001;
+
+extern bool relative_mode;
+
+extern xyze_pos_t current_position,  // High-level current tool position
+                  destination;       // Destination for a move
+
+// G60/G61 Position Save and Return
+#if SAVED_POSITIONS
+  extern uint8_t saved_slots[(SAVED_POSITIONS + 7) >> 3];
+  extern xyz_pos_t stored_position[SAVED_POSITIONS];
+#endif
+
+// Scratch space for a cartesian result
+extern xyz_pos_t cartes;
+
+// Until kinematics.cpp is created, declare this here
+#if IS_KINEMATIC
+  extern abc_pos_t delta;
+#endif
+
+#if HAS_ABL_NOT_UBL
+  extern feedRate_t xy_probe_feedrate_mm_s;
+  #define XY_PROBE_FEEDRATE_MM_S xy_probe_feedrate_mm_s
+#elif defined(XY_PROBE_SPEED)
+  #define XY_PROBE_FEEDRATE_MM_S MMM_TO_MMS(XY_PROBE_SPEED)
+#else
+  #define XY_PROBE_FEEDRATE_MM_S PLANNER_XY_FEEDRATE()
+#endif
+
+constexpr feedRate_t z_probe_fast_mm_s = MMM_TO_MMS(Z_PROBE_SPEED_FAST);
+
+/**
+ * Feed rates are often configured with mm/m
+ * but the planner and stepper like mm/s units.
+ */
+constexpr xyz_feedrate_t homing_feedrate_mm_m = HOMING_FEEDRATE_MM_M;
+FORCE_INLINE feedRate_t homing_feedrate(const AxisEnum a) {
+  float v;
+  #if ENABLED(DELTA)
+    v = homing_feedrate_mm_m.z;
+  #else
+    switch (a) {
+      case X_AXIS: v = homing_feedrate_mm_m.x; break;
+      case Y_AXIS: v = homing_feedrate_mm_m.y; break;
+      case Z_AXIS:
+          default: v = homing_feedrate_mm_m.z;
+    }
+  #endif
+  return MMM_TO_MMS(v);
+}
+
+feedRate_t get_homing_bump_feedrate(const AxisEnum axis);
+
+/**
+ * The default feedrate for many moves, set by the most recent move
+ */
+extern feedRate_t feedrate_mm_s;
+
+/**
+ * Feedrate scaling is applied to all G0/G1, G2/G3, and G5 moves
+ */
+extern int16_t feedrate_percentage;
+#define MMS_SCALED(V) ((V) * 0.01f * feedrate_percentage)
+
+// The active extruder (tool). Set with T<extruder> command.
+#if HAS_MULTI_EXTRUDER
+  extern uint8_t active_extruder;
+#else
+  constexpr uint8_t active_extruder = 0;
+#endif
+
+#if ENABLED(LCD_SHOW_E_TOTAL)
+  extern float e_move_accumulator;
+#endif
+
+#ifdef __IMXRT1062__
+  #define DEFS_PROGMEM
+#else
+  #define DEFS_PROGMEM PROGMEM
+#endif
+
+inline float pgm_read_any(const float *p)   { return TERN(__IMXRT1062__, *p, pgm_read_float(p)); }
+inline int8_t pgm_read_any(const int8_t *p) { return TERN(__IMXRT1062__, *p, pgm_read_byte(p)); }
+
+#define XYZ_DEFS(T, NAME, OPT) \
+  inline T NAME(const AxisEnum axis) { \
+    static const XYZval<T> NAME##_P DEFS_PROGMEM = { X_##OPT, Y_##OPT, Z_##OPT }; \
+    return pgm_read_any(&NAME##_P[axis]); \
+  }
+XYZ_DEFS(float, base_min_pos,   MIN_POS);
+XYZ_DEFS(float, base_max_pos,   MAX_POS);
+XYZ_DEFS(float, base_home_pos,  HOME_POS);
+XYZ_DEFS(float, max_length,     MAX_LENGTH);
+XYZ_DEFS(int8_t, home_dir, HOME_DIR);
+
+inline float home_bump_mm(const AxisEnum axis) {
+  static const xyz_pos_t home_bump_mm_P DEFS_PROGMEM = HOMING_BUMP_MM;
+  return pgm_read_any(&home_bump_mm_P[axis]);
+}
+
+#if HAS_WORKSPACE_OFFSET
+  void update_workspace_offset(const AxisEnum axis);
+#else
+  inline void update_workspace_offset(const AxisEnum) {}
+#endif
+
+#if HAS_HOTEND_OFFSET
+  extern xyz_pos_t hotend_offset[HOTENDS];
+  void reset_hotend_offsets();
+#elif HOTENDS
+  constexpr xyz_pos_t hotend_offset[HOTENDS] = { { 0 } };
+#else
+  constexpr xyz_pos_t hotend_offset[1] = { { 0 } };
+#endif
+
+#if HAS_SOFTWARE_ENDSTOPS
+
+  typedef struct {
+    bool _enabled, _loose;
+    bool enabled() { return _enabled && !_loose; }
+
+    xyz_pos_t min, max;
+    void get_manual_axis_limits(const AxisEnum axis, float &amin, float &amax) {
+      amin = -100000; amax = 100000; // "No limits"
+      #if HAS_SOFTWARE_ENDSTOPS
+        if (enabled()) switch (axis) {
+          case X_AXIS:
+            TERN_(MIN_SOFTWARE_ENDSTOP_X, amin = min.x);
+            TERN_(MAX_SOFTWARE_ENDSTOP_X, amax = max.x);
+            break;
+          case Y_AXIS:
+            TERN_(MIN_SOFTWARE_ENDSTOP_Y, amin = min.y);
+            TERN_(MAX_SOFTWARE_ENDSTOP_Y, amax = max.y);
+            break;
+          case Z_AXIS:
+            TERN_(MIN_SOFTWARE_ENDSTOP_Z, amin = min.z);
+            TERN_(MAX_SOFTWARE_ENDSTOP_Z, amax = max.z);
+          default: break;
+        }
+      #endif
+    }
+  } soft_endstops_t;
+
+  extern soft_endstops_t soft_endstop;
+  void apply_motion_limits(xyz_pos_t &target);
+  void update_software_endstops(const AxisEnum axis
+    #if HAS_HOTEND_OFFSET
+      , const uint8_t old_tool_index=0, const uint8_t new_tool_index=0
+    #endif
+  );
+  #define SET_SOFT_ENDSTOP_LOOSE(loose) (soft_endstop._loose = loose)
+
+#else // !HAS_SOFTWARE_ENDSTOPS
+
+  typedef struct {
+    bool enabled() { return false; }
+    void get_manual_axis_limits(const AxisEnum axis, float &amin, float &amax) {
+      // No limits
+      amin = current_position[axis] - 1000;
+      amax = current_position[axis] + 1000;
+    }
+  } soft_endstops_t;
+  extern soft_endstops_t soft_endstop;
+  #define apply_motion_limits(V)        NOOP
+  #define update_software_endstops(...) NOOP
+  #define SET_SOFT_ENDSTOP_LOOSE(V)     NOOP
+
+#endif // !HAS_SOFTWARE_ENDSTOPS
+
+void report_real_position();
+void report_current_position();
+void report_current_position_projected();
+
+void get_cartesian_from_steppers();
+void set_current_from_steppers_for_axis(const AxisEnum axis);
+
+void quickstop_stepper();
+
+/**
+ * sync_plan_position
+ *
+ * Set the planner/stepper positions directly from current_position with
+ * no kinematic translation. Used for homing axes and cartesian/core syncing.
+ */
+void sync_plan_position();
+void sync_plan_position_e();
+
+/**
+ * Move the planner to the current position from wherever it last moved
+ * (or from wherever it has been told it is located).
+ */
+void line_to_current_position(const feedRate_t &fr_mm_s=feedrate_mm_s);
+
+#if EXTRUDERS
+  void unscaled_e_move(const float &length, const feedRate_t &fr_mm_s);
+#endif
+
+void prepare_line_to_destination();
+
+void _internal_move_to_destination(const feedRate_t &fr_mm_s=0.0f
+  #if IS_KINEMATIC
+    , const bool is_fast=false
+  #endif
+);
+
+inline void prepare_internal_move_to_destination(const feedRate_t &fr_mm_s=0.0f) {
+  _internal_move_to_destination(fr_mm_s);
+}
+
+#if IS_KINEMATIC
+  void prepare_fast_move_to_destination(const feedRate_t &scaled_fr_mm_s=MMS_SCALED(feedrate_mm_s));
+
+  inline void prepare_internal_fast_move_to_destination(const feedRate_t &fr_mm_s=0.0f) {
+    _internal_move_to_destination(fr_mm_s, true);
+  }
+#endif
+
+/**
+ * Blocking movement and shorthand functions
+ */
+void do_blocking_move_to(const float rx, const float ry, const float rz, const feedRate_t &fr_mm_s=0.0f);
+void do_blocking_move_to(const xy_pos_t &raw, const feedRate_t &fr_mm_s=0.0f);
+void do_blocking_move_to(const xyz_pos_t &raw, const feedRate_t &fr_mm_s=0.0f);
+void do_blocking_move_to(const xyze_pos_t &raw, const feedRate_t &fr_mm_s=0.0f);
+
+void do_blocking_move_to_x(const float &rx, const feedRate_t &fr_mm_s=0.0f);
+void do_blocking_move_to_y(const float &ry, const feedRate_t &fr_mm_s=0.0f);
+void do_blocking_move_to_z(const float &rz, const feedRate_t &fr_mm_s=0.0f);
+
+void do_blocking_move_to_xy(const float &rx, const float &ry, const feedRate_t &fr_mm_s=0.0f);
+void do_blocking_move_to_xy(const xy_pos_t &raw, const feedRate_t &fr_mm_s=0.0f);
+FORCE_INLINE void do_blocking_move_to_xy(const xyz_pos_t &raw, const feedRate_t &fr_mm_s=0.0f)  { do_blocking_move_to_xy(xy_pos_t(raw), fr_mm_s); }
+FORCE_INLINE void do_blocking_move_to_xy(const xyze_pos_t &raw, const feedRate_t &fr_mm_s=0.0f) { do_blocking_move_to_xy(xy_pos_t(raw), fr_mm_s); }
+
+void do_blocking_move_to_xy_z(const xy_pos_t &raw, const float &z, const feedRate_t &fr_mm_s=0.0f);
+FORCE_INLINE void do_blocking_move_to_xy_z(const xyz_pos_t &raw, const float &z, const feedRate_t &fr_mm_s=0.0f)  { do_blocking_move_to_xy_z(xy_pos_t(raw), z, fr_mm_s); }
+FORCE_INLINE void do_blocking_move_to_xy_z(const xyze_pos_t &raw, const float &z, const feedRate_t &fr_mm_s=0.0f) { do_blocking_move_to_xy_z(xy_pos_t(raw), z, fr_mm_s); }
+
+void remember_feedrate_and_scaling();
+void remember_feedrate_scaling_off();
+void restore_feedrate_and_scaling();
+
+void do_z_clearance(const float &zclear, const bool z_trusted=true, const bool raise_on_untrusted=true, const bool lower_allowed=false);
+
+/**
+ * Homing and Trusted Axes
+ */
+constexpr uint8_t xyz_bits = _BV(X_AXIS) | _BV(Y_AXIS) | _BV(Z_AXIS);
+extern uint8_t axis_homed, axis_trusted;
+
+void homeaxis(const AxisEnum axis);
+void set_axis_is_at_home(const AxisEnum axis);
+void set_axis_never_homed(const AxisEnum axis);
+uint8_t axes_should_home(uint8_t axis_bits=0x07);
+bool homing_needed_error(uint8_t axis_bits=0x07);
+
+FORCE_INLINE bool axis_was_homed(const AxisEnum axis)     { return TEST(axis_homed, axis); }
+FORCE_INLINE bool axis_is_trusted(const AxisEnum axis)    { return TEST(axis_trusted, axis); }
+FORCE_INLINE bool axis_should_home(const AxisEnum axis)   { return (axes_should_home() & _BV(axis)) != 0; }
+FORCE_INLINE bool no_axes_homed()                         { return !axis_homed; }
+FORCE_INLINE bool all_axes_homed()                        { return xyz_bits == (axis_homed & xyz_bits); }
+FORCE_INLINE bool homing_needed()                         { return !all_axes_homed(); }
+FORCE_INLINE bool all_axes_trusted()                      { return xyz_bits == (axis_trusted & xyz_bits); }
+FORCE_INLINE void set_axis_homed(const AxisEnum axis)     { SBI(axis_homed, axis); }
+FORCE_INLINE void set_axis_unhomed(const AxisEnum axis)   { CBI(axis_homed, axis); }
+FORCE_INLINE void set_axis_trusted(const AxisEnum axis)   { SBI(axis_trusted, axis); }
+FORCE_INLINE void set_axis_untrusted(const AxisEnum axis) { CBI(axis_trusted, axis); }
+FORCE_INLINE void set_all_homed()                         { axis_homed = axis_trusted = xyz_bits; }
+FORCE_INLINE void set_all_unhomed()                       { axis_homed = axis_trusted = 0; }
+
+#if ENABLED(NO_MOTION_BEFORE_HOMING)
+  #define MOTION_CONDITIONS (IsRunning() && !homing_needed_error())
+#else
+  #define MOTION_CONDITIONS IsRunning()
+#endif
+
+#define BABYSTEP_ALLOWED() ((ENABLED(BABYSTEP_WITHOUT_HOMING) || all_axes_trusted()) && (ENABLED(BABYSTEP_ALWAYS_AVAILABLE) || printer_busy()))
+
+/**
+ * Workspace offsets
+ */
+#if HAS_HOME_OFFSET || HAS_POSITION_SHIFT
+  #if HAS_HOME_OFFSET
+    extern xyz_pos_t home_offset;
+  #endif
+  #if HAS_POSITION_SHIFT
+    extern xyz_pos_t position_shift;
+  #endif
+  #if HAS_HOME_OFFSET && HAS_POSITION_SHIFT
+    extern xyz_pos_t workspace_offset;
+    #define _WS workspace_offset
+  #elif HAS_HOME_OFFSET
+    #define _WS home_offset
+  #else
+    #define _WS position_shift
+  #endif
+  #define NATIVE_TO_LOGICAL(POS, AXIS) ((POS) + _WS[AXIS])
+  #define LOGICAL_TO_NATIVE(POS, AXIS) ((POS) - _WS[AXIS])
+  FORCE_INLINE void toLogical(xy_pos_t &raw)   { raw += _WS; }
+  FORCE_INLINE void toLogical(xyz_pos_t &raw)  { raw += _WS; }
+  FORCE_INLINE void toLogical(xyze_pos_t &raw) { raw += _WS; }
+  FORCE_INLINE void toNative(xy_pos_t &raw)    { raw -= _WS; }
+  FORCE_INLINE void toNative(xyz_pos_t &raw)   { raw -= _WS; }
+  FORCE_INLINE void toNative(xyze_pos_t &raw)  { raw -= _WS; }
+#else
+  #define NATIVE_TO_LOGICAL(POS, AXIS) (POS)
+  #define LOGICAL_TO_NATIVE(POS, AXIS) (POS)
+  FORCE_INLINE void toLogical(xy_pos_t&)   {}
+  FORCE_INLINE void toLogical(xyz_pos_t&)  {}
+  FORCE_INLINE void toLogical(xyze_pos_t&) {}
+  FORCE_INLINE void toNative(xy_pos_t&)    {}
+  FORCE_INLINE void toNative(xyz_pos_t&)   {}
+  FORCE_INLINE void toNative(xyze_pos_t&)  {}
+#endif
+#define LOGICAL_X_POSITION(POS) NATIVE_TO_LOGICAL(POS, X_AXIS)
+#define LOGICAL_Y_POSITION(POS) NATIVE_TO_LOGICAL(POS, Y_AXIS)
+#define LOGICAL_Z_POSITION(POS) NATIVE_TO_LOGICAL(POS, Z_AXIS)
+#define RAW_X_POSITION(POS)     LOGICAL_TO_NATIVE(POS, X_AXIS)
+#define RAW_Y_POSITION(POS)     LOGICAL_TO_NATIVE(POS, Y_AXIS)
+#define RAW_Z_POSITION(POS)     LOGICAL_TO_NATIVE(POS, Z_AXIS)
+
+/**
+ * position_is_reachable family of functions
+ */
+
+#if IS_KINEMATIC // (DELTA or SCARA)
+
+  #if HAS_SCARA_OFFSET
+    extern abc_pos_t scara_home_offset; // A and B angular offsets, Z mm offset
+  #endif
+
+  // Return true if the given point is within the printable area
+  inline bool position_is_reachable(const float &rx, const float &ry, const float inset=0) {
+    #if ENABLED(DELTA)
+      return HYPOT2(rx, ry) <= sq(DELTA_PRINTABLE_RADIUS - inset + fslop);
+    #elif IS_SCARA
+      const float R2 = HYPOT2(rx - SCARA_OFFSET_X, ry - SCARA_OFFSET_Y);
+      return (
+        R2 <= sq(L1 + L2) - inset
+        #if MIDDLE_DEAD_ZONE_R > 0
+          && R2 >= sq(float(MIDDLE_DEAD_ZONE_R))
+        #endif
+      );
+    #endif
+  }
+
+  inline bool position_is_reachable(const xy_pos_t &pos, const float inset=0) {
+    return position_is_reachable(pos.x, pos.y, inset);
+  }
+
+#else // CARTESIAN
+
+  // Return true if the given position is within the machine bounds.
+  inline bool position_is_reachable(const float &rx, const float &ry) {
+    if (!WITHIN(ry, Y_MIN_POS - fslop, Y_MAX_POS + fslop)) return false;
+    #if ENABLED(DUAL_X_CARRIAGE)
+      if (active_extruder)
+        return WITHIN(rx, X2_MIN_POS - fslop, X2_MAX_POS + fslop);
+      else
+        return WITHIN(rx, X1_MIN_POS - fslop, X1_MAX_POS + fslop);
+    #else
+      return WITHIN(rx, X_MIN_POS - fslop, X_MAX_POS + fslop);
+    #endif
+  }
+  inline bool position_is_reachable(const xy_pos_t &pos) { return position_is_reachable(pos.x, pos.y); }
+
+#endif // CARTESIAN
+
+/**
+ * Duplication mode
+ */
+#if HAS_DUPLICATION_MODE
+  extern bool extruder_duplication_enabled;       // Used in Dual X mode 2
+#endif
+
+/**
+ * Dual X Carriage
+ */
+#if ENABLED(DUAL_X_CARRIAGE)
+
+  enum DualXMode : char {
+    DXC_FULL_CONTROL_MODE,
+    DXC_AUTO_PARK_MODE,
+    DXC_DUPLICATION_MODE,
+    DXC_MIRRORED_MODE
+  };
+
+  extern DualXMode dual_x_carriage_mode;
+  extern float inactive_extruder_x,               // Used in mode 0 & 1
+               duplicate_extruder_x_offset;       // Used in mode 2 & 3
+  extern xyz_pos_t raised_parked_position;        // Used in mode 1
+  extern bool active_extruder_parked;             // Used in mode 1, 2 & 3
+  extern millis_t delayed_move_time;              // Used in mode 1
+  extern int16_t duplicate_extruder_temp_offset;  // Used in mode 2 & 3
+  extern bool idex_mirrored_mode;                 // Used in mode 3
+
+  FORCE_INLINE bool idex_is_duplicating() { return dual_x_carriage_mode >= DXC_DUPLICATION_MODE; }
+
+  float x_home_pos(const uint8_t extruder);
+
+  FORCE_INLINE int x_home_dir(const uint8_t extruder) { return extruder ? X2_HOME_DIR : X_HOME_DIR; }
+
+  void set_duplication_enabled(const bool dupe, const int8_t tool_index=-1);
+  void idex_set_mirrored_mode(const bool mirr);
+  void idex_set_parked(const bool park=true);
+
+#else
+
+  #if ENABLED(MULTI_NOZZLE_DUPLICATION)
+    extern uint8_t duplication_e_mask;
+    enum DualXMode : char { DXC_DUPLICATION_MODE = 2 };
+    FORCE_INLINE void set_duplication_enabled(const bool dupe) { extruder_duplication_enabled = dupe; }
+  #endif
+
+  FORCE_INLINE int x_home_dir(const uint8_t) { return home_dir(X_AXIS); }
+
+#endif
+
+#if HAS_M206_COMMAND
+  void set_home_offset(const AxisEnum axis, const float v);
+#endif
+
+#if USE_SENSORLESS
+  struct sensorless_t;
+  sensorless_t start_sensorless_homing_per_axis(const AxisEnum axis);
+  void end_sensorless_homing_per_axis(const AxisEnum axis, sensorless_t enable_stealth);
+#endif
diff --git a/Marlin/src/module/planner.cpp b/Marlin/src/module/planner.cpp
new file mode 100644
index 0000000..5897d10
--- /dev/null
+++ b/Marlin/src/module/planner.cpp
@@ -0,0 +1,3099 @@
+/**
+ * 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/>.
+ *
+ */
+
+/**
+ * planner.cpp
+ *
+ * Buffer movement commands and manage the acceleration profile plan
+ *
+ * Derived from Grbl
+ * Copyright (c) 2009-2011 Simen Svale Skogsrud
+ *
+ * The ring buffer implementation gleaned from the wiring_serial library by David A. Mellis.
+ *
+ *
+ * Reasoning behind the mathematics in this module (in the key of 'Mathematica'):
+ *
+ * s == speed, a == acceleration, t == time, d == distance
+ *
+ * Basic definitions:
+ *   Speed[s_, a_, t_] := s + (a*t)
+ *   Travel[s_, a_, t_] := Integrate[Speed[s, a, t], t]
+ *
+ * Distance to reach a specific speed with a constant acceleration:
+ *   Solve[{Speed[s, a, t] == m, Travel[s, a, t] == d}, d, t]
+ *   d -> (m^2 - s^2)/(2 a) --> estimate_acceleration_distance()
+ *
+ * Speed after a given distance of travel with constant acceleration:
+ *   Solve[{Speed[s, a, t] == m, Travel[s, a, t] == d}, m, t]
+ *   m -> Sqrt[2 a d + s^2]
+ *
+ * DestinationSpeed[s_, a_, d_] := Sqrt[2 a d + s^2]
+ *
+ * When to start braking (di) to reach a specified destination speed (s2) after accelerating
+ * from initial speed s1 without ever stopping at a plateau:
+ *   Solve[{DestinationSpeed[s1, a, di] == DestinationSpeed[s2, a, d - di]}, di]
+ *   di -> (2 a d - s1^2 + s2^2)/(4 a) --> intersection_distance()
+ *
+ * IntersectionDistance[s1_, s2_, a_, d_] := (2 a d - s1^2 + s2^2)/(4 a)
+ *
+ * --
+ *
+ * The fast inverse function needed for Bézier interpolation for AVR
+ * was designed, written and tested by Eduardo José Tagle on April/2018
+ */
+
+#include "planner.h"
+#include "stepper.h"
+#include "motion.h"
+#include "temperature.h"
+#include "../lcd/marlinui.h"
+#include "../gcode/parser.h"
+
+#include "../MarlinCore.h"
+
+#if HAS_LEVELING
+  #include "../feature/bedlevel/bedlevel.h"
+#endif
+
+#if ENABLED(FILAMENT_WIDTH_SENSOR)
+  #include "../feature/filwidth.h"
+#endif
+
+#if ENABLED(BARICUDA)
+  #include "../feature/baricuda.h"
+#endif
+
+#if ENABLED(MIXING_EXTRUDER)
+  #include "../feature/mixing.h"
+#endif
+
+#if ENABLED(AUTO_POWER_CONTROL)
+  #include "../feature/power.h"
+#endif
+
+#if ENABLED(EXTERNAL_CLOSED_LOOP_CONTROLLER)
+  #include "../feature/closedloop.h"
+#endif
+
+#if ENABLED(BACKLASH_COMPENSATION)
+  #include "../feature/backlash.h"
+#endif
+
+#if ENABLED(CANCEL_OBJECTS)
+  #include "../feature/cancel_object.h"
+#endif
+
+#if ENABLED(POWER_LOSS_RECOVERY)
+  #include "../feature/powerloss.h"
+#endif
+
+#if HAS_CUTTER
+  #include "../feature/spindle_laser.h"
+#endif
+
+// Delay for delivery of first block to the stepper ISR, if the queue contains 2 or
+// fewer movements. The delay is measured in milliseconds, and must be less than 250ms
+#define BLOCK_DELAY_FOR_1ST_MOVE 100
+
+Planner planner;
+
+// public:
+
+/**
+ * A ring buffer of moves described in steps
+ */
+block_t Planner::block_buffer[BLOCK_BUFFER_SIZE];
+volatile uint8_t Planner::block_buffer_head,    // Index of the next block to be pushed
+                 Planner::block_buffer_nonbusy, // Index of the first non-busy block
+                 Planner::block_buffer_planned, // Index of the optimally planned block
+                 Planner::block_buffer_tail;    // Index of the busy block, if any
+uint16_t Planner::cleaning_buffer_counter;      // A counter to disable queuing of blocks
+uint8_t Planner::delay_before_delivering;       // This counter delays delivery of blocks when queue becomes empty to allow the opportunity of merging blocks
+
+planner_settings_t Planner::settings;           // Initialized by settings.load()
+
+#if ENABLED(LASER_POWER_INLINE)
+  laser_state_t Planner::laser_inline;          // Current state for blocks
+#endif
+
+uint32_t Planner::max_acceleration_steps_per_s2[XYZE_N]; // (steps/s^2) Derived from mm_per_s2
+
+float Planner::steps_to_mm[XYZE_N];             // (mm) Millimeters per step
+
+#if HAS_JUNCTION_DEVIATION
+  float Planner::junction_deviation_mm;         // (mm) M205 J
+  #if HAS_LINEAR_E_JERK
+    float Planner::max_e_jerk[DISTINCT_E];      // Calculated from junction_deviation_mm
+  #endif
+#endif
+
+#if HAS_CLASSIC_JERK
+  TERN(HAS_LINEAR_E_JERK, xyz_pos_t, xyze_pos_t) Planner::max_jerk;
+#endif
+
+#if ENABLED(SD_ABORT_ON_ENDSTOP_HIT)
+  bool Planner::abort_on_endstop_hit = false;
+#endif
+
+#if ENABLED(DISTINCT_E_FACTORS)
+  uint8_t Planner::last_extruder = 0;     // Respond to extruder change
+#endif
+
+#if ENABLED(DIRECT_STEPPING)
+  uint32_t Planner::last_page_step_rate = 0;
+  xyze_bool_t Planner::last_page_dir{0};
+#endif
+
+#if EXTRUDERS
+  int16_t Planner::flow_percentage[EXTRUDERS] = ARRAY_BY_EXTRUDERS1(100); // Extrusion factor for each extruder
+  float Planner::e_factor[EXTRUDERS] = ARRAY_BY_EXTRUDERS1(1.0f); // The flow percentage and volumetric multiplier combine to scale E movement
+#endif
+
+#if DISABLED(NO_VOLUMETRICS)
+  float Planner::filament_size[EXTRUDERS],          // diameter of filament (in millimeters), typically around 1.75 or 2.85, 0 disables the volumetric calculations for the extruder
+        Planner::volumetric_area_nominal = CIRCLE_AREA(float(DEFAULT_NOMINAL_FILAMENT_DIA) * 0.5f), // Nominal cross-sectional area
+        Planner::volumetric_multiplier[EXTRUDERS];  // Reciprocal of cross-sectional area of filament (in mm^2). Pre-calculated to reduce computation in the planner
+#endif
+
+#if ENABLED(VOLUMETRIC_EXTRUDER_LIMIT)
+  float Planner::volumetric_extruder_limit[EXTRUDERS],          // max mm^3/sec the extruder is able to handle
+        Planner::volumetric_extruder_feedrate_limit[EXTRUDERS]; // pre calculated extruder feedrate limit based on volumetric_extruder_limit; pre-calculated to reduce computation in the planner
+#endif
+
+#if HAS_LEVELING
+  bool Planner::leveling_active = false; // Flag that auto bed leveling is enabled
+  #if ABL_PLANAR
+    matrix_3x3 Planner::bed_level_matrix; // Transform to compensate for bed level
+  #endif
+  #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
+    float Planner::z_fade_height,      // Initialized by settings.load()
+          Planner::inverse_z_fade_height,
+          Planner::last_fade_z;
+  #endif
+#else
+  constexpr bool Planner::leveling_active;
+#endif
+
+skew_factor_t Planner::skew_factor; // Initialized by settings.load()
+
+#if ENABLED(AUTOTEMP)
+  float Planner::autotemp_max = 250,
+        Planner::autotemp_min = 210,
+        Planner::autotemp_factor = 0.1f;
+  bool Planner::autotemp_enabled = false;
+#endif
+
+// private:
+
+xyze_long_t Planner::position{0};
+
+uint32_t Planner::cutoff_long;
+
+xyze_float_t Planner::previous_speed;
+float Planner::previous_nominal_speed_sqr;
+
+#if ENABLED(DISABLE_INACTIVE_EXTRUDER)
+  last_move_t Planner::g_uc_extruder_last_move[EXTRUDERS] = { 0 };
+#endif
+
+#ifdef XY_FREQUENCY_LIMIT
+  int8_t Planner::xy_freq_limit_hz = XY_FREQUENCY_LIMIT;
+  float Planner::xy_freq_min_speed_factor = (XY_FREQUENCY_MIN_PERCENT) * 0.01f;
+  int32_t Planner::xy_freq_min_interval_us = LROUND(1000000.0 / (XY_FREQUENCY_LIMIT));
+#endif
+
+#if ENABLED(LIN_ADVANCE)
+  float Planner::extruder_advance_K[EXTRUDERS]; // Initialized by settings.load()
+#endif
+
+#if HAS_POSITION_FLOAT
+  xyze_pos_t Planner::position_float; // Needed for accurate maths. Steps cannot be used!
+#endif
+
+#if IS_KINEMATIC
+  xyze_pos_t Planner::position_cart;
+#endif
+
+#if HAS_WIRED_LCD
+  volatile uint32_t Planner::block_buffer_runtime_us = 0;
+#endif
+
+/**
+ * Class and Instance Methods
+ */
+
+Planner::Planner() { init(); }
+
+void Planner::init() {
+  position.reset();
+  TERN_(HAS_POSITION_FLOAT, position_float.reset());
+  TERN_(IS_KINEMATIC, position_cart.reset());
+  previous_speed.reset();
+  previous_nominal_speed_sqr = 0;
+  TERN_(ABL_PLANAR, bed_level_matrix.set_to_identity());
+  clear_block_buffer();
+  delay_before_delivering = 0;
+  #if ENABLED(DIRECT_STEPPING)
+    last_page_step_rate = 0;
+    last_page_dir.reset();
+  #endif
+}
+
+#if ENABLED(S_CURVE_ACCELERATION)
+  #ifdef __AVR__
+    /**
+     * This routine returns 0x1000000 / d, getting the inverse as fast as possible.
+     * A fast-converging iterative Newton-Raphson method can reach full precision in
+     * just 1 iteration, and takes 211 cycles (worst case; the mean case is less, up
+     * to 30 cycles for small divisors), instead of the 500 cycles a normal division
+     * would take.
+     *
+     * Inspired by the following page:
+     *  https://stackoverflow.com/questions/27801397/newton-raphson-division-with-big-integers
+     *
+     * Suppose we want to calculate  floor(2 ^ k / B)  where B is a positive integer
+     * Then, B must be <= 2^k, otherwise, the quotient is 0.
+     *
+     * The Newton - Raphson iteration for x = B / 2 ^ k yields:
+     *  q[n + 1] = q[n] * (2 - q[n] * B / 2 ^ k)
+     *
+     * This can be rearranged to:
+     *  q[n + 1] = q[n] * (2 ^ (k + 1) - q[n] * B) >> k
+     *
+     * Each iteration requires only integer multiplications and bit shifts.
+     * It doesn't necessarily converge to floor(2 ^ k / B) but in the worst case
+     * it eventually alternates between floor(2 ^ k / B) and ceil(2 ^ k / B).
+     * So it checks for this case and extracts floor(2 ^ k / B).
+     *
+     * A simple but important optimization for this approach is to truncate
+     * multiplications (i.e., calculate only the higher bits of the product) in the
+     * early iterations of the Newton - Raphson method. This is done so the results
+     * of the early iterations are far from the quotient. Then it doesn't matter if
+     * they are done inaccurately.
+     * It's important to pick a good starting value for x. Knowing how many
+     * digits the divisor has, it can be estimated:
+     *
+     *   2^k / x = 2 ^ log2(2^k / x)
+     *   2^k / x = 2 ^(log2(2^k)-log2(x))
+     *   2^k / x = 2 ^(k*log2(2)-log2(x))
+     *   2^k / x = 2 ^ (k-log2(x))
+     *   2^k / x >= 2 ^ (k-floor(log2(x)))
+     *   floor(log2(x)) is simply the index of the most significant bit set.
+     *
+     * If this estimation can be improved even further the number of iterations can be
+     * reduced a lot, saving valuable execution time.
+     * The paper "Software Integer Division" by Thomas L.Rodeheffer, Microsoft
+     * Research, Silicon Valley,August 26, 2008, available at
+     * https://www.microsoft.com/en-us/research/wp-content/uploads/2008/08/tr-2008-141.pdf
+     * suggests, for its integer division algorithm, using a table to supply the first
+     * 8 bits of precision, then, due to the quadratic convergence nature of the
+     * Newton-Raphon iteration, just 2 iterations should be enough to get maximum
+     * precision of the division.
+     * By precomputing values of inverses for small denominator values, just one
+     * Newton-Raphson iteration is enough to reach full precision.
+     * This code uses the top 9 bits of the denominator as index.
+     *
+     * The AVR assembly function implements this C code using the data below:
+     *
+     *  // For small divisors, it is best to directly retrieve the results
+     *  if (d <= 110) return pgm_read_dword(&small_inv_tab[d]);
+     *
+     *  // Compute initial estimation of 0x1000000/x -
+     *  // Get most significant bit set on divider
+     *  uint8_t idx = 0;
+     *  uint32_t nr = d;
+     *  if (!(nr & 0xFF0000)) {
+     *    nr <<= 8; idx += 8;
+     *    if (!(nr & 0xFF0000)) { nr <<= 8; idx += 8; }
+     *  }
+     *  if (!(nr & 0xF00000)) { nr <<= 4; idx += 4; }
+     *  if (!(nr & 0xC00000)) { nr <<= 2; idx += 2; }
+     *  if (!(nr & 0x800000)) { nr <<= 1; idx += 1; }
+     *
+     *  // Isolate top 9 bits of the denominator, to be used as index into the initial estimation table
+     *  uint32_t tidx = nr >> 15,                                       // top 9 bits. bit8 is always set
+     *           ie = inv_tab[tidx & 0xFF] + 256,                       // Get the table value. bit9 is always set
+     *           x = idx <= 8 ? (ie >> (8 - idx)) : (ie << (idx - 8));  // Position the estimation at the proper place
+     *
+     *  x = uint32_t((x * uint64_t(_BV(25) - x * d)) >> 24);            // Refine estimation by newton-raphson. 1 iteration is enough
+     *  const uint32_t r = _BV(24) - x * d;                             // Estimate remainder
+     *  if (r >= d) x++;                                                // Check whether to adjust result
+     *  return uint32_t(x);                                             // x holds the proper estimation
+     */
+    static uint32_t get_period_inverse(uint32_t d) {
+
+      static const uint8_t inv_tab[256] PROGMEM = {
+        255,253,252,250,248,246,244,242,240,238,236,234,233,231,229,227,
+        225,224,222,220,218,217,215,213,212,210,208,207,205,203,202,200,
+        199,197,195,194,192,191,189,188,186,185,183,182,180,179,178,176,
+        175,173,172,170,169,168,166,165,164,162,161,160,158,157,156,154,
+        153,152,151,149,148,147,146,144,143,142,141,139,138,137,136,135,
+        134,132,131,130,129,128,127,126,125,123,122,121,120,119,118,117,
+        116,115,114,113,112,111,110,109,108,107,106,105,104,103,102,101,
+        100,99,98,97,96,95,94,93,92,91,90,89,88,88,87,86,
+        85,84,83,82,81,80,80,79,78,77,76,75,74,74,73,72,
+        71,70,70,69,68,67,66,66,65,64,63,62,62,61,60,59,
+        59,58,57,56,56,55,54,53,53,52,51,50,50,49,48,48,
+        47,46,46,45,44,43,43,42,41,41,40,39,39,38,37,37,
+        36,35,35,34,33,33,32,32,31,30,30,29,28,28,27,27,
+        26,25,25,24,24,23,22,22,21,21,20,19,19,18,18,17,
+        17,16,15,15,14,14,13,13,12,12,11,10,10,9,9,8,
+        8,7,7,6,6,5,5,4,4,3,3,2,2,1,0,0
+      };
+
+      // For small denominators, it is cheaper to directly store the result.
+      //  For bigger ones, just ONE Newton-Raphson iteration is enough to get
+      //  maximum precision we need
+      static const uint32_t small_inv_tab[111] PROGMEM = {
+        16777216,16777216,8388608,5592405,4194304,3355443,2796202,2396745,2097152,1864135,1677721,1525201,1398101,1290555,1198372,1118481,
+        1048576,986895,932067,883011,838860,798915,762600,729444,699050,671088,645277,621378,599186,578524,559240,541200,
+        524288,508400,493447,479349,466033,453438,441505,430185,419430,409200,399457,390167,381300,372827,364722,356962,
+        349525,342392,335544,328965,322638,316551,310689,305040,299593,294337,289262,284359,279620,275036,270600,266305,
+        262144,258111,254200,250406,246723,243148,239674,236298,233016,229824,226719,223696,220752,217885,215092,212369,
+        209715,207126,204600,202135,199728,197379,195083,192841,190650,188508,186413,184365,182361,180400,178481,176602,
+        174762,172960,171196,169466,167772,166111,164482,162885,161319,159783,158275,156796,155344,153919,152520
+      };
+
+      // For small divisors, it is best to directly retrieve the results
+      if (d <= 110) return pgm_read_dword(&small_inv_tab[d]);
+
+      uint8_t r8 = d & 0xFF,
+              r9 = (d >> 8) & 0xFF,
+              r10 = (d >> 16) & 0xFF,
+              r2,r3,r4,r5,r6,r7,r11,r12,r13,r14,r15,r16,r17,r18;
+      const uint8_t* ptab = inv_tab;
+
+      __asm__ __volatile__(
+        // %8:%7:%6 = interval
+        // r31:r30: MUST be those registers, and they must point to the inv_tab
+
+        A("clr %13")                       // %13 = 0
+
+        // Now we must compute
+        // result = 0xFFFFFF / d
+        // %8:%7:%6 = interval
+        // %16:%15:%14 = nr
+        // %13 = 0
+
+        // A plain division of 24x24 bits should take 388 cycles to complete. We will
+        // use Newton-Raphson for the calculation, and will strive to get way less cycles
+        // for the same result - Using C division, it takes 500cycles to complete .
+
+        A("clr %3")                       // idx = 0
+        A("mov %14,%6")
+        A("mov %15,%7")
+        A("mov %16,%8")                   // nr = interval
+        A("tst %16")                      // nr & 0xFF0000 == 0 ?
+        A("brne 2f")                      // No, skip this
+        A("mov %16,%15")
+        A("mov %15,%14")                  // nr <<= 8, %14 not needed
+        A("subi %3,-8")                   // idx += 8
+        A("tst %16")                      // nr & 0xFF0000 == 0 ?
+        A("brne 2f")                      // No, skip this
+        A("mov %16,%15")                  // nr <<= 8, %14 not needed
+        A("clr %15")                      // We clear %14
+        A("subi %3,-8")                   // idx += 8
+
+        // here %16 != 0 and %16:%15 contains at least 9 MSBits, or both %16:%15 are 0
+        L("2")
+        A("cpi %16,0x10")                 // (nr & 0xF00000) == 0 ?
+        A("brcc 3f")                      // No, skip this
+        A("swap %15")                     // Swap nibbles
+        A("swap %16")                     // Swap nibbles. Low nibble is 0
+        A("mov %14, %15")
+        A("andi %14,0x0F")                // Isolate low nibble
+        A("andi %15,0xF0")                // Keep proper nibble in %15
+        A("or %16, %14")                  // %16:%15 <<= 4
+        A("subi %3,-4")                   // idx += 4
+
+        L("3")
+        A("cpi %16,0x40")                 // (nr & 0xC00000) == 0 ?
+        A("brcc 4f")                      // No, skip this
+        A("add %15,%15")
+        A("adc %16,%16")
+        A("add %15,%15")
+        A("adc %16,%16")                  // %16:%15 <<= 2
+        A("subi %3,-2")                   // idx += 2
+
+        L("4")
+        A("cpi %16,0x80")                 // (nr & 0x800000) == 0 ?
+        A("brcc 5f")                      // No, skip this
+        A("add %15,%15")
+        A("adc %16,%16")                  // %16:%15 <<= 1
+        A("inc %3")                       // idx += 1
+
+        // Now %16:%15 contains its MSBit set to 1, or %16:%15 is == 0. We are now absolutely sure
+        // we have at least 9 MSBits available to enter the initial estimation table
+        L("5")
+        A("add %15,%15")
+        A("adc %16,%16")                  // %16:%15 = tidx = (nr <<= 1), we lose the top MSBit (always set to 1, %16 is the index into the inverse table)
+        A("add r30,%16")                  // Only use top 8 bits
+        A("adc r31,%13")                  // r31:r30 = inv_tab + (tidx)
+        A("lpm %14, Z")                   // %14 = inv_tab[tidx]
+        A("ldi %15, 1")                   // %15 = 1  %15:%14 = inv_tab[tidx] + 256
+
+        // We must scale the approximation to the proper place
+        A("clr %16")                      // %16 will always be 0 here
+        A("subi %3,8")                    // idx == 8 ?
+        A("breq 6f")                      // yes, no need to scale
+        A("brcs 7f")                      // If C=1, means idx < 8, result was negative!
+
+        // idx > 8, now %3 = idx - 8. We must perform a left shift. idx range:[1-8]
+        A("sbrs %3,0")                    // shift by 1bit position?
+        A("rjmp 8f")                      // No
+        A("add %14,%14")
+        A("adc %15,%15")                  // %15:16 <<= 1
+        L("8")
+        A("sbrs %3,1")                    // shift by 2bit position?
+        A("rjmp 9f")                      // No
+        A("add %14,%14")
+        A("adc %15,%15")
+        A("add %14,%14")
+        A("adc %15,%15")                  // %15:16 <<= 1
+        L("9")
+        A("sbrs %3,2")                    // shift by 4bits position?
+        A("rjmp 16f")                     // No
+        A("swap %15")                     // Swap nibbles. lo nibble of %15 will always be 0
+        A("swap %14")                     // Swap nibbles
+        A("mov %12,%14")
+        A("andi %12,0x0F")                // isolate low nibble
+        A("andi %14,0xF0")                // and clear it
+        A("or %15,%12")                   // %15:%16 <<= 4
+        L("16")
+        A("sbrs %3,3")                    // shift by 8bits position?
+        A("rjmp 6f")                      // No, we are done
+        A("mov %16,%15")
+        A("mov %15,%14")
+        A("clr %14")
+        A("jmp 6f")
+
+        // idx < 8, now %3 = idx - 8. Get the count of bits
+        L("7")
+        A("neg %3")                       // %3 = -idx = count of bits to move right. idx range:[1...8]
+        A("sbrs %3,0")                    // shift by 1 bit position ?
+        A("rjmp 10f")                     // No, skip it
+        A("asr %15")                      // (bit7 is always 0 here)
+        A("ror %14")
+        L("10")
+        A("sbrs %3,1")                    // shift by 2 bit position ?
+        A("rjmp 11f")                     // No, skip it
+        A("asr %15")                      // (bit7 is always 0 here)
+        A("ror %14")
+        A("asr %15")                      // (bit7 is always 0 here)
+        A("ror %14")
+        L("11")
+        A("sbrs %3,2")                    // shift by 4 bit position ?
+        A("rjmp 12f")                     // No, skip it
+        A("swap %15")                     // Swap nibbles
+        A("andi %14, 0xF0")               // Lose the lowest nibble
+        A("swap %14")                     // Swap nibbles. Upper nibble is 0
+        A("or %14,%15")                   // Pass nibble from upper byte
+        A("andi %15, 0x0F")               // And get rid of that nibble
+        L("12")
+        A("sbrs %3,3")                    // shift by 8 bit position ?
+        A("rjmp 6f")                      // No, skip it
+        A("mov %14,%15")
+        A("clr %15")
+        L("6")                            // %16:%15:%14 = initial estimation of 0x1000000 / d
+
+        // Now, we must refine the estimation present on %16:%15:%14 using 1 iteration
+        // of Newton-Raphson. As it has a quadratic convergence, 1 iteration is enough
+        // to get more than 18bits of precision (the initial table lookup gives 9 bits of
+        // precision to start from). 18bits of precision is all what is needed here for result
+
+        // %8:%7:%6 = d = interval
+        // %16:%15:%14 = x = initial estimation of 0x1000000 / d
+        // %13 = 0
+        // %3:%2:%1:%0 = working accumulator
+
+        // Compute 1<<25 - x*d. Result should never exceed 25 bits and should always be positive
+        A("clr %0")
+        A("clr %1")
+        A("clr %2")
+        A("ldi %3,2")                     // %3:%2:%1:%0 = 0x2000000
+        A("mul %6,%14")                   // r1:r0 = LO(d) * LO(x)
+        A("sub %0,r0")
+        A("sbc %1,r1")
+        A("sbc %2,%13")
+        A("sbc %3,%13")                   // %3:%2:%1:%0 -= LO(d) * LO(x)
+        A("mul %7,%14")                   // r1:r0 = MI(d) * LO(x)
+        A("sub %1,r0")
+        A("sbc %2,r1" )
+        A("sbc %3,%13")                   // %3:%2:%1:%0 -= MI(d) * LO(x) << 8
+        A("mul %8,%14")                   // r1:r0 = HI(d) * LO(x)
+        A("sub %2,r0")
+        A("sbc %3,r1")                    // %3:%2:%1:%0 -= MIL(d) * LO(x) << 16
+        A("mul %6,%15")                   // r1:r0 = LO(d) * MI(x)
+        A("sub %1,r0")
+        A("sbc %2,r1")
+        A("sbc %3,%13")                   // %3:%2:%1:%0 -= LO(d) * MI(x) << 8
+        A("mul %7,%15")                   // r1:r0 = MI(d) * MI(x)
+        A("sub %2,r0")
+        A("sbc %3,r1")                    // %3:%2:%1:%0 -= MI(d) * MI(x) << 16
+        A("mul %8,%15")                   // r1:r0 = HI(d) * MI(x)
+        A("sub %3,r0")                    // %3:%2:%1:%0 -= MIL(d) * MI(x) << 24
+        A("mul %6,%16")                   // r1:r0 = LO(d) * HI(x)
+        A("sub %2,r0")
+        A("sbc %3,r1")                    // %3:%2:%1:%0 -= LO(d) * HI(x) << 16
+        A("mul %7,%16")                   // r1:r0 = MI(d) * HI(x)
+        A("sub %3,r0")                    // %3:%2:%1:%0 -= MI(d) * HI(x) << 24
+        // %3:%2:%1:%0 = (1<<25) - x*d     [169]
+
+        // We need to multiply that result by x, and we are only interested in the top 24bits of that multiply
+
+        // %16:%15:%14 = x = initial estimation of 0x1000000 / d
+        // %3:%2:%1:%0 = (1<<25) - x*d = acc
+        // %13 = 0
+
+        // result = %11:%10:%9:%5:%4
+        A("mul %14,%0")                   // r1:r0 = LO(x) * LO(acc)
+        A("mov %4,r1")
+        A("clr %5")
+        A("clr %9")
+        A("clr %10")
+        A("clr %11")                      // %11:%10:%9:%5:%4 = LO(x) * LO(acc) >> 8
+        A("mul %15,%0")                   // r1:r0 = MI(x) * LO(acc)
+        A("add %4,r0")
+        A("adc %5,r1")
+        A("adc %9,%13")
+        A("adc %10,%13")
+        A("adc %11,%13")                  // %11:%10:%9:%5:%4 += MI(x) * LO(acc)
+        A("mul %16,%0")                   // r1:r0 = HI(x) * LO(acc)
+        A("add %5,r0")
+        A("adc %9,r1")
+        A("adc %10,%13")
+        A("adc %11,%13")                  // %11:%10:%9:%5:%4 += MI(x) * LO(acc) << 8
+
+        A("mul %14,%1")                   // r1:r0 = LO(x) * MIL(acc)
+        A("add %4,r0")
+        A("adc %5,r1")
+        A("adc %9,%13")
+        A("adc %10,%13")
+        A("adc %11,%13")                  // %11:%10:%9:%5:%4 = LO(x) * MIL(acc)
+        A("mul %15,%1")                   // r1:r0 = MI(x) * MIL(acc)
+        A("add %5,r0")
+        A("adc %9,r1")
+        A("adc %10,%13")
+        A("adc %11,%13")                  // %11:%10:%9:%5:%4 += MI(x) * MIL(acc) << 8
+        A("mul %16,%1")                   // r1:r0 = HI(x) * MIL(acc)
+        A("add %9,r0")
+        A("adc %10,r1")
+        A("adc %11,%13")                  // %11:%10:%9:%5:%4 += MI(x) * MIL(acc) << 16
+
+        A("mul %14,%2")                   // r1:r0 = LO(x) * MIH(acc)
+        A("add %5,r0")
+        A("adc %9,r1")
+        A("adc %10,%13")
+        A("adc %11,%13")                  // %11:%10:%9:%5:%4 = LO(x) * MIH(acc) << 8
+        A("mul %15,%2")                   // r1:r0 = MI(x) * MIH(acc)
+        A("add %9,r0")
+        A("adc %10,r1")
+        A("adc %11,%13")                  // %11:%10:%9:%5:%4 += MI(x) * MIH(acc) << 16
+        A("mul %16,%2")                   // r1:r0 = HI(x) * MIH(acc)
+        A("add %10,r0")
+        A("adc %11,r1")                   // %11:%10:%9:%5:%4 += MI(x) * MIH(acc) << 24
+
+        A("mul %14,%3")                   // r1:r0 = LO(x) * HI(acc)
+        A("add %9,r0")
+        A("adc %10,r1")
+        A("adc %11,%13")                  // %11:%10:%9:%5:%4 = LO(x) * HI(acc) << 16
+        A("mul %15,%3")                   // r1:r0 = MI(x) * HI(acc)
+        A("add %10,r0")
+        A("adc %11,r1")                   // %11:%10:%9:%5:%4 += MI(x) * HI(acc) << 24
+        A("mul %16,%3")                   // r1:r0 = HI(x) * HI(acc)
+        A("add %11,r0")                   // %11:%10:%9:%5:%4 += MI(x) * HI(acc) << 32
+
+        // At this point, %11:%10:%9 contains the new estimation of x.
+
+        // Finally, we must correct the result. Estimate remainder as
+        // (1<<24) - x*d
+        // %11:%10:%9 = x
+        // %8:%7:%6 = d = interval" "\n\t"
+        A("ldi %3,1")
+        A("clr %2")
+        A("clr %1")
+        A("clr %0")                       // %3:%2:%1:%0 = 0x1000000
+        A("mul %6,%9")                    // r1:r0 = LO(d) * LO(x)
+        A("sub %0,r0")
+        A("sbc %1,r1")
+        A("sbc %2,%13")
+        A("sbc %3,%13")                   // %3:%2:%1:%0 -= LO(d) * LO(x)
+        A("mul %7,%9")                    // r1:r0 = MI(d) * LO(x)
+        A("sub %1,r0")
+        A("sbc %2,r1")
+        A("sbc %3,%13")                   // %3:%2:%1:%0 -= MI(d) * LO(x) << 8
+        A("mul %8,%9")                    // r1:r0 = HI(d) * LO(x)
+        A("sub %2,r0")
+        A("sbc %3,r1")                    // %3:%2:%1:%0 -= MIL(d) * LO(x) << 16
+        A("mul %6,%10")                   // r1:r0 = LO(d) * MI(x)
+        A("sub %1,r0")
+        A("sbc %2,r1")
+        A("sbc %3,%13")                   // %3:%2:%1:%0 -= LO(d) * MI(x) << 8
+        A("mul %7,%10")                   // r1:r0 = MI(d) * MI(x)
+        A("sub %2,r0")
+        A("sbc %3,r1")                    // %3:%2:%1:%0 -= MI(d) * MI(x) << 16
+        A("mul %8,%10")                   // r1:r0 = HI(d) * MI(x)
+        A("sub %3,r0")                    // %3:%2:%1:%0 -= MIL(d) * MI(x) << 24
+        A("mul %6,%11")                   // r1:r0 = LO(d) * HI(x)
+        A("sub %2,r0")
+        A("sbc %3,r1")                    // %3:%2:%1:%0 -= LO(d) * HI(x) << 16
+        A("mul %7,%11")                   // r1:r0 = MI(d) * HI(x)
+        A("sub %3,r0")                    // %3:%2:%1:%0 -= MI(d) * HI(x) << 24
+        // %3:%2:%1:%0 = r = (1<<24) - x*d
+        // %8:%7:%6 = d = interval
+
+        // Perform the final correction
+        A("sub %0,%6")
+        A("sbc %1,%7")
+        A("sbc %2,%8")                    // r -= d
+        A("brcs 14f")                     // if ( r >= d)
+
+        // %11:%10:%9 = x
+        A("ldi %3,1")
+        A("add %9,%3")
+        A("adc %10,%13")
+        A("adc %11,%13")                  // x++
+        L("14")
+
+        // Estimation is done. %11:%10:%9 = x
+        A("clr __zero_reg__")              // Make C runtime happy
+        // [211 cycles total]
+        : "=r" (r2),
+          "=r" (r3),
+          "=r" (r4),
+          "=d" (r5),
+          "=r" (r6),
+          "=r" (r7),
+          "+r" (r8),
+          "+r" (r9),
+          "+r" (r10),
+          "=d" (r11),
+          "=r" (r12),
+          "=r" (r13),
+          "=d" (r14),
+          "=d" (r15),
+          "=d" (r16),
+          "=d" (r17),
+          "=d" (r18),
+          "+z" (ptab)
+        :
+        : "r0", "r1", "cc"
+      );
+
+      // Return the result
+      return r11 | (uint16_t(r12) << 8) | (uint32_t(r13) << 16);
+    }
+  #else
+    // All other 32-bit MPUs can easily do inverse using hardware division,
+    // so we don't need to reduce precision or to use assembly language at all.
+    // This routine, for all other archs, returns 0x100000000 / d ~= 0xFFFFFFFF / d
+    static FORCE_INLINE uint32_t get_period_inverse(const uint32_t d) {
+      return d ? 0xFFFFFFFF / d : 0xFFFFFFFF;
+    }
+  #endif
+#endif
+
+#define MINIMAL_STEP_RATE 120
+
+/**
+ * Get the current block for processing
+ * and mark the block as busy.
+ * Return nullptr if the buffer is empty
+ * or if there is a first-block delay.
+ *
+ * WARNING: Called from Stepper ISR context!
+ */
+block_t* Planner::get_current_block() {
+  // Get the number of moves in the planner queue so far
+  const uint8_t nr_moves = movesplanned();
+
+  // If there are any moves queued ...
+  if (nr_moves) {
+
+    // If there is still delay of delivery of blocks running, decrement it
+    if (delay_before_delivering) {
+      --delay_before_delivering;
+      // If the number of movements queued is less than 3, and there is still time
+      //  to wait, do not deliver anything
+      if (nr_moves < 3 && delay_before_delivering) return nullptr;
+      delay_before_delivering = 0;
+    }
+
+    // If we are here, there is no excuse to deliver the block
+    block_t * const block = &block_buffer[block_buffer_tail];
+
+    // No trapezoid calculated? Don't execute yet.
+    if (TEST(block->flag, BLOCK_BIT_RECALCULATE)) return nullptr;
+
+    // We can't be sure how long an active block will take, so don't count it.
+    TERN_(HAS_WIRED_LCD, block_buffer_runtime_us -= block->segment_time_us);
+
+    // As this block is busy, advance the nonbusy block pointer
+    block_buffer_nonbusy = next_block_index(block_buffer_tail);
+
+    // Push block_buffer_planned pointer, if encountered.
+    if (block_buffer_tail == block_buffer_planned)
+      block_buffer_planned = block_buffer_nonbusy;
+
+    // Return the block
+    return block;
+  }
+
+  // The queue became empty
+  TERN_(HAS_WIRED_LCD, clear_block_buffer_runtime()); // paranoia. Buffer is empty now - so reset accumulated time to zero.
+
+  return nullptr;
+}
+
+/**
+ * Calculate trapezoid parameters, multiplying the entry- and exit-speeds
+ * by the provided factors.
+ **
+ * ############ VERY IMPORTANT ############
+ * NOTE that the PRECONDITION to call this function is that the block is
+ * NOT BUSY and it is marked as RECALCULATE. That WARRANTIES the Stepper ISR
+ * is not and will not use the block while we modify it, so it is safe to
+ * alter its values.
+ */
+void Planner::calculate_trapezoid_for_block(block_t* const block, const float &entry_factor, const float &exit_factor) {
+
+  uint32_t initial_rate = CEIL(block->nominal_rate * entry_factor),
+           final_rate = CEIL(block->nominal_rate * exit_factor); // (steps per second)
+
+  // Limit minimal step rate (Otherwise the timer will overflow.)
+  NOLESS(initial_rate, uint32_t(MINIMAL_STEP_RATE));
+  NOLESS(final_rate, uint32_t(MINIMAL_STEP_RATE));
+
+  #if ENABLED(S_CURVE_ACCELERATION)
+    uint32_t cruise_rate = initial_rate;
+  #endif
+
+  const int32_t accel = block->acceleration_steps_per_s2;
+
+          // Steps required for acceleration, deceleration to/from nominal rate
+  uint32_t accelerate_steps = CEIL(estimate_acceleration_distance(initial_rate, block->nominal_rate, accel)),
+           decelerate_steps = FLOOR(estimate_acceleration_distance(block->nominal_rate, final_rate, -accel));
+          // Steps between acceleration and deceleration, if any
+  int32_t plateau_steps = block->step_event_count - accelerate_steps - decelerate_steps;
+
+  // Does accelerate_steps + decelerate_steps exceed step_event_count?
+  // Then we can't possibly reach the nominal rate, there will be no cruising.
+  // Use intersection_distance() to calculate accel / braking time in order to
+  // reach the final_rate exactly at the end of this block.
+  if (plateau_steps < 0) {
+    const float accelerate_steps_float = CEIL(intersection_distance(initial_rate, final_rate, accel, block->step_event_count));
+    accelerate_steps = _MIN(uint32_t(_MAX(accelerate_steps_float, 0)), block->step_event_count);
+    plateau_steps = 0;
+
+    #if ENABLED(S_CURVE_ACCELERATION)
+      // We won't reach the cruising rate. Let's calculate the speed we will reach
+      cruise_rate = final_speed(initial_rate, accel, accelerate_steps);
+    #endif
+  }
+  #if ENABLED(S_CURVE_ACCELERATION)
+    else // We have some plateau time, so the cruise rate will be the nominal rate
+      cruise_rate = block->nominal_rate;
+  #endif
+
+  #if ENABLED(S_CURVE_ACCELERATION)
+    // Jerk controlled speed requires to express speed versus time, NOT steps
+    uint32_t acceleration_time = ((float)(cruise_rate - initial_rate) / accel) * (STEPPER_TIMER_RATE),
+             deceleration_time = ((float)(cruise_rate - final_rate) / accel) * (STEPPER_TIMER_RATE),
+    // And to offload calculations from the ISR, we also calculate the inverse of those times here
+             acceleration_time_inverse = get_period_inverse(acceleration_time),
+             deceleration_time_inverse = get_period_inverse(deceleration_time);
+  #endif
+
+  // Store new block parameters
+  block->accelerate_until = accelerate_steps;
+  block->decelerate_after = accelerate_steps + plateau_steps;
+  block->initial_rate = initial_rate;
+  #if ENABLED(S_CURVE_ACCELERATION)
+    block->acceleration_time = acceleration_time;
+    block->deceleration_time = deceleration_time;
+    block->acceleration_time_inverse = acceleration_time_inverse;
+    block->deceleration_time_inverse = deceleration_time_inverse;
+    block->cruise_rate = cruise_rate;
+  #endif
+  block->final_rate = final_rate;
+
+  /**
+   * Laser trapezoid calculations
+   *
+   * Approximate the trapezoid with the laser, incrementing the power every `entry_per` while accelerating
+   * and decrementing it every `exit_power_per` while decelerating, thus ensuring power is related to feedrate.
+   *
+   * LASER_POWER_INLINE_TRAPEZOID_CONT doesn't need this as it continuously approximates
+   *
+   * Note this may behave unreliably when running with S_CURVE_ACCELERATION
+   */
+  #if ENABLED(LASER_POWER_INLINE_TRAPEZOID)
+    if (block->laser.power > 0) { // No need to care if power == 0
+      const uint8_t entry_power = block->laser.power * entry_factor; // Power on block entry
+      #if DISABLED(LASER_POWER_INLINE_TRAPEZOID_CONT)
+        // Speedup power
+        const uint8_t entry_power_diff = block->laser.power - entry_power;
+        if (entry_power_diff) {
+          block->laser.entry_per = accelerate_steps / entry_power_diff;
+          block->laser.power_entry = entry_power;
+        }
+        else {
+          block->laser.entry_per = 0;
+          block->laser.power_entry = block->laser.power;
+        }
+        // Slowdown power
+        const uint8_t exit_power = block->laser.power * exit_factor, // Power on block entry
+                      exit_power_diff = block->laser.power - exit_power;
+        if (exit_power_diff) {
+          block->laser.exit_per = (block->step_event_count - block->decelerate_after) / exit_power_diff;
+          block->laser.power_exit = exit_power;
+        }
+        else {
+          block->laser.exit_per = 0;
+          block->laser.power_exit = block->laser.power;
+        }
+      #else
+        block->laser.power_entry = entry_power;
+      #endif
+    }
+  #endif
+}
+
+/*                            PLANNER SPEED DEFINITION
+                                     +--------+   <- current->nominal_speed
+                                    /          \
+         current->entry_speed ->   +            \
+                                   |             + <- next->entry_speed (aka exit speed)
+                                   +-------------+
+                                       time -->
+
+  Recalculates the motion plan according to the following basic guidelines:
+
+    1. Go over every feasible block sequentially in reverse order and calculate the junction speeds
+        (i.e. current->entry_speed) such that:
+      a. No junction speed exceeds the pre-computed maximum junction speed limit or nominal speeds of
+         neighboring blocks.
+      b. A block entry speed cannot exceed one reverse-computed from its exit speed (next->entry_speed)
+         with a maximum allowable deceleration over the block travel distance.
+      c. The last (or newest appended) block is planned from a complete stop (an exit speed of zero).
+    2. Go over every block in chronological (forward) order and dial down junction speed values if
+      a. The exit speed exceeds the one forward-computed from its entry speed with the maximum allowable
+         acceleration over the block travel distance.
+
+  When these stages are complete, the planner will have maximized the velocity profiles throughout the all
+  of the planner blocks, where every block is operating at its maximum allowable acceleration limits. In
+  other words, for all of the blocks in the planner, the plan is optimal and no further speed improvements
+  are possible. If a new block is added to the buffer, the plan is recomputed according to the said
+  guidelines for a new optimal plan.
+
+  To increase computational efficiency of these guidelines, a set of planner block pointers have been
+  created to indicate stop-compute points for when the planner guidelines cannot logically make any further
+  changes or improvements to the plan when in normal operation and new blocks are streamed and added to the
+  planner buffer. For example, if a subset of sequential blocks in the planner have been planned and are
+  bracketed by junction velocities at their maximums (or by the first planner block as well), no new block
+  added to the planner buffer will alter the velocity profiles within them. So we no longer have to compute
+  them. Or, if a set of sequential blocks from the first block in the planner (or a optimal stop-compute
+  point) are all accelerating, they are all optimal and can not be altered by a new block added to the
+  planner buffer, as this will only further increase the plan speed to chronological blocks until a maximum
+  junction velocity is reached. However, if the operational conditions of the plan changes from infrequently
+  used feed holds or feedrate overrides, the stop-compute pointers will be reset and the entire plan is
+  recomputed as stated in the general guidelines.
+
+  Planner buffer index mapping:
+  - block_buffer_tail: Points to the beginning of the planner buffer. First to be executed or being executed.
+  - block_buffer_head: Points to the buffer block after the last block in the buffer. Used to indicate whether
+      the buffer is full or empty. As described for standard ring buffers, this block is always empty.
+  - block_buffer_planned: Points to the first buffer block after the last optimally planned block for normal
+      streaming operating conditions. Use for planning optimizations by avoiding recomputing parts of the
+      planner buffer that don't change with the addition of a new block, as describe above. In addition,
+      this block can never be less than block_buffer_tail and will always be pushed forward and maintain
+      this requirement when encountered by the Planner::release_current_block() routine during a cycle.
+
+  NOTE: Since the planner only computes on what's in the planner buffer, some motions with lots of short
+  line segments, like G2/3 arcs or complex curves, may seem to move slow. This is because there simply isn't
+  enough combined distance traveled in the entire buffer to accelerate up to the nominal speed and then
+  decelerate to a complete stop at the end of the buffer, as stated by the guidelines. If this happens and
+  becomes an annoyance, there are a few simple solutions: (1) Maximize the machine acceleration. The planner
+  will be able to compute higher velocity profiles within the same combined distance. (2) Maximize line
+  motion(s) distance per block to a desired tolerance. The more combined distance the planner has to use,
+  the faster it can go. (3) Maximize the planner buffer size. This also will increase the combined distance
+  for the planner to compute over. It also increases the number of computations the planner has to perform
+  to compute an optimal plan, so select carefully.
+*/
+
+// The kernel called by recalculate() when scanning the plan from last to first entry.
+void Planner::reverse_pass_kernel(block_t* const current, const block_t * const next) {
+  if (current) {
+    // If entry speed is already at the maximum entry speed, and there was no change of speed
+    // in the next block, there is no need to recheck. Block is cruising and there is no need to
+    // compute anything for this block,
+    // If not, block entry speed needs to be recalculated to ensure maximum possible planned speed.
+    const float max_entry_speed_sqr = current->max_entry_speed_sqr;
+
+    // Compute maximum entry speed decelerating over the current block from its exit speed.
+    // If not at the maximum entry speed, or the previous block entry speed changed
+    if (current->entry_speed_sqr != max_entry_speed_sqr || (next && TEST(next->flag, BLOCK_BIT_RECALCULATE))) {
+
+      // If nominal length true, max junction speed is guaranteed to be reached.
+      // If a block can de/ac-celerate from nominal speed to zero within the length of the block, then
+      // the current block and next block junction speeds are guaranteed to always be at their maximum
+      // junction speeds in deceleration and acceleration, respectively. This is due to how the current
+      // block nominal speed limits both the current and next maximum junction speeds. Hence, in both
+      // the reverse and forward planners, the corresponding block junction speed will always be at the
+      // the maximum junction speed and may always be ignored for any speed reduction checks.
+
+      const float new_entry_speed_sqr = TEST(current->flag, BLOCK_BIT_NOMINAL_LENGTH)
+        ? max_entry_speed_sqr
+        : _MIN(max_entry_speed_sqr, max_allowable_speed_sqr(-current->acceleration, next ? next->entry_speed_sqr : sq(float(MINIMUM_PLANNER_SPEED)), current->millimeters));
+      if (current->entry_speed_sqr != new_entry_speed_sqr) {
+
+        // Need to recalculate the block speed - Mark it now, so the stepper
+        // ISR does not consume the block before being recalculated
+        SBI(current->flag, BLOCK_BIT_RECALCULATE);
+
+        // But there is an inherent race condition here, as the block may have
+        // become BUSY just before being marked RECALCULATE, so check for that!
+        if (stepper.is_block_busy(current)) {
+          // Block became busy. Clear the RECALCULATE flag (no point in
+          // recalculating BUSY blocks). And don't set its speed, as it can't
+          // be updated at this time.
+          CBI(current->flag, BLOCK_BIT_RECALCULATE);
+        }
+        else {
+          // Block is not BUSY so this is ahead of the Stepper ISR:
+          // Just Set the new entry speed.
+          current->entry_speed_sqr = new_entry_speed_sqr;
+        }
+      }
+    }
+  }
+}
+
+/**
+ * recalculate() needs to go over the current plan twice.
+ * Once in reverse and once forward. This implements the reverse pass.
+ */
+void Planner::reverse_pass() {
+  // Initialize block index to the last block in the planner buffer.
+  uint8_t block_index = prev_block_index(block_buffer_head);
+
+  // Read the index of the last buffer planned block.
+  // The ISR may change it so get a stable local copy.
+  uint8_t planned_block_index = block_buffer_planned;
+
+  // If there was a race condition and block_buffer_planned was incremented
+  //  or was pointing at the head (queue empty) break loop now and avoid
+  //  planning already consumed blocks
+  if (planned_block_index == block_buffer_head) return;
+
+  // Reverse Pass: Coarsely maximize all possible deceleration curves back-planning from the last
+  // block in buffer. Cease planning when the last optimal planned or tail pointer is reached.
+  // NOTE: Forward pass will later refine and correct the reverse pass to create an optimal plan.
+  const block_t *next = nullptr;
+  while (block_index != planned_block_index) {
+
+    // Perform the reverse pass
+    block_t *current = &block_buffer[block_index];
+
+    // Only consider non sync and page blocks
+    if (!TEST(current->flag, BLOCK_BIT_SYNC_POSITION) && !IS_PAGE(current)) {
+      reverse_pass_kernel(current, next);
+      next = current;
+    }
+
+    // Advance to the next
+    block_index = prev_block_index(block_index);
+
+    // The ISR could advance the block_buffer_planned while we were doing the reverse pass.
+    // We must try to avoid using an already consumed block as the last one - So follow
+    // changes to the pointer and make sure to limit the loop to the currently busy block
+    while (planned_block_index != block_buffer_planned) {
+
+      // If we reached the busy block or an already processed block, break the loop now
+      if (block_index == planned_block_index) return;
+
+      // Advance the pointer, following the busy block
+      planned_block_index = next_block_index(planned_block_index);
+    }
+  }
+}
+
+// The kernel called by recalculate() when scanning the plan from first to last entry.
+void Planner::forward_pass_kernel(const block_t* const previous, block_t* const current, const uint8_t block_index) {
+  if (previous) {
+    // If the previous block is an acceleration block, too short to complete the full speed
+    // change, adjust the entry speed accordingly. Entry speeds have already been reset,
+    // maximized, and reverse-planned. If nominal length is set, max junction speed is
+    // guaranteed to be reached. No need to recheck.
+    if (!TEST(previous->flag, BLOCK_BIT_NOMINAL_LENGTH) &&
+      previous->entry_speed_sqr < current->entry_speed_sqr) {
+
+      // Compute the maximum allowable speed
+      const float new_entry_speed_sqr = max_allowable_speed_sqr(-previous->acceleration, previous->entry_speed_sqr, previous->millimeters);
+
+      // If true, current block is full-acceleration and we can move the planned pointer forward.
+      if (new_entry_speed_sqr < current->entry_speed_sqr) {
+
+        // Mark we need to recompute the trapezoidal shape, and do it now,
+        // so the stepper ISR does not consume the block before being recalculated
+        SBI(current->flag, BLOCK_BIT_RECALCULATE);
+
+        // But there is an inherent race condition here, as the block maybe
+        // became BUSY, just before it was marked as RECALCULATE, so check
+        // if that is the case!
+        if (stepper.is_block_busy(current)) {
+          // Block became busy. Clear the RECALCULATE flag (no point in
+          //  recalculating BUSY blocks and don't set its speed, as it can't
+          //  be updated at this time.
+          CBI(current->flag, BLOCK_BIT_RECALCULATE);
+        }
+        else {
+          // Block is not BUSY, we won the race against the Stepper ISR:
+
+          // Always <= max_entry_speed_sqr. Backward pass sets this.
+          current->entry_speed_sqr = new_entry_speed_sqr; // Always <= max_entry_speed_sqr. Backward pass sets this.
+
+          // Set optimal plan pointer.
+          block_buffer_planned = block_index;
+        }
+      }
+    }
+
+    // Any block set at its maximum entry speed also creates an optimal plan up to this
+    // point in the buffer. When the plan is bracketed by either the beginning of the
+    // buffer and a maximum entry speed or two maximum entry speeds, every block in between
+    // cannot logically be further improved. Hence, we don't have to recompute them anymore.
+    if (current->entry_speed_sqr == current->max_entry_speed_sqr)
+      block_buffer_planned = block_index;
+  }
+}
+
+/**
+ * recalculate() needs to go over the current plan twice.
+ * Once in reverse and once forward. This implements the forward pass.
+ */
+void Planner::forward_pass() {
+
+  // Forward Pass: Forward plan the acceleration curve from the planned pointer onward.
+  // Also scans for optimal plan breakpoints and appropriately updates the planned pointer.
+
+  // Begin at buffer planned pointer. Note that block_buffer_planned can be modified
+  //  by the stepper ISR,  so read it ONCE. It it guaranteed that block_buffer_planned
+  //  will never lead head, so the loop is safe to execute. Also note that the forward
+  //  pass will never modify the values at the tail.
+  uint8_t block_index = block_buffer_planned;
+
+  block_t *block;
+  const block_t * previous = nullptr;
+  while (block_index != block_buffer_head) {
+
+    // Perform the forward pass
+    block = &block_buffer[block_index];
+
+    // Skip SYNC and page blocks
+    if (!TEST(block->flag, BLOCK_BIT_SYNC_POSITION) && !IS_PAGE(block)) {
+      // If there's no previous block or the previous block is not
+      // BUSY (thus, modifiable) run the forward_pass_kernel. Otherwise,
+      // the previous block became BUSY, so assume the current block's
+      // entry speed can't be altered (since that would also require
+      // updating the exit speed of the previous block).
+      if (!previous || !stepper.is_block_busy(previous))
+        forward_pass_kernel(previous, block, block_index);
+      previous = block;
+    }
+    // Advance to the previous
+    block_index = next_block_index(block_index);
+  }
+}
+
+/**
+ * Recalculate the trapezoid speed profiles for all blocks in the plan
+ * according to the entry_factor for each junction. Must be called by
+ * recalculate() after updating the blocks.
+ */
+void Planner::recalculate_trapezoids() {
+  // The tail may be changed by the ISR so get a local copy.
+  uint8_t block_index = block_buffer_tail,
+          head_block_index = block_buffer_head;
+  // Since there could be a sync block in the head of the queue, and the
+  // next loop must not recalculate the head block (as it needs to be
+  // specially handled), scan backwards to the first non-SYNC block.
+  while (head_block_index != block_index) {
+
+    // Go back (head always point to the first free block)
+    const uint8_t prev_index = prev_block_index(head_block_index);
+
+    // Get the pointer to the block
+    block_t *prev = &block_buffer[prev_index];
+
+    // If not dealing with a sync block, we are done. The last block is not a SYNC block
+    if (!TEST(prev->flag, BLOCK_BIT_SYNC_POSITION)) break;
+
+    // Examine the previous block. This and all following are SYNC blocks
+    head_block_index = prev_index;
+  }
+
+  // Go from the tail (currently executed block) to the first block, without including it)
+  block_t *block = nullptr, *next = nullptr;
+  float current_entry_speed = 0.0, next_entry_speed = 0.0;
+  while (block_index != head_block_index) {
+
+    next = &block_buffer[block_index];
+
+    // Skip sync and page blocks
+    if (!TEST(next->flag, BLOCK_BIT_SYNC_POSITION) && !IS_PAGE(next)) {
+      next_entry_speed = SQRT(next->entry_speed_sqr);
+
+      if (block) {
+        // Recalculate if current block entry or exit junction speed has changed.
+        if (TEST(block->flag, BLOCK_BIT_RECALCULATE) || TEST(next->flag, BLOCK_BIT_RECALCULATE)) {
+
+          // Mark the current block as RECALCULATE, to protect it from the Stepper ISR running it.
+          // Note that due to the above condition, there's a chance the current block isn't marked as
+          // RECALCULATE yet, but the next one is. That's the reason for the following line.
+          SBI(block->flag, BLOCK_BIT_RECALCULATE);
+
+          // But there is an inherent race condition here, as the block maybe
+          // became BUSY, just before it was marked as RECALCULATE, so check
+          // if that is the case!
+          if (!stepper.is_block_busy(block)) {
+            // Block is not BUSY, we won the race against the Stepper ISR:
+
+            // NOTE: Entry and exit factors always > 0 by all previous logic operations.
+            const float current_nominal_speed = SQRT(block->nominal_speed_sqr),
+                        nomr = 1.0f / current_nominal_speed;
+            calculate_trapezoid_for_block(block, current_entry_speed * nomr, next_entry_speed * nomr);
+            #if ENABLED(LIN_ADVANCE)
+              if (block->use_advance_lead) {
+                const float comp = block->e_D_ratio * extruder_advance_K[active_extruder] * settings.axis_steps_per_mm[E_AXIS];
+                block->max_adv_steps = current_nominal_speed * comp;
+                block->final_adv_steps = next_entry_speed * comp;
+              }
+            #endif
+          }
+
+          // Reset current only to ensure next trapezoid is computed - The
+          // stepper is free to use the block from now on.
+          CBI(block->flag, BLOCK_BIT_RECALCULATE);
+        }
+      }
+
+      block = next;
+      current_entry_speed = next_entry_speed;
+    }
+
+    block_index = next_block_index(block_index);
+  }
+
+  // Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated.
+  if (next) {
+
+    // Mark the next(last) block as RECALCULATE, to prevent the Stepper ISR running it.
+    // As the last block is always recalculated here, there is a chance the block isn't
+    // marked as RECALCULATE yet. That's the reason for the following line.
+    SBI(next->flag, BLOCK_BIT_RECALCULATE);
+
+    // But there is an inherent race condition here, as the block maybe
+    // became BUSY, just before it was marked as RECALCULATE, so check
+    // if that is the case!
+    if (!stepper.is_block_busy(block)) {
+      // Block is not BUSY, we won the race against the Stepper ISR:
+
+      const float next_nominal_speed = SQRT(next->nominal_speed_sqr),
+                  nomr = 1.0f / next_nominal_speed;
+      calculate_trapezoid_for_block(next, next_entry_speed * nomr, float(MINIMUM_PLANNER_SPEED) * nomr);
+      #if ENABLED(LIN_ADVANCE)
+        if (next->use_advance_lead) {
+          const float comp = next->e_D_ratio * extruder_advance_K[active_extruder] * settings.axis_steps_per_mm[E_AXIS];
+          next->max_adv_steps = next_nominal_speed * comp;
+          next->final_adv_steps = (MINIMUM_PLANNER_SPEED) * comp;
+        }
+      #endif
+    }
+
+    // Reset next only to ensure its trapezoid is computed - The stepper is free to use
+    // the block from now on.
+    CBI(next->flag, BLOCK_BIT_RECALCULATE);
+  }
+}
+
+void Planner::recalculate() {
+  // Initialize block index to the last block in the planner buffer.
+  const uint8_t block_index = prev_block_index(block_buffer_head);
+  // If there is just one block, no planning can be done. Avoid it!
+  if (block_index != block_buffer_planned) {
+    reverse_pass();
+    forward_pass();
+  }
+  recalculate_trapezoids();
+}
+
+#if ENABLED(AUTOTEMP)
+
+  void Planner::getHighESpeed() {
+    static float oldt = 0;
+
+    if (!autotemp_enabled) return;
+    if (thermalManager.degTargetHotend(0) + 2 < autotemp_min) return; // probably temperature set to zero.
+
+    float high = 0.0;
+    for (uint8_t b = block_buffer_tail; b != block_buffer_head; b = next_block_index(b)) {
+      block_t* block = &block_buffer[b];
+      if (block->steps.x || block->steps.y || block->steps.z) {
+        const float se = (float)block->steps.e / block->step_event_count * SQRT(block->nominal_speed_sqr); // mm/sec;
+        NOLESS(high, se);
+      }
+    }
+
+    float t = autotemp_min + high * autotemp_factor;
+    LIMIT(t, autotemp_min, autotemp_max);
+    if (t < oldt) t = t * (1 - float(AUTOTEMP_OLDWEIGHT)) + oldt * float(AUTOTEMP_OLDWEIGHT);
+    oldt = t;
+    thermalManager.setTargetHotend(t, 0);
+  }
+
+#endif // AUTOTEMP
+
+/**
+ * Maintain fans, paste extruder pressure,
+ */
+void Planner::check_axes_activity() {
+
+  #if ANY(DISABLE_X, DISABLE_Y, DISABLE_Z, DISABLE_E)
+    xyze_bool_t axis_active = { false };
+  #endif
+
+  #if HAS_FAN
+    uint8_t tail_fan_speed[FAN_COUNT];
+  #endif
+
+  #if ENABLED(BARICUDA)
+    #if HAS_HEATER_1
+      uint8_t tail_valve_pressure;
+    #endif
+    #if HAS_HEATER_2
+      uint8_t tail_e_to_p_pressure;
+    #endif
+  #endif
+
+  if (has_blocks_queued()) {
+
+    #if HAS_FAN || ENABLED(BARICUDA)
+      block_t *block = &block_buffer[block_buffer_tail];
+    #endif
+
+    #if HAS_FAN
+      FANS_LOOP(i)
+        tail_fan_speed[i] = thermalManager.scaledFanSpeed(i, block->fan_speed[i]);
+    #endif
+
+    #if ENABLED(BARICUDA)
+      TERN_(HAS_HEATER_1, tail_valve_pressure = block->valve_pressure);
+      TERN_(HAS_HEATER_2, tail_e_to_p_pressure = block->e_to_p_pressure);
+    #endif
+
+    #if ANY(DISABLE_X, DISABLE_Y, DISABLE_Z, DISABLE_E)
+      for (uint8_t b = block_buffer_tail; b != block_buffer_head; b = next_block_index(b)) {
+        block_t *block = &block_buffer[b];
+        if (ENABLED(DISABLE_X) && block->steps.x) axis_active.x = true;
+        if (ENABLED(DISABLE_Y) && block->steps.y) axis_active.y = true;
+        if (ENABLED(DISABLE_Z) && block->steps.z) axis_active.z = true;
+        if (ENABLED(DISABLE_E) && block->steps.e) axis_active.e = true;
+      }
+    #endif
+  }
+  else {
+
+    TERN_(HAS_CUTTER, cutter.refresh());
+
+    #if HAS_FAN
+      FANS_LOOP(i)
+        tail_fan_speed[i] = thermalManager.scaledFanSpeed(i);
+    #endif
+
+    #if ENABLED(BARICUDA)
+      TERN_(HAS_HEATER_1, tail_valve_pressure = baricuda_valve_pressure);
+      TERN_(HAS_HEATER_2, tail_e_to_p_pressure = baricuda_e_to_p_pressure);
+    #endif
+  }
+
+  //
+  // Disable inactive axes
+  //
+  if (TERN0(DISABLE_X, !axis_active.x)) DISABLE_AXIS_X();
+  if (TERN0(DISABLE_Y, !axis_active.y)) DISABLE_AXIS_Y();
+  if (TERN0(DISABLE_Z, !axis_active.z)) DISABLE_AXIS_Z();
+  if (TERN0(DISABLE_E, !axis_active.e)) disable_e_steppers();
+
+  //
+  // Update Fan speeds
+  //
+  #if HAS_FAN
+
+    #if FAN_KICKSTART_TIME > 0
+      static millis_t fan_kick_end[FAN_COUNT] = { 0 };
+      #define KICKSTART_FAN(f)                         \
+        if (tail_fan_speed[f]) {                       \
+          millis_t ms = millis();                      \
+          if (fan_kick_end[f] == 0) {                  \
+            fan_kick_end[f] = ms + FAN_KICKSTART_TIME; \
+            tail_fan_speed[f] = 255;                   \
+          } else if (PENDING(ms, fan_kick_end[f]))     \
+            tail_fan_speed[f] = 255;                   \
+        } else fan_kick_end[f] = 0
+    #else
+      #define KICKSTART_FAN(f) NOOP
+    #endif
+
+    #if FAN_MIN_PWM != 0 || FAN_MAX_PWM != 255
+      #define CALC_FAN_SPEED(f) (tail_fan_speed[f] ? map(tail_fan_speed[f], 1, 255, FAN_MIN_PWM, FAN_MAX_PWM) : FAN_OFF_PWM)
+    #else
+      #define CALC_FAN_SPEED(f) (tail_fan_speed[f] ?: FAN_OFF_PWM)
+    #endif
+
+    #if ENABLED(FAN_SOFT_PWM)
+      #define _FAN_SET(F) thermalManager.soft_pwm_amount_fan[F] = CALC_FAN_SPEED(F);
+    #elif ENABLED(FAST_PWM_FAN)
+      #define _FAN_SET(F) set_pwm_duty(FAN##F##_PIN, CALC_FAN_SPEED(F));
+    #else
+      #define _FAN_SET(F) analogWrite(pin_t(FAN##F##_PIN), CALC_FAN_SPEED(F));
+    #endif
+    #define FAN_SET(F) do{ KICKSTART_FAN(F); _FAN_SET(F); }while(0)
+
+    TERN_(HAS_FAN0, FAN_SET(0));
+    TERN_(HAS_FAN1, FAN_SET(1));
+    TERN_(HAS_FAN2, FAN_SET(2));
+    TERN_(HAS_FAN3, FAN_SET(3));
+    TERN_(HAS_FAN4, FAN_SET(4));
+    TERN_(HAS_FAN5, FAN_SET(5));
+    TERN_(HAS_FAN6, FAN_SET(6));
+    TERN_(HAS_FAN7, FAN_SET(7));
+  #endif // HAS_FAN
+
+  TERN_(AUTOTEMP, getHighESpeed());
+
+  #if ENABLED(BARICUDA)
+    TERN_(HAS_HEATER_1, analogWrite(pin_t(HEATER_1_PIN), tail_valve_pressure));
+    TERN_(HAS_HEATER_2, analogWrite(pin_t(HEATER_2_PIN), tail_e_to_p_pressure));
+  #endif
+}
+
+#if DISABLED(NO_VOLUMETRICS)
+
+  /**
+   * Get a volumetric multiplier from a filament diameter.
+   * This is the reciprocal of the circular cross-section area.
+   * Return 1.0 with volumetric off or a diameter of 0.0.
+   */
+  inline float calculate_volumetric_multiplier(const float &diameter) {
+    return (parser.volumetric_enabled && diameter) ? 1.0f / CIRCLE_AREA(diameter * 0.5f) : 1;
+  }
+
+  /**
+   * Convert the filament sizes into volumetric multipliers.
+   * The multiplier converts a given E value into a length.
+   */
+  void Planner::calculate_volumetric_multipliers() {
+    LOOP_L_N(i, COUNT(filament_size)) {
+      volumetric_multiplier[i] = calculate_volumetric_multiplier(filament_size[i]);
+      refresh_e_factor(i);
+    }
+    #if ENABLED(VOLUMETRIC_EXTRUDER_LIMIT)
+      calculate_volumetric_extruder_limits(); // update volumetric_extruder_limits as well.
+    #endif
+  }
+
+#endif // !NO_VOLUMETRICS
+
+#if ENABLED(VOLUMETRIC_EXTRUDER_LIMIT)
+
+  /**
+   * Convert volumetric based limits into pre calculated extruder feedrate limits.
+   */
+  void Planner::calculate_volumetric_extruder_limit(const uint8_t e) {
+    const float &lim = volumetric_extruder_limit[e], &siz = filament_size[e];
+    volumetric_extruder_feedrate_limit[e] = (lim && siz) ? lim / CIRCLE_AREA(siz * 0.5f) : 0;
+  }
+  void Planner::calculate_volumetric_extruder_limits() {
+    LOOP_L_N(e, EXTRUDERS) calculate_volumetric_extruder_limit(e);
+  }
+
+#endif
+
+#if ENABLED(FILAMENT_WIDTH_SENSOR)
+  /**
+   * Convert the ratio value given by the filament width sensor
+   * into a volumetric multiplier. Conversion differs when using
+   * linear extrusion vs volumetric extrusion.
+   */
+  void Planner::apply_filament_width_sensor(const int8_t encoded_ratio) {
+    // Reconstitute the nominal/measured ratio
+    const float nom_meas_ratio = 1 + 0.01f * encoded_ratio,
+                ratio_2 = sq(nom_meas_ratio);
+
+    volumetric_multiplier[FILAMENT_SENSOR_EXTRUDER_NUM] = parser.volumetric_enabled
+      ? ratio_2 / CIRCLE_AREA(filwidth.nominal_mm * 0.5f) // Volumetric uses a true volumetric multiplier
+      : ratio_2;                                          // Linear squares the ratio, which scales the volume
+
+    refresh_e_factor(FILAMENT_SENSOR_EXTRUDER_NUM);
+  }
+#endif
+
+#if HAS_LEVELING
+
+  constexpr xy_pos_t level_fulcrum = {
+    TERN(Z_SAFE_HOMING, Z_SAFE_HOMING_X_POINT, X_HOME_POS),
+    TERN(Z_SAFE_HOMING, Z_SAFE_HOMING_Y_POINT, Y_HOME_POS)
+  };
+
+  /**
+   * rx, ry, rz - Cartesian positions in mm
+   *              Leveled XYZ on completion
+   */
+  void Planner::apply_leveling(xyz_pos_t &raw) {
+    if (!leveling_active) return;
+
+    #if ABL_PLANAR
+
+      xy_pos_t d = raw - level_fulcrum;
+      apply_rotation_xyz(bed_level_matrix, d.x, d.y, raw.z);
+      raw = d + level_fulcrum;
+
+    #elif HAS_MESH
+
+      #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
+        const float fade_scaling_factor = fade_scaling_factor_for_z(raw.z);
+      #elif DISABLED(MESH_BED_LEVELING)
+        constexpr float fade_scaling_factor = 1.0;
+      #endif
+
+      raw.z += (
+        #if ENABLED(MESH_BED_LEVELING)
+          mbl.get_z(raw
+            #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
+              , fade_scaling_factor
+            #endif
+          )
+        #elif ENABLED(AUTO_BED_LEVELING_UBL)
+          fade_scaling_factor ? fade_scaling_factor * ubl.get_z_correction(raw) : 0.0
+        #elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
+          fade_scaling_factor ? fade_scaling_factor * bilinear_z_offset(raw) : 0.0
+        #endif
+      );
+
+    #endif
+  }
+
+  void Planner::unapply_leveling(xyz_pos_t &raw) {
+
+    if (leveling_active) {
+
+      #if ABL_PLANAR
+
+        matrix_3x3 inverse = matrix_3x3::transpose(bed_level_matrix);
+
+        xy_pos_t d = raw - level_fulcrum;
+        apply_rotation_xyz(inverse, d.x, d.y, raw.z);
+        raw = d + level_fulcrum;
+
+      #elif HAS_MESH
+
+        #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
+          const float fade_scaling_factor = fade_scaling_factor_for_z(raw.z);
+        #elif DISABLED(MESH_BED_LEVELING)
+          constexpr float fade_scaling_factor = 1.0;
+        #endif
+
+        raw.z -= (
+          #if ENABLED(MESH_BED_LEVELING)
+            mbl.get_z(raw
+              #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
+                , fade_scaling_factor
+              #endif
+            )
+          #elif ENABLED(AUTO_BED_LEVELING_UBL)
+            fade_scaling_factor ? fade_scaling_factor * ubl.get_z_correction(raw) : 0.0
+          #elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
+            fade_scaling_factor ? fade_scaling_factor * bilinear_z_offset(raw) : 0.0
+          #endif
+        );
+
+      #endif
+    }
+  }
+
+#endif // HAS_LEVELING
+
+#if ENABLED(FWRETRACT)
+  /**
+   * rz, e - Cartesian positions in mm
+   */
+  void Planner::apply_retract(float &rz, float &e) {
+    rz += fwretract.current_hop;
+    e -= fwretract.current_retract[active_extruder];
+  }
+
+  void Planner::unapply_retract(float &rz, float &e) {
+    rz -= fwretract.current_hop;
+    e += fwretract.current_retract[active_extruder];
+  }
+
+#endif
+
+void Planner::quick_stop() {
+
+  // Remove all the queued blocks. Note that this function is NOT
+  // called from the Stepper ISR, so we must consider tail as readonly!
+  // that is why we set head to tail - But there is a race condition that
+  // must be handled: The tail could change between the read and the assignment
+  // so this must be enclosed in a critical section
+
+  const bool was_enabled = stepper.suspend();
+
+  // Drop all queue entries
+  block_buffer_nonbusy = block_buffer_planned = block_buffer_head = block_buffer_tail;
+
+  // Restart the block delay for the first movement - As the queue was
+  // forced to empty, there's no risk the ISR will touch this.
+  delay_before_delivering = BLOCK_DELAY_FOR_1ST_MOVE;
+
+  #if HAS_WIRED_LCD
+    // Clear the accumulated runtime
+    clear_block_buffer_runtime();
+  #endif
+
+  // Make sure to drop any attempt of queuing moves for 1 second
+  cleaning_buffer_counter = TEMP_TIMER_FREQUENCY;
+
+  // Reenable Stepper ISR
+  if (was_enabled) stepper.wake_up();
+
+  // And stop the stepper ISR
+  stepper.quick_stop();
+}
+
+void Planner::endstop_triggered(const AxisEnum axis) {
+  // Record stepper position and discard the current block
+  stepper.endstop_triggered(axis);
+}
+
+float Planner::triggered_position_mm(const AxisEnum axis) {
+  return stepper.triggered_position(axis) * steps_to_mm[axis];
+}
+
+void Planner::finish_and_disable() {
+  while (has_blocks_queued() || cleaning_buffer_counter) idle();
+  disable_all_steppers();
+}
+
+/**
+ * Get an axis position according to stepper position(s)
+ * For CORE machines apply translation from ABC to XYZ.
+ */
+float Planner::get_axis_position_mm(const AxisEnum axis) {
+  float axis_steps;
+  #if IS_CORE
+
+    // Requesting one of the "core" axes?
+    if (axis == CORE_AXIS_1 || axis == CORE_AXIS_2) {
+
+      // Protect the access to the position.
+      const bool was_enabled = stepper.suspend();
+
+      const int32_t p1 = stepper.position(CORE_AXIS_1),
+                    p2 = stepper.position(CORE_AXIS_2);
+
+      if (was_enabled) stepper.wake_up();
+
+      // ((a1+a2)+(a1-a2))/2 -> (a1+a2+a1-a2)/2 -> (a1+a1)/2 -> a1
+      // ((a1+a2)-(a1-a2))/2 -> (a1+a2-a1+a2)/2 -> (a2+a2)/2 -> a2
+      axis_steps = (axis == CORE_AXIS_2 ? CORESIGN(p1 - p2) : p1 + p2) * 0.5f;
+    }
+    else
+      axis_steps = stepper.position(axis);
+
+  #elif ENABLED(MARKFORGED_XY)
+
+    // Requesting one of the joined axes?
+    if (axis == CORE_AXIS_1 || axis == CORE_AXIS_2) {
+      // Protect the access to the position.
+      const bool was_enabled = stepper.suspend();
+
+      const int32_t p1 = stepper.position(CORE_AXIS_1),
+                    p2 = stepper.position(CORE_AXIS_2);
+
+      if (was_enabled) stepper.wake_up();
+
+      axis_steps = ((axis == CORE_AXIS_1) ? p1 - p2 : p2);
+    }
+    else
+      axis_steps = stepper.position(axis);
+
+  #else
+
+    axis_steps = stepper.position(axis);
+
+  #endif
+
+  return axis_steps * steps_to_mm[axis];
+}
+
+/**
+ * Block until all buffered steps are executed / cleaned
+ */
+void Planner::synchronize() {
+  while (has_blocks_queued() || cleaning_buffer_counter
+      || TERN0(EXTERNAL_CLOSED_LOOP_CONTROLLER, CLOSED_LOOP_WAITING())
+  ) idle();
+}
+
+/**
+ * Planner::_buffer_steps
+ *
+ * Add a new linear movement to the planner queue (in terms of steps).
+ *
+ *  target        - target position in steps units
+ *  target_float  - target position in direct (mm, degrees) units. optional
+ *  fr_mm_s       - (target) speed of the move
+ *  extruder      - target extruder
+ *  millimeters   - the length of the movement, if known
+ *
+ * Returns true if movement was properly queued, false otherwise (if cleaning)
+ */
+bool Planner::_buffer_steps(const xyze_long_t &target
+  #if HAS_POSITION_FLOAT
+    , const xyze_pos_t &target_float
+  #endif
+  #if HAS_DIST_MM_ARG
+    , const xyze_float_t &cart_dist_mm
+  #endif
+  , feedRate_t fr_mm_s, const uint8_t extruder, const float &millimeters
+) {
+
+  // If we are cleaning, do not accept queuing of movements
+  if (cleaning_buffer_counter) return false;
+
+  // Wait for the next available block
+  uint8_t next_buffer_head;
+  block_t * const block = get_next_free_block(next_buffer_head);
+
+  // Fill the block with the specified movement
+  if (!_populate_block(block, false, target
+    #if HAS_POSITION_FLOAT
+      , target_float
+    #endif
+    #if HAS_DIST_MM_ARG
+      , cart_dist_mm
+    #endif
+    , fr_mm_s, extruder, millimeters
+  )) {
+    // Movement was not queued, probably because it was too short.
+    //  Simply accept that as movement queued and done
+    return true;
+  }
+
+  // If this is the first added movement, reload the delay, otherwise, cancel it.
+  if (block_buffer_head == block_buffer_tail) {
+    // If it was the first queued block, restart the 1st block delivery delay, to
+    // give the planner an opportunity to queue more movements and plan them
+    // As there are no queued movements, the Stepper ISR will not touch this
+    // variable, so there is no risk setting this here (but it MUST be done
+    // before the following line!!)
+    delay_before_delivering = BLOCK_DELAY_FOR_1ST_MOVE;
+  }
+
+  // Move buffer head
+  block_buffer_head = next_buffer_head;
+
+  // Recalculate and optimize trapezoidal speed profiles
+  recalculate();
+
+  // Movement successfully queued!
+  return true;
+}
+
+/**
+ * Planner::_populate_block
+ *
+ * Fills a new linear movement in the block (in terms of steps).
+ *
+ *  target      - target position in steps units
+ *  fr_mm_s     - (target) speed of the move
+ *  extruder    - target extruder
+ *
+ * Returns true if movement is acceptable, false otherwise
+ */
+bool Planner::_populate_block(block_t * const block, bool split_move,
+  const abce_long_t &target
+  #if HAS_POSITION_FLOAT
+    , const xyze_pos_t &target_float
+  #endif
+  #if HAS_DIST_MM_ARG
+    , const xyze_float_t &cart_dist_mm
+  #endif
+  , feedRate_t fr_mm_s, const uint8_t extruder, const float &millimeters/*=0.0*/
+) {
+
+  const int32_t da = target.a - position.a,
+                db = target.b - position.b,
+                dc = target.c - position.c;
+
+  #if EXTRUDERS
+    int32_t de = target.e - position.e;
+  #else
+    constexpr int32_t de = 0;
+  #endif
+
+  /* <-- add a slash to enable
+    SERIAL_ECHOLNPAIR(
+      "  _populate_block FR:", fr_mm_s,
+      " A:", target.a, " (", da, " steps)"
+      " B:", target.b, " (", db, " steps)"
+      " C:", target.c, " (", dc, " steps)"
+      #if EXTRUDERS
+        " E:", target.e, " (", de, " steps)"
+      #endif
+    );
+  //*/
+
+  #if EITHER(PREVENT_COLD_EXTRUSION, PREVENT_LENGTHY_EXTRUDE)
+    if (de) {
+      #if ENABLED(PREVENT_COLD_EXTRUSION)
+        if (thermalManager.tooColdToExtrude(extruder)) {
+          position.e = target.e; // Behave as if the move really took place, but ignore E part
+          TERN_(HAS_POSITION_FLOAT, position_float.e = target_float.e);
+          de = 0; // no difference
+          SERIAL_ECHO_MSG(STR_ERR_COLD_EXTRUDE_STOP);
+        }
+      #endif // PREVENT_COLD_EXTRUSION
+      #if ENABLED(PREVENT_LENGTHY_EXTRUDE)
+        const float e_steps = ABS(de * e_factor[extruder]);
+        const float max_e_steps = settings.axis_steps_per_mm[E_AXIS_N(extruder)] * (EXTRUDE_MAXLENGTH);
+        if (e_steps > max_e_steps) {
+          #if ENABLED(MIXING_EXTRUDER)
+            bool ignore_e = false;
+            float collector[MIXING_STEPPERS];
+            mixer.refresh_collector(1.0, mixer.get_current_vtool(), collector);
+            MIXER_STEPPER_LOOP(e)
+              if (e_steps * collector[e] > max_e_steps) { ignore_e = true; break; }
+          #else
+            constexpr bool ignore_e = true;
+          #endif
+          if (ignore_e) {
+            position.e = target.e; // Behave as if the move really took place, but ignore E part
+            TERN_(HAS_POSITION_FLOAT, position_float.e = target_float.e);
+            de = 0; // no difference
+            SERIAL_ECHO_MSG(STR_ERR_LONG_EXTRUDE_STOP);
+          }
+        }
+      #endif // PREVENT_LENGTHY_EXTRUDE
+    }
+  #endif // PREVENT_COLD_EXTRUSION || PREVENT_LENGTHY_EXTRUDE
+
+  // Compute direction bit-mask for this block
+  uint8_t dm = 0;
+  #if CORE_IS_XY
+    if (da < 0) SBI(dm, X_HEAD);                // Save the real Extruder (head) direction in X Axis
+    if (db < 0) SBI(dm, Y_HEAD);                // ...and Y
+    if (dc < 0) SBI(dm, Z_AXIS);
+    if (da + db < 0) SBI(dm, A_AXIS);           // Motor A direction
+    if (CORESIGN(da - db) < 0) SBI(dm, B_AXIS); // Motor B direction
+  #elif CORE_IS_XZ
+    if (da < 0) SBI(dm, X_HEAD);                // Save the real Extruder (head) direction in X Axis
+    if (db < 0) SBI(dm, Y_AXIS);
+    if (dc < 0) SBI(dm, Z_HEAD);                // ...and Z
+    if (da + dc < 0) SBI(dm, A_AXIS);           // Motor A direction
+    if (CORESIGN(da - dc) < 0) SBI(dm, C_AXIS); // Motor C direction
+  #elif CORE_IS_YZ
+    if (da < 0) SBI(dm, X_AXIS);
+    if (db < 0) SBI(dm, Y_HEAD);                // Save the real Extruder (head) direction in Y Axis
+    if (dc < 0) SBI(dm, Z_HEAD);                // ...and Z
+    if (db + dc < 0) SBI(dm, B_AXIS);           // Motor B direction
+    if (CORESIGN(db - dc) < 0) SBI(dm, C_AXIS); // Motor C direction
+  #elif ENABLED(MARKFORGED_XY)
+    if (da < 0) SBI(dm, X_HEAD);                // Save the real Extruder (head) direction in X Axis
+    if (db < 0) SBI(dm, Y_HEAD);                // ...and Y
+    if (dc < 0) SBI(dm, Z_AXIS);
+    if (da + db < 0) SBI(dm, A_AXIS);           // Motor A direction
+    if (db < 0) SBI(dm, B_AXIS);                // Motor B direction
+  #else
+    if (da < 0) SBI(dm, X_AXIS);
+    if (db < 0) SBI(dm, Y_AXIS);
+    if (dc < 0) SBI(dm, Z_AXIS);
+  #endif
+  if (de < 0) SBI(dm, E_AXIS);
+
+  #if EXTRUDERS
+    const float esteps_float = de * e_factor[extruder];
+    const uint32_t esteps = ABS(esteps_float) + 0.5f;
+  #else
+    constexpr uint32_t esteps = 0;
+  #endif
+
+  // Clear all flags, including the "busy" bit
+  block->flag = 0x00;
+
+  // Set direction bits
+  block->direction_bits = dm;
+
+  // Update block laser power
+  #if ENABLED(LASER_POWER_INLINE)
+    laser_inline.status.isPlanned = true;
+    block->laser.status = laser_inline.status;
+    block->laser.power = laser_inline.power;
+  #endif
+
+  // Number of steps for each axis
+  // See https://www.corexy.com/theory.html
+  #if CORE_IS_XY
+    block->steps.set(ABS(da + db), ABS(da - db), ABS(dc));
+  #elif CORE_IS_XZ
+    block->steps.set(ABS(da + dc), ABS(db), ABS(da - dc));
+  #elif CORE_IS_YZ
+    block->steps.set(ABS(da), ABS(db + dc), ABS(db - dc));
+  #elif ENABLED(MARKFORGED_XY)
+    block->steps.set(ABS(da + db), ABS(db), ABS(dc));
+  #elif IS_SCARA
+    block->steps.set(ABS(da), ABS(db), ABS(dc));
+  #else
+    // default non-h-bot planning
+    block->steps.set(ABS(da), ABS(db), ABS(dc));
+  #endif
+
+  /**
+   * This part of the code calculates the total length of the movement.
+   * For cartesian bots, the X_AXIS is the real X movement and same for Y_AXIS.
+   * But for corexy bots, that is not true. The "X_AXIS" and "Y_AXIS" motors (that should be named to A_AXIS
+   * and B_AXIS) cannot be used for X and Y length, because A=X+Y and B=X-Y.
+   * So we need to create other 2 "AXIS", named X_HEAD and Y_HEAD, meaning the real displacement of the Head.
+   * Having the real displacement of the head, we can calculate the total movement length and apply the desired speed.
+   */
+  struct DistanceMM : abce_float_t {
+    #if EITHER(IS_CORE, MARKFORGED_XY)
+      xyz_pos_t head;
+    #endif
+  } steps_dist_mm;
+  #if IS_CORE
+    #if CORE_IS_XY
+      steps_dist_mm.head.x = da * steps_to_mm[A_AXIS];
+      steps_dist_mm.head.y = db * steps_to_mm[B_AXIS];
+      steps_dist_mm.z      = dc * steps_to_mm[Z_AXIS];
+      steps_dist_mm.a      = (da + db) * steps_to_mm[A_AXIS];
+      steps_dist_mm.b      = CORESIGN(da - db) * steps_to_mm[B_AXIS];
+    #elif CORE_IS_XZ
+      steps_dist_mm.head.x = da * steps_to_mm[A_AXIS];
+      steps_dist_mm.y      = db * steps_to_mm[Y_AXIS];
+      steps_dist_mm.head.z = dc * steps_to_mm[C_AXIS];
+      steps_dist_mm.a      = (da + dc) * steps_to_mm[A_AXIS];
+      steps_dist_mm.c      = CORESIGN(da - dc) * steps_to_mm[C_AXIS];
+    #elif CORE_IS_YZ
+      steps_dist_mm.x      = da * steps_to_mm[X_AXIS];
+      steps_dist_mm.head.y = db * steps_to_mm[B_AXIS];
+      steps_dist_mm.head.z = dc * steps_to_mm[C_AXIS];
+      steps_dist_mm.b      = (db + dc) * steps_to_mm[B_AXIS];
+      steps_dist_mm.c      = CORESIGN(db - dc) * steps_to_mm[C_AXIS];
+    #endif
+  #elif ENABLED(MARKFORGED_XY)
+    steps_dist_mm.head.x = da * steps_to_mm[A_AXIS];
+    steps_dist_mm.head.y = db * steps_to_mm[B_AXIS];
+    steps_dist_mm.z      = dc * steps_to_mm[Z_AXIS];
+    steps_dist_mm.a      = (da - db) * steps_to_mm[A_AXIS];
+    steps_dist_mm.b      = db * steps_to_mm[B_AXIS];
+  #else
+    steps_dist_mm.a = da * steps_to_mm[A_AXIS];
+    steps_dist_mm.b = db * steps_to_mm[B_AXIS];
+    steps_dist_mm.c = dc * steps_to_mm[C_AXIS];
+  #endif
+
+  #if EXTRUDERS
+    steps_dist_mm.e = esteps_float * steps_to_mm[E_AXIS_N(extruder)];
+  #else
+    steps_dist_mm.e = 0.0f;
+  #endif
+
+  TERN_(LCD_SHOW_E_TOTAL, e_move_accumulator += steps_dist_mm.e);
+
+  if (block->steps.a < MIN_STEPS_PER_SEGMENT && block->steps.b < MIN_STEPS_PER_SEGMENT && block->steps.c < MIN_STEPS_PER_SEGMENT) {
+    block->millimeters = (0
+      #if EXTRUDERS
+        + ABS(steps_dist_mm.e)
+      #endif
+    );
+  }
+  else {
+    if (millimeters)
+      block->millimeters = millimeters;
+    else
+      block->millimeters = SQRT(
+        #if EITHER(CORE_IS_XY, MARKFORGED_XY)
+          sq(steps_dist_mm.head.x) + sq(steps_dist_mm.head.y) + sq(steps_dist_mm.z)
+        #elif CORE_IS_XZ
+          sq(steps_dist_mm.head.x) + sq(steps_dist_mm.y) + sq(steps_dist_mm.head.z)
+        #elif CORE_IS_YZ
+          sq(steps_dist_mm.x) + sq(steps_dist_mm.head.y) + sq(steps_dist_mm.head.z)
+        #else
+          sq(steps_dist_mm.x) + sq(steps_dist_mm.y) + sq(steps_dist_mm.z)
+        #endif
+      );
+
+    /**
+     * At this point at least one of the axes has more steps than
+     * MIN_STEPS_PER_SEGMENT, ensuring the segment won't get dropped as
+     * zero-length. It's important to not apply corrections
+     * to blocks that would get dropped!
+     *
+     * A correction function is permitted to add steps to an axis, it
+     * should *never* remove steps!
+     */
+    TERN_(BACKLASH_COMPENSATION, backlash.add_correction_steps(da, db, dc, dm, block));
+  }
+
+  #if EXTRUDERS
+    block->steps.e = esteps;
+  #endif
+
+  block->step_event_count = _MAX(block->steps.a, block->steps.b, block->steps.c, esteps);
+
+  // Bail if this is a zero-length block
+  if (block->step_event_count < MIN_STEPS_PER_SEGMENT) return false;
+
+  #if ENABLED(MIXING_EXTRUDER)
+    MIXER_POPULATE_BLOCK();
+  #endif
+
+  TERN_(HAS_CUTTER, block->cutter_power = cutter.power);
+
+  #if HAS_FAN
+    FANS_LOOP(i) block->fan_speed[i] = thermalManager.fan_speed[i];
+  #endif
+
+  #if ENABLED(BARICUDA)
+    block->valve_pressure = baricuda_valve_pressure;
+    block->e_to_p_pressure = baricuda_e_to_p_pressure;
+  #endif
+
+  #if HAS_MULTI_EXTRUDER
+    block->extruder = extruder;
+  #endif
+
+  #if ENABLED(AUTO_POWER_CONTROL)
+    if (block->steps.x || block->steps.y || block->steps.z)
+      powerManager.power_on();
+  #endif
+
+  // Enable active axes
+  #if EITHER(CORE_IS_XY, MARKFORGED_XY)
+    if (block->steps.a || block->steps.b) {
+      ENABLE_AXIS_X();
+      ENABLE_AXIS_Y();
+    }
+    #if DISABLED(Z_LATE_ENABLE)
+      if (block->steps.z) ENABLE_AXIS_Z();
+    #endif
+  #elif CORE_IS_XZ
+    if (block->steps.a || block->steps.c) {
+      ENABLE_AXIS_X();
+      ENABLE_AXIS_Z();
+    }
+    if (block->steps.y) ENABLE_AXIS_Y();
+  #elif CORE_IS_YZ
+    if (block->steps.b || block->steps.c) {
+      ENABLE_AXIS_Y();
+      ENABLE_AXIS_Z();
+    }
+    if (block->steps.x) ENABLE_AXIS_X();
+  #else
+    if (block->steps.x) ENABLE_AXIS_X();
+    if (block->steps.y) ENABLE_AXIS_Y();
+    #if DISABLED(Z_LATE_ENABLE)
+      if (block->steps.z) ENABLE_AXIS_Z();
+    #endif
+  #endif
+
+  // Enable extruder(s)
+  #if EXTRUDERS
+    if (esteps) {
+      TERN_(AUTO_POWER_CONTROL, powerManager.power_on());
+
+      #if ENABLED(DISABLE_INACTIVE_EXTRUDER) // Enable only the selected extruder
+
+        LOOP_L_N(i, EXTRUDERS)
+          if (g_uc_extruder_last_move[i]) g_uc_extruder_last_move[i]--;
+
+        #define ENABLE_ONE_E(N) do{ \
+          if (extruder == N) { \
+            ENABLE_AXIS_E##N(); \
+            g_uc_extruder_last_move[N] = (BLOCK_BUFFER_SIZE) * 2; \
+            if ((N) == 0 && TERN0(HAS_DUPLICATION_MODE, extruder_duplication_enabled)) \
+              ENABLE_AXIS_E1(); \
+          } \
+          else if (!g_uc_extruder_last_move[N]) { \
+            DISABLE_AXIS_E##N(); \
+            if ((N) == 0 && TERN0(HAS_DUPLICATION_MODE, extruder_duplication_enabled)) \
+              DISABLE_AXIS_E1(); \
+          } \
+        }while(0);
+
+      #else
+
+        #define ENABLE_ONE_E(N) ENABLE_AXIS_E##N();
+
+      #endif
+
+      REPEAT(EXTRUDERS, ENABLE_ONE_E); // (ENABLE_ONE_E must end with semicolon)
+    }
+  #endif // EXTRUDERS
+
+  if (esteps)
+    NOLESS(fr_mm_s, settings.min_feedrate_mm_s);
+  else
+    NOLESS(fr_mm_s, settings.min_travel_feedrate_mm_s);
+
+  const float inverse_millimeters = 1.0f / block->millimeters;  // Inverse millimeters to remove multiple divides
+
+  // Calculate inverse time for this move. No divide by zero due to previous checks.
+  // Example: At 120mm/s a 60mm move takes 0.5s. So this will give 2.0.
+  float inverse_secs = fr_mm_s * inverse_millimeters;
+
+  // Get the number of non busy movements in queue (non busy means that they can be altered)
+  const uint8_t moves_queued = nonbusy_movesplanned();
+
+  // Slow down when the buffer starts to empty, rather than wait at the corner for a buffer refill
+  #if EITHER(SLOWDOWN, HAS_WIRED_LCD) || defined(XY_FREQUENCY_LIMIT)
+    // Segment time im micro seconds
+    int32_t segment_time_us = LROUND(1000000.0f / inverse_secs);
+  #endif
+
+  #if ENABLED(SLOWDOWN)
+    #ifndef SLOWDOWN_DIVISOR
+      #define SLOWDOWN_DIVISOR 2
+    #endif
+    if (WITHIN(moves_queued, 2, (BLOCK_BUFFER_SIZE) / (SLOWDOWN_DIVISOR) - 1)) {
+      const int32_t time_diff = settings.min_segment_time_us - segment_time_us;
+      if (time_diff > 0) {
+        // Buffer is draining so add extra time. The amount of time added increases if the buffer is still emptied more.
+        const int32_t nst = segment_time_us + LROUND(2 * time_diff / moves_queued);
+        inverse_secs = 1000000.0f / nst;
+        #if defined(XY_FREQUENCY_LIMIT) || HAS_WIRED_LCD
+          segment_time_us = nst;
+        #endif
+      }
+    }
+  #endif
+
+  #if HAS_WIRED_LCD
+    // Protect the access to the position.
+    const bool was_enabled = stepper.suspend();
+
+    block_buffer_runtime_us += segment_time_us;
+    block->segment_time_us = segment_time_us;
+
+    if (was_enabled) stepper.wake_up();
+  #endif
+
+  block->nominal_speed_sqr = sq(block->millimeters * inverse_secs);   // (mm/sec)^2 Always > 0
+  block->nominal_rate = CEIL(block->step_event_count * inverse_secs); // (step/sec) Always > 0
+
+  #if ENABLED(FILAMENT_WIDTH_SENSOR)
+    if (extruder == FILAMENT_SENSOR_EXTRUDER_NUM)   // Only for extruder with filament sensor
+      filwidth.advance_e(steps_dist_mm.e);
+  #endif
+
+  // Calculate and limit speed in mm/sec
+
+  xyze_float_t current_speed;
+  float speed_factor = 1.0f; // factor <1 decreases speed
+
+  // Linear axes first with less logic
+  LOOP_XYZ(i) {
+    current_speed[i] = steps_dist_mm[i] * inverse_secs;
+    const feedRate_t cs = ABS(current_speed[i]),
+                 max_fr = settings.max_feedrate_mm_s[i];
+    if (cs > max_fr) NOMORE(speed_factor, max_fr / cs);
+  }
+
+  // Limit speed on extruders, if any
+  #if EXTRUDERS
+    {
+      current_speed.e = steps_dist_mm.e * inverse_secs;
+      #if HAS_MIXER_SYNC_CHANNEL
+        // Move all mixing extruders at the specified rate
+        if (mixer.get_current_vtool() == MIXER_AUTORETRACT_TOOL)
+          current_speed.e *= MIXING_STEPPERS;
+      #endif
+
+      const feedRate_t cs = ABS(current_speed.e),
+                   max_fr = settings.max_feedrate_mm_s[E_AXIS_N(extruder)]
+                            * TERN(HAS_MIXER_SYNC_CHANNEL, MIXING_STEPPERS, 1);
+
+      if (cs > max_fr) NOMORE(speed_factor, max_fr / cs); //respect max feedrate on any movement (doesn't matter if E axes only or not)
+
+      #if ENABLED(VOLUMETRIC_EXTRUDER_LIMIT)
+        const feedRate_t max_vfr = volumetric_extruder_feedrate_limit[extruder]
+                                   * TERN(HAS_MIXER_SYNC_CHANNEL, MIXING_STEPPERS, 1);
+
+        // TODO: Doesn't work properly for joined segments. Set MIN_STEPS_PER_SEGMENT 1 as workaround.
+
+        if (block->steps.a || block->steps.b || block->steps.c) {
+
+          if (max_vfr > 0 && cs > max_vfr) {
+            NOMORE(speed_factor, max_vfr / cs); // respect volumetric extruder limit (if any)
+            /* <-- add a slash to enable
+            SERIAL_ECHOPAIR("volumetric extruder limit enforced: ", (cs * CIRCLE_AREA(filament_size[extruder] * 0.5f)));
+            SERIAL_ECHOPAIR(" mm^3/s (", cs);
+            SERIAL_ECHOPAIR(" mm/s) limited to ", (max_vfr * CIRCLE_AREA(filament_size[extruder] * 0.5f)));
+            SERIAL_ECHOPAIR(" mm^3/s (", max_vfr);
+            SERIAL_ECHOLNPGM(" mm/s)");
+            //*/
+          }
+        }
+      #endif
+    }
+  #endif
+
+  #ifdef XY_FREQUENCY_LIMIT
+
+    static uint8_t old_direction_bits; // = 0
+
+    if (xy_freq_limit_hz) {
+      // Check and limit the xy direction change frequency
+      const uint8_t direction_change = block->direction_bits ^ old_direction_bits;
+      old_direction_bits = block->direction_bits;
+      segment_time_us = LROUND(float(segment_time_us) / speed_factor);
+
+      static int32_t xs0, xs1, xs2, ys0, ys1, ys2;
+      if (segment_time_us > xy_freq_min_interval_us)
+        xs2 = xs1 = ys2 = ys1 = xy_freq_min_interval_us;
+      else {
+        xs2 = xs1; xs1 = xs0;
+        ys2 = ys1; ys1 = ys0;
+      }
+      xs0 = TEST(direction_change, X_AXIS) ? segment_time_us : xy_freq_min_interval_us;
+      ys0 = TEST(direction_change, Y_AXIS) ? segment_time_us : xy_freq_min_interval_us;
+
+      if (segment_time_us < xy_freq_min_interval_us) {
+        const int32_t least_xy_segment_time = _MIN(_MAX(xs0, xs1, xs2), _MAX(ys0, ys1, ys2));
+        if (least_xy_segment_time < xy_freq_min_interval_us) {
+          float freq_xy_feedrate = (speed_factor * least_xy_segment_time) / xy_freq_min_interval_us;
+          NOLESS(freq_xy_feedrate, xy_freq_min_speed_factor);
+          NOMORE(speed_factor, freq_xy_feedrate);
+        }
+      }
+    }
+
+  #endif // XY_FREQUENCY_LIMIT
+
+  // Correct the speed
+  if (speed_factor < 1.0f) {
+    current_speed *= speed_factor;
+    block->nominal_rate *= speed_factor;
+    block->nominal_speed_sqr = block->nominal_speed_sqr * sq(speed_factor);
+  }
+
+  // Compute and limit the acceleration rate for the trapezoid generator.
+  const float steps_per_mm = block->step_event_count * inverse_millimeters;
+  uint32_t accel;
+  if (!block->steps.a && !block->steps.b && !block->steps.c) {
+    // convert to: acceleration steps/sec^2
+    accel = CEIL(settings.retract_acceleration * steps_per_mm);
+    TERN_(LIN_ADVANCE, block->use_advance_lead = false);
+  }
+  else {
+    #define LIMIT_ACCEL_LONG(AXIS,INDX) do{ \
+      if (block->steps[AXIS] && max_acceleration_steps_per_s2[AXIS+INDX] < accel) { \
+        const uint32_t comp = max_acceleration_steps_per_s2[AXIS+INDX] * block->step_event_count; \
+        if (accel * block->steps[AXIS] > comp) accel = comp / block->steps[AXIS]; \
+      } \
+    }while(0)
+
+    #define LIMIT_ACCEL_FLOAT(AXIS,INDX) do{ \
+      if (block->steps[AXIS] && max_acceleration_steps_per_s2[AXIS+INDX] < accel) { \
+        const float comp = (float)max_acceleration_steps_per_s2[AXIS+INDX] * (float)block->step_event_count; \
+        if ((float)accel * (float)block->steps[AXIS] > comp) accel = comp / (float)block->steps[AXIS]; \
+      } \
+    }while(0)
+
+    // Start with print or travel acceleration
+    accel = CEIL((esteps ? settings.acceleration : settings.travel_acceleration) * steps_per_mm);
+
+    #if ENABLED(LIN_ADVANCE)
+
+      #define MAX_E_JERK(N) TERN(HAS_LINEAR_E_JERK, max_e_jerk[E_INDEX_N(N)], max_jerk.e)
+
+      /**
+       * Use LIN_ADVANCE for blocks if all these are true:
+       *
+       * esteps             : This is a print move, because we checked for A, B, C steps before.
+       *
+       * extruder_advance_K[active_extruder] : There is an advance factor set for this extruder.
+       *
+       * de > 0             : Extruder is running forward (e.g., for "Wipe while retracting" (Slic3r) or "Combing" (Cura) moves)
+       */
+      block->use_advance_lead =  esteps
+                              && extruder_advance_K[active_extruder]
+                              && de > 0;
+
+      if (block->use_advance_lead) {
+        block->e_D_ratio = (target_float.e - position_float.e) /
+          #if IS_KINEMATIC
+            block->millimeters
+          #else
+            SQRT(sq(target_float.x - position_float.x)
+               + sq(target_float.y - position_float.y)
+               + sq(target_float.z - position_float.z))
+          #endif
+        ;
+
+        // Check for unusual high e_D ratio to detect if a retract move was combined with the last print move due to min. steps per segment. Never execute this with advance!
+        // This assumes no one will use a retract length of 0mm < retr_length < ~0.2mm and no one will print 100mm wide lines using 3mm filament or 35mm wide lines using 1.75mm filament.
+        if (block->e_D_ratio > 3.0f)
+          block->use_advance_lead = false;
+        else {
+          const uint32_t max_accel_steps_per_s2 = MAX_E_JERK(extruder) / (extruder_advance_K[active_extruder] * block->e_D_ratio) * steps_per_mm;
+          if (TERN0(LA_DEBUG, accel > max_accel_steps_per_s2))
+            SERIAL_ECHOLNPGM("Acceleration limited.");
+          NOMORE(accel, max_accel_steps_per_s2);
+        }
+      }
+    #endif
+
+    // Limit acceleration per axis
+    if (block->step_event_count <= cutoff_long) {
+      LIMIT_ACCEL_LONG(A_AXIS, 0);
+      LIMIT_ACCEL_LONG(B_AXIS, 0);
+      LIMIT_ACCEL_LONG(C_AXIS, 0);
+      LIMIT_ACCEL_LONG(E_AXIS, E_INDEX_N(extruder));
+    }
+    else {
+      LIMIT_ACCEL_FLOAT(A_AXIS, 0);
+      LIMIT_ACCEL_FLOAT(B_AXIS, 0);
+      LIMIT_ACCEL_FLOAT(C_AXIS, 0);
+      LIMIT_ACCEL_FLOAT(E_AXIS, E_INDEX_N(extruder));
+    }
+  }
+  block->acceleration_steps_per_s2 = accel;
+  block->acceleration = accel / steps_per_mm;
+  #if DISABLED(S_CURVE_ACCELERATION)
+    block->acceleration_rate = (uint32_t)(accel * (4096.0f * 4096.0f / (STEPPER_TIMER_RATE)));
+  #endif
+  #if ENABLED(LIN_ADVANCE)
+    if (block->use_advance_lead) {
+      block->advance_speed = (STEPPER_TIMER_RATE) / (extruder_advance_K[active_extruder] * block->e_D_ratio * block->acceleration * settings.axis_steps_per_mm[E_AXIS_N(extruder)]);
+      #if ENABLED(LA_DEBUG)
+        if (extruder_advance_K[active_extruder] * block->e_D_ratio * block->acceleration * 2 < SQRT(block->nominal_speed_sqr) * block->e_D_ratio)
+          SERIAL_ECHOLNPGM("More than 2 steps per eISR loop executed.");
+        if (block->advance_speed < 200)
+          SERIAL_ECHOLNPGM("eISR running at > 10kHz.");
+      #endif
+    }
+  #endif
+
+  float vmax_junction_sqr; // Initial limit on the segment entry velocity (mm/s)^2
+
+  #if HAS_JUNCTION_DEVIATION
+    /**
+     * Compute maximum allowable entry speed at junction by centripetal acceleration approximation.
+     * Let a circle be tangent to both previous and current path line segments, where the junction
+     * deviation is defined as the distance from the junction to the closest edge of the circle,
+     * colinear with the circle center. The circular segment joining the two paths represents the
+     * path of centripetal acceleration. Solve for max velocity based on max acceleration about the
+     * radius of the circle, defined indirectly by junction deviation. This may be also viewed as
+     * path width or max_jerk in the previous Grbl version. This approach does not actually deviate
+     * from path, but used as a robust way to compute cornering speeds, as it takes into account the
+     * nonlinearities of both the junction angle and junction velocity.
+     *
+     * NOTE: If the junction deviation value is finite, Grbl executes the motions in an exact path
+     * mode (G61). If the junction deviation value is zero, Grbl will execute the motion in an exact
+     * stop mode (G61.1) manner. In the future, if continuous mode (G64) is desired, the math here
+     * is exactly the same. Instead of motioning all the way to junction point, the machine will
+     * just follow the arc circle defined here. The Arduino doesn't have the CPU cycles to perform
+     * a continuous mode path, but ARM-based microcontrollers most certainly do.
+     *
+     * NOTE: The max junction speed is a fixed value, since machine acceleration limits cannot be
+     * changed dynamically during operation nor can the line move geometry. This must be kept in
+     * memory in the event of a feedrate override changing the nominal speeds of blocks, which can
+     * change the overall maximum entry speed conditions of all blocks.
+     *
+     * #######
+     * https://github.com/MarlinFirmware/Marlin/issues/10341#issuecomment-388191754
+     *
+     * hoffbaked: on May 10 2018 tuned and improved the GRBL algorithm for Marlin:
+          Okay! It seems to be working good. I somewhat arbitrarily cut it off at 1mm
+          on then on anything with less sides than an octagon. With this, and the
+          reverse pass actually recalculating things, a corner acceleration value
+          of 1000 junction deviation of .05 are pretty reasonable. If the cycles
+          can be spared, a better acos could be used. For all I know, it may be
+          already calculated in a different place. */
+
+    // Unit vector of previous path line segment
+    static xyze_float_t prev_unit_vec;
+
+    xyze_float_t unit_vec =
+      #if HAS_DIST_MM_ARG
+        cart_dist_mm
+      #else
+        { steps_dist_mm.x, steps_dist_mm.y, steps_dist_mm.z, steps_dist_mm.e }
+      #endif
+    ;
+
+    /**
+     * On CoreXY the length of the vector [A,B] is SQRT(2) times the length of the head movement vector [X,Y].
+     * So taking Z and E into account, we cannot scale to a unit vector with "inverse_millimeters".
+     * => normalize the complete junction vector.
+     * Elsewise, when needed JD will factor-in the E component
+     */
+    if (EITHER(IS_CORE, MARKFORGED_XY) || esteps > 0)
+      normalize_junction_vector(unit_vec);  // Normalize with XYZE components
+    else
+      unit_vec *= inverse_millimeters;      // Use pre-calculated (1 / SQRT(x^2 + y^2 + z^2))
+
+    // Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles.
+    if (moves_queued && !UNEAR_ZERO(previous_nominal_speed_sqr)) {
+      // Compute cosine of angle between previous and current path. (prev_unit_vec is negative)
+      // NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity.
+      float junction_cos_theta = (-prev_unit_vec.x * unit_vec.x) + (-prev_unit_vec.y * unit_vec.y)
+                               + (-prev_unit_vec.z * unit_vec.z) + (-prev_unit_vec.e * unit_vec.e);
+
+      // NOTE: Computed without any expensive trig, sin() or acos(), by trig half angle identity of cos(theta).
+      if (junction_cos_theta > 0.999999f) {
+        // For a 0 degree acute junction, just set minimum junction speed.
+        vmax_junction_sqr = sq(float(MINIMUM_PLANNER_SPEED));
+      }
+      else {
+        NOLESS(junction_cos_theta, -0.999999f); // Check for numerical round-off to avoid divide by zero.
+
+        // Convert delta vector to unit vector
+        xyze_float_t junction_unit_vec = unit_vec - prev_unit_vec;
+        normalize_junction_vector(junction_unit_vec);
+
+        const float junction_acceleration = limit_value_by_axis_maximum(block->acceleration, junction_unit_vec),
+                    sin_theta_d2 = SQRT(0.5f * (1.0f - junction_cos_theta)); // Trig half angle identity. Always positive.
+
+        vmax_junction_sqr = junction_acceleration * junction_deviation_mm * sin_theta_d2 / (1.0f - sin_theta_d2);
+
+        #if ENABLED(JD_HANDLE_SMALL_SEGMENTS)
+
+          // For small moves with >135° junction (octagon) find speed for approximate arc
+          if (block->millimeters < 1 && junction_cos_theta < -0.7071067812f) {
+
+            #if ENABLED(JD_USE_MATH_ACOS)
+
+              #error "TODO: Inline maths with the MCU / FPU."
+
+            #elif ENABLED(JD_USE_LOOKUP_TABLE)
+
+              // Fast acos approximation (max. error +-0.01 rads)
+              // Based on LUT table and linear interpolation
+
+              /**
+               *  // Generate the JD Lookup Table
+               *  constexpr float c = 1.00751495f; // Correction factor to center error around 0
+               *  for (int i = 0; i < jd_lut_count - 1; ++i) {
+               *    const float x0 = (sq(i) - 1) / sq(i),
+               *                y0 = acos(x0) * (i == 0 ? 1 : c),
+               *                x1 = i < jd_lut_count - 1 ?  0.5 * x0 + 0.5 : 0.999999f,
+               *                y1 = acos(x1) * (i < jd_lut_count - 1 ? c : 1);
+               *    jd_lut_k[i] = (y0 - y1) / (x0 - x1);
+               *    jd_lut_b[i] = (y1 * x0 - y0 * x1) / (x0 - x1);
+               *  }
+               *
+               *  // Compute correction factor (Set c to 1.0f first!)
+               *  float min = INFINITY, max = -min;
+               *  for (float t = 0; t <= 1; t += 0.0003f) {
+               *    const float e = acos(t) / approx(t);
+               *    if (isfinite(e)) {
+               *      if (e < min) min = e;
+               *      if (e > max) max = e;
+               *    }
+               *  }
+               *  fprintf(stderr, "%.9gf, ", (min + max) / 2);
+               */
+              static constexpr int16_t  jd_lut_count = 16;
+              static constexpr uint16_t jd_lut_tll   = _BV(jd_lut_count - 1);
+              static constexpr int16_t  jd_lut_tll0  = __builtin_clz(jd_lut_tll) + 1; // i.e., 16 - jd_lut_count + 1
+              static constexpr float jd_lut_k[jd_lut_count] PROGMEM = {
+                -1.03145837f, -1.30760646f, -1.75205851f, -2.41705704f,
+                -3.37769222f, -4.74888992f, -6.69649887f, -9.45661736f,
+                -13.3640480f, -18.8928222f, -26.7136841f, -37.7754593f,
+                -53.4201813f, -75.5458374f, -106.836761f, -218.532821f };
+              static constexpr float jd_lut_b[jd_lut_count] PROGMEM = {
+                 1.57079637f,  1.70887053f,  2.04220939f,  2.62408352f,
+                 3.52467871f,  4.85302639f,  6.77020454f,  9.50875854f,
+                 13.4009285f,  18.9188995f,  26.7321243f,  37.7885055f,
+                 53.4293975f,  75.5523529f,  106.841369f,  218.534011f };
+
+              const float neg = junction_cos_theta < 0 ? -1 : 1,
+                          t = neg * junction_cos_theta;
+
+              const int16_t idx = (t < 0.00000003f) ? 0 : __builtin_clz(uint16_t((1.0f - t) * jd_lut_tll)) - jd_lut_tll0;
+
+              float junction_theta = t * pgm_read_float(&jd_lut_k[idx]) + pgm_read_float(&jd_lut_b[idx]);
+              if (neg > 0) junction_theta = RADIANS(180) - junction_theta; // acos(-t)
+
+            #else
+
+              // Fast acos(-t) approximation (max. error +-0.033rad = 1.89°)
+              // Based on MinMax polynomial published by W. Randolph Franklin, see
+              // https://wrf.ecse.rpi.edu/Research/Short_Notes/arcsin/onlyelem.html
+              //  acos( t) = pi / 2 - asin(x)
+              //  acos(-t) = pi - acos(t) ... pi / 2 + asin(x)
+
+              const float neg = junction_cos_theta < 0 ? -1 : 1,
+                          t = neg * junction_cos_theta,
+                          asinx =       0.032843707f
+                                + t * (-1.451838349f
+                                + t * ( 29.66153956f
+                                + t * (-131.1123477f
+                                + t * ( 262.8130562f
+                                + t * (-242.7199627f
+                                + t * ( 84.31466202f ) ))))),
+                          junction_theta = RADIANS(90) + neg * asinx; // acos(-t)
+
+              // NOTE: junction_theta bottoms out at 0.033 which avoids divide by 0.
+
+            #endif
+
+            const float limit_sqr = (block->millimeters * junction_acceleration) / junction_theta;
+            NOMORE(vmax_junction_sqr, limit_sqr);
+          }
+
+        #endif // JD_HANDLE_SMALL_SEGMENTS
+      }
+
+      // Get the lowest speed
+      vmax_junction_sqr = _MIN(vmax_junction_sqr, block->nominal_speed_sqr, previous_nominal_speed_sqr);
+    }
+    else // Init entry speed to zero. Assume it starts from rest. Planner will correct this later.
+      vmax_junction_sqr = 0;
+
+    prev_unit_vec = unit_vec;
+
+  #endif
+
+  #ifdef USE_CACHED_SQRT
+    #define CACHED_SQRT(N, V) \
+      static float saved_V, N; \
+      if (V != saved_V) { N = SQRT(V); saved_V = V; }
+  #else
+    #define CACHED_SQRT(N, V) const float N = SQRT(V)
+  #endif
+
+  #if HAS_CLASSIC_JERK
+
+    /**
+     * Adapted from Průša MKS firmware
+     * https://github.com/prusa3d/Prusa-Firmware
+     */
+    CACHED_SQRT(nominal_speed, block->nominal_speed_sqr);
+
+    // Exit speed limited by a jerk to full halt of a previous last segment
+    static float previous_safe_speed;
+
+    // Start with a safe speed (from which the machine may halt to stop immediately).
+    float safe_speed = nominal_speed;
+
+    #ifndef TRAVEL_EXTRA_XYJERK
+      #define TRAVEL_EXTRA_XYJERK 0
+    #endif
+    const float extra_xyjerk = (de <= 0) ? TRAVEL_EXTRA_XYJERK : 0;
+
+    uint8_t limited = 0;
+    TERN(HAS_LINEAR_E_JERK, LOOP_XYZ, LOOP_XYZE)(i) {
+      const float jerk = ABS(current_speed[i]),   // cs : Starting from zero, change in speed for this axis
+                  maxj = (max_jerk[i] + (i == X_AXIS || i == Y_AXIS ? extra_xyjerk : 0.0f)); // mj : The max jerk setting for this axis
+      if (jerk > maxj) {                          // cs > mj : New current speed too fast?
+        if (limited) {                            // limited already?
+          const float mjerk = nominal_speed * maxj; // ns*mj
+          if (jerk * safe_speed > mjerk) safe_speed = mjerk / jerk; // ns*mj/cs
+        }
+        else {
+          safe_speed *= maxj / jerk;              // Initial limit: ns*mj/cs
+          ++limited;                              // Initially limited
+        }
+      }
+    }
+
+    float vmax_junction;
+    if (moves_queued && !UNEAR_ZERO(previous_nominal_speed_sqr)) {
+      // Estimate a maximum velocity allowed at a joint of two successive segments.
+      // If this maximum velocity allowed is lower than the minimum of the entry / exit safe velocities,
+      // then the machine is not coasting anymore and the safe entry / exit velocities shall be used.
+
+      // Factor to multiply the previous / current nominal velocities to get componentwise limited velocities.
+      float v_factor = 1;
+      limited = 0;
+
+      // The junction velocity will be shared between successive segments. Limit the junction velocity to their minimum.
+      // Pick the smaller of the nominal speeds. Higher speed shall not be achieved at the junction during coasting.
+      CACHED_SQRT(previous_nominal_speed, previous_nominal_speed_sqr);
+
+      float smaller_speed_factor = 1.0f;
+      if (nominal_speed < previous_nominal_speed) {
+        vmax_junction = nominal_speed;
+        smaller_speed_factor = vmax_junction / previous_nominal_speed;
+      }
+      else
+        vmax_junction = previous_nominal_speed;
+
+      // Now limit the jerk in all axes.
+      TERN(HAS_LINEAR_E_JERK, LOOP_XYZ, LOOP_XYZE)(axis) {
+        // Limit an axis. We have to differentiate: coasting, reversal of an axis, full stop.
+        float v_exit = previous_speed[axis] * smaller_speed_factor,
+              v_entry = current_speed[axis];
+        if (limited) {
+          v_exit *= v_factor;
+          v_entry *= v_factor;
+        }
+
+        // Calculate jerk depending on whether the axis is coasting in the same direction or reversing.
+        const float jerk = (v_exit > v_entry)
+            ? //                                  coasting             axis reversal
+              ( (v_entry > 0 || v_exit < 0) ? (v_exit - v_entry) : _MAX(v_exit, -v_entry) )
+            : // v_exit <= v_entry                coasting             axis reversal
+              ( (v_entry < 0 || v_exit > 0) ? (v_entry - v_exit) : _MAX(-v_exit, v_entry) );
+
+        const float maxj = (max_jerk[axis] + (axis == X_AXIS || axis == Y_AXIS ? extra_xyjerk : 0.0f));
+
+        if (jerk > maxj) {
+          v_factor *= maxj / jerk;
+          ++limited;
+        }
+      }
+      if (limited) vmax_junction *= v_factor;
+      // Now the transition velocity is known, which maximizes the shared exit / entry velocity while
+      // respecting the jerk factors, it may be possible, that applying separate safe exit / entry velocities will achieve faster prints.
+      const float vmax_junction_threshold = vmax_junction * 0.99f;
+      if (previous_safe_speed > vmax_junction_threshold && safe_speed > vmax_junction_threshold)
+        vmax_junction = safe_speed;
+    }
+    else
+      vmax_junction = safe_speed;
+
+    previous_safe_speed = safe_speed;
+
+    #if HAS_JUNCTION_DEVIATION
+      NOMORE(vmax_junction_sqr, sq(vmax_junction));   // Throttle down to max speed
+    #else
+      vmax_junction_sqr = sq(vmax_junction);          // Go up or down to the new speed
+    #endif
+
+  #endif // Classic Jerk Limiting
+
+  // Max entry speed of this block equals the max exit speed of the previous block.
+  block->max_entry_speed_sqr = vmax_junction_sqr;
+
+  // Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED.
+  const float v_allowable_sqr = max_allowable_speed_sqr(-block->acceleration, sq(float(MINIMUM_PLANNER_SPEED)), block->millimeters);
+
+  // If we are trying to add a split block, start with the
+  // max. allowed speed to avoid an interrupted first move.
+  block->entry_speed_sqr = !split_move ? sq(float(MINIMUM_PLANNER_SPEED)) : _MIN(vmax_junction_sqr, v_allowable_sqr);
+
+  // Initialize planner efficiency flags
+  // Set flag if block will always reach maximum junction speed regardless of entry/exit speeds.
+  // If a block can de/ac-celerate from nominal speed to zero within the length of the block, then
+  // the current block and next block junction speeds are guaranteed to always be at their maximum
+  // junction speeds in deceleration and acceleration, respectively. This is due to how the current
+  // block nominal speed limits both the current and next maximum junction speeds. Hence, in both
+  // the reverse and forward planners, the corresponding block junction speed will always be at the
+  // the maximum junction speed and may always be ignored for any speed reduction checks.
+  block->flag |= block->nominal_speed_sqr <= v_allowable_sqr ? BLOCK_FLAG_RECALCULATE | BLOCK_FLAG_NOMINAL_LENGTH : BLOCK_FLAG_RECALCULATE;
+
+  // Update previous path unit_vector and nominal speed
+  previous_speed = current_speed;
+  previous_nominal_speed_sqr = block->nominal_speed_sqr;
+
+  position = target;  // Update the position
+
+  TERN_(HAS_POSITION_FLOAT, position_float = target_float);
+  TERN_(GRADIENT_MIX, mixer.gradient_control(target_float.z));
+  TERN_(POWER_LOSS_RECOVERY, block->sdpos = recovery.command_sdpos());
+
+  return true;        // Movement was accepted
+
+} // _populate_block()
+
+/**
+ * Planner::buffer_sync_block
+ * Add a block to the buffer that just updates the position
+ */
+void Planner::buffer_sync_block() {
+  // Wait for the next available block
+  uint8_t next_buffer_head;
+  block_t * const block = get_next_free_block(next_buffer_head);
+
+  // Clear block
+  memset(block, 0, sizeof(block_t));
+
+  block->flag = BLOCK_FLAG_SYNC_POSITION;
+
+  block->position = position;
+
+  // If this is the first added movement, reload the delay, otherwise, cancel it.
+  if (block_buffer_head == block_buffer_tail) {
+    // If it was the first queued block, restart the 1st block delivery delay, to
+    // give the planner an opportunity to queue more movements and plan them
+    // As there are no queued movements, the Stepper ISR will not touch this
+    // variable, so there is no risk setting this here (but it MUST be done
+    // before the following line!!)
+    delay_before_delivering = BLOCK_DELAY_FOR_1ST_MOVE;
+  }
+
+  block_buffer_head = next_buffer_head;
+
+  stepper.wake_up();
+} // buffer_sync_block()
+
+/**
+ * Planner::buffer_segment
+ *
+ * Add a new linear movement to the buffer in axis units.
+ *
+ * Leveling and kinematics should be applied ahead of calling this.
+ *
+ *  a,b,c,e     - target positions in mm and/or degrees
+ *  fr_mm_s     - (target) speed of the move
+ *  extruder    - target extruder
+ *  millimeters - the length of the movement, if known
+ *
+ * Return 'false' if no segment was queued due to cleaning, cold extrusion, full queue, etc.
+ */
+bool Planner::buffer_segment(const float &a, const float &b, const float &c, const float &e
+  #if HAS_DIST_MM_ARG
+    , const xyze_float_t &cart_dist_mm
+  #endif
+  , const feedRate_t &fr_mm_s, const uint8_t extruder, const float &millimeters/*=0.0*/
+) {
+
+  // If we are cleaning, do not accept queuing of movements
+  if (cleaning_buffer_counter) return false;
+
+  // When changing extruders recalculate steps corresponding to the E position
+  #if ENABLED(DISTINCT_E_FACTORS)
+    if (last_extruder != extruder && settings.axis_steps_per_mm[E_AXIS_N(extruder)] != settings.axis_steps_per_mm[E_AXIS_N(last_extruder)]) {
+      position.e = LROUND(position.e * settings.axis_steps_per_mm[E_AXIS_N(extruder)] * steps_to_mm[E_AXIS_N(last_extruder)]);
+      last_extruder = extruder;
+    }
+  #endif
+
+  // The target position of the tool in absolute steps
+  // Calculate target position in absolute steps
+  const abce_long_t target = {
+    int32_t(LROUND(a * settings.axis_steps_per_mm[A_AXIS])),
+    int32_t(LROUND(b * settings.axis_steps_per_mm[B_AXIS])),
+    int32_t(LROUND(c * settings.axis_steps_per_mm[C_AXIS])),
+    int32_t(LROUND(e * settings.axis_steps_per_mm[E_AXIS_N(extruder)]))
+  };
+
+  #if HAS_POSITION_FLOAT
+    const xyze_pos_t target_float = { a, b, c, e };
+  #endif
+
+  // DRYRUN prevents E moves from taking place
+  if (DEBUGGING(DRYRUN) || TERN0(CANCEL_OBJECTS, cancelable.skipping)) {
+    position.e = target.e;
+    TERN_(HAS_POSITION_FLOAT, position_float.e = e);
+  }
+
+  /* <-- add a slash to enable
+    SERIAL_ECHOPAIR("  buffer_segment FR:", fr_mm_s);
+    #if IS_KINEMATIC
+      SERIAL_ECHOPAIR(" A:", a);
+      SERIAL_ECHOPAIR(" (", position.a);
+      SERIAL_ECHOPAIR("->", target.a);
+      SERIAL_ECHOPAIR(") B:", b);
+    #else
+      SERIAL_ECHOPAIR_P(SP_X_LBL, a);
+      SERIAL_ECHOPAIR(" (", position.x);
+      SERIAL_ECHOPAIR("->", target.x);
+      SERIAL_CHAR(')');
+      SERIAL_ECHOPAIR_P(SP_Y_LBL, b);
+    #endif
+    SERIAL_ECHOPAIR(" (", position.y);
+    SERIAL_ECHOPAIR("->", target.y);
+    #if ENABLED(DELTA)
+      SERIAL_ECHOPAIR(") C:", c);
+    #else
+      SERIAL_CHAR(')');
+      SERIAL_ECHOPAIR_P(SP_Z_LBL, c);
+    #endif
+    SERIAL_ECHOPAIR(" (", position.z);
+    SERIAL_ECHOPAIR("->", target.z);
+    SERIAL_CHAR(')');
+    SERIAL_ECHOPAIR_P(SP_E_LBL, e);
+    SERIAL_ECHOPAIR(" (", position.e);
+    SERIAL_ECHOPAIR("->", target.e);
+    SERIAL_ECHOLNPGM(")");
+  //*/
+
+  // Queue the movement. Return 'false' if the move was not queued.
+  if (!_buffer_steps(target
+      #if HAS_POSITION_FLOAT
+        , target_float
+      #endif
+      #if HAS_DIST_MM_ARG
+        , cart_dist_mm
+      #endif
+      , fr_mm_s, extruder, millimeters)
+  ) return false;
+
+  stepper.wake_up();
+  return true;
+} // buffer_segment()
+
+/**
+ * Add a new linear movement to the buffer.
+ * The target is cartesian. It's translated to
+ * delta/scara if needed.
+ *
+ *  rx,ry,rz,e   - target position in mm or degrees
+ *  fr_mm_s      - (target) speed of the move (mm/s)
+ *  extruder     - target extruder
+ *  millimeters  - the length of the movement, if known
+ *  inv_duration - the reciprocal if the duration of the movement, if known (kinematic only if feeedrate scaling is enabled)
+ */
+bool Planner::buffer_line(const float &rx, const float &ry, const float &rz, const float &e, const feedRate_t &fr_mm_s, const uint8_t extruder, const float millimeters
+  #if ENABLED(SCARA_FEEDRATE_SCALING)
+    , const float &inv_duration
+  #endif
+) {
+  xyze_pos_t machine = { rx, ry, rz, e };
+  TERN_(HAS_POSITION_MODIFIERS, apply_modifiers(machine));
+
+  #if IS_KINEMATIC
+
+    #if HAS_JUNCTION_DEVIATION
+      const xyze_pos_t cart_dist_mm = {
+        rx - position_cart.x, ry - position_cart.y,
+        rz - position_cart.z, e  - position_cart.e
+      };
+    #else
+      const xyz_pos_t cart_dist_mm = { rx - position_cart.x, ry - position_cart.y, rz - position_cart.z };
+    #endif
+
+    float mm = millimeters;
+    if (mm == 0.0)
+      mm = (cart_dist_mm.x != 0.0 || cart_dist_mm.y != 0.0) ? cart_dist_mm.magnitude() : ABS(cart_dist_mm.z);
+
+    // Cartesian XYZ to kinematic ABC, stored in global 'delta'
+    inverse_kinematics(machine);
+
+    #if ENABLED(SCARA_FEEDRATE_SCALING)
+      // For SCARA scale the feed rate from mm/s to degrees/s
+      // i.e., Complete the angular vector in the given time.
+      const float duration_recip = inv_duration ?: fr_mm_s / mm;
+      const xyz_pos_t diff = delta - position_float;
+      const feedRate_t feedrate = diff.magnitude() * duration_recip;
+    #else
+      const feedRate_t feedrate = fr_mm_s;
+    #endif
+    if (buffer_segment(delta.a, delta.b, delta.c, machine.e
+      #if HAS_JUNCTION_DEVIATION
+        , cart_dist_mm
+      #endif
+      , feedrate, extruder, mm
+    )) {
+      position_cart.set(rx, ry, rz, e);
+      return true;
+    }
+    else
+      return false;
+  #else
+    return buffer_segment(machine, fr_mm_s, extruder, millimeters);
+  #endif
+} // buffer_line()
+
+#if ENABLED(DIRECT_STEPPING)
+
+  void Planner::buffer_page(const page_idx_t page_idx, const uint8_t extruder, const uint16_t num_steps) {
+    if (!last_page_step_rate) {
+      kill(GET_TEXT(MSG_BAD_PAGE_SPEED));
+      return;
+    }
+
+    uint8_t next_buffer_head;
+    block_t * const block = get_next_free_block(next_buffer_head);
+
+    block->flag = BLOCK_FLAG_IS_PAGE;
+
+    #if FAN_COUNT > 0
+      FANS_LOOP(i) block->fan_speed[i] = thermalManager.fan_speed[i];
+    #endif
+
+    #if HAS_MULTI_EXTRUDER
+      block->extruder = extruder;
+    #endif
+
+    block->page_idx = page_idx;
+
+    block->step_event_count = num_steps;
+    block->initial_rate =
+      block->final_rate =
+      block->nominal_rate = last_page_step_rate; // steps/s
+
+    block->accelerate_until = 0;
+    block->decelerate_after = block->step_event_count;
+
+    // Will be set to last direction later if directional format.
+    block->direction_bits = 0;
+
+    #define PAGE_UPDATE_DIR(AXIS) \
+      if (!last_page_dir[_AXIS(AXIS)]) SBI(block->direction_bits, _AXIS(AXIS));
+
+    if (!DirectStepping::Config::DIRECTIONAL) {
+      PAGE_UPDATE_DIR(X);
+      PAGE_UPDATE_DIR(Y);
+      PAGE_UPDATE_DIR(Z);
+      PAGE_UPDATE_DIR(E);
+    }
+
+    // If this is the first added movement, reload the delay, otherwise, cancel it.
+    if (block_buffer_head == block_buffer_tail) {
+      // If it was the first queued block, restart the 1st block delivery delay, to
+      // give the planner an opportunity to queue more movements and plan them
+      // As there are no queued movements, the Stepper ISR will not touch this
+      // variable, so there is no risk setting this here (but it MUST be done
+      // before the following line!!)
+      delay_before_delivering = BLOCK_DELAY_FOR_1ST_MOVE;
+    }
+
+    // Move buffer head
+    block_buffer_head = next_buffer_head;
+
+    enable_all_steppers();
+    stepper.wake_up();
+  }
+
+#endif // DIRECT_STEPPING
+
+/**
+ * Directly set the planner ABC position (and stepper positions)
+ * converting mm (or angles for SCARA) into steps.
+ *
+ * The provided ABC position is in machine units.
+ */
+
+void Planner::set_machine_position_mm(const float &a, const float &b, const float &c, const float &e) {
+  TERN_(DISTINCT_E_FACTORS, last_extruder = active_extruder);
+  TERN_(HAS_POSITION_FLOAT, position_float.set(a, b, c, e));
+  position.set(LROUND(a * settings.axis_steps_per_mm[A_AXIS]),
+               LROUND(b * settings.axis_steps_per_mm[B_AXIS]),
+               LROUND(c * settings.axis_steps_per_mm[C_AXIS]),
+               LROUND(e * settings.axis_steps_per_mm[E_AXIS_N(active_extruder)]));
+  if (has_blocks_queued()) {
+    //previous_nominal_speed_sqr = 0.0; // Reset planner junction speeds. Assume start from rest.
+    //previous_speed.reset();
+    buffer_sync_block();
+  }
+  else
+    stepper.set_position(position);
+}
+
+void Planner::set_position_mm(const float &rx, const float &ry, const float &rz, const float &e) {
+  xyze_pos_t machine = { rx, ry, rz, e };
+  #if HAS_POSITION_MODIFIERS
+    apply_modifiers(machine, true);
+  #endif
+  #if IS_KINEMATIC
+    position_cart.set(rx, ry, rz, e);
+    inverse_kinematics(machine);
+    set_machine_position_mm(delta.a, delta.b, delta.c, machine.e);
+  #else
+    set_machine_position_mm(machine);
+  #endif
+}
+
+/**
+ * Setters for planner position (also setting stepper position).
+ */
+void Planner::set_e_position_mm(const float &e) {
+  const uint8_t axis_index = E_AXIS_N(active_extruder);
+  TERN_(DISTINCT_E_FACTORS, last_extruder = active_extruder);
+
+  const float e_new = e - TERN0(FWRETRACT, fwretract.current_retract[active_extruder]);
+  position.e = LROUND(settings.axis_steps_per_mm[axis_index] * e_new);
+  TERN_(HAS_POSITION_FLOAT, position_float.e = e_new);
+  TERN_(IS_KINEMATIC, position_cart.e = e);
+
+  if (has_blocks_queued())
+    buffer_sync_block();
+  else
+    stepper.set_axis_position(E_AXIS, position.e);
+}
+
+// Recalculate the steps/s^2 acceleration rates, based on the mm/s^2
+void Planner::reset_acceleration_rates() {
+  #if ENABLED(DISTINCT_E_FACTORS)
+    #define AXIS_CONDITION (i < E_AXIS || i == E_AXIS_N(active_extruder))
+  #else
+    #define AXIS_CONDITION true
+  #endif
+  uint32_t highest_rate = 1;
+  LOOP_XYZE_N(i) {
+    max_acceleration_steps_per_s2[i] = settings.max_acceleration_mm_per_s2[i] * settings.axis_steps_per_mm[i];
+    if (AXIS_CONDITION) NOLESS(highest_rate, max_acceleration_steps_per_s2[i]);
+  }
+  cutoff_long = 4294967295UL / highest_rate; // 0xFFFFFFFFUL
+  TERN_(HAS_LINEAR_E_JERK, recalculate_max_e_jerk());
+}
+
+// Recalculate position, steps_to_mm if settings.axis_steps_per_mm changes!
+void Planner::refresh_positioning() {
+  LOOP_XYZE_N(i) steps_to_mm[i] = 1.0f / settings.axis_steps_per_mm[i];
+  set_position_mm(current_position);
+  reset_acceleration_rates();
+}
+
+inline void limit_and_warn(float &val, const uint8_t axis, PGM_P const setting_name, const xyze_float_t &max_limit) {
+  const uint8_t lim_axis = axis > E_AXIS ? E_AXIS : axis;
+  const float before = val;
+  LIMIT(val, 0.1, max_limit[lim_axis]);
+  if (before != val) {
+    SERIAL_CHAR(axis_codes[lim_axis]);
+    SERIAL_ECHOPGM(" Max ");
+    serialprintPGM(setting_name);
+    SERIAL_ECHOLNPAIR(" limited to ", val);
+  }
+}
+
+void Planner::set_max_acceleration(const uint8_t axis, float targetValue) {
+  #if ENABLED(LIMITED_MAX_ACCEL_EDITING)
+    #ifdef MAX_ACCEL_EDIT_VALUES
+      constexpr xyze_float_t max_accel_edit = MAX_ACCEL_EDIT_VALUES;
+      const xyze_float_t &max_acc_edit_scaled = max_accel_edit;
+    #else
+      constexpr xyze_float_t max_accel_edit = DEFAULT_MAX_ACCELERATION;
+      const xyze_float_t max_acc_edit_scaled = max_accel_edit * 2;
+    #endif
+    limit_and_warn(targetValue, axis, PSTR("Acceleration"), max_acc_edit_scaled);
+  #endif
+  settings.max_acceleration_mm_per_s2[axis] = targetValue;
+
+  // Update steps per s2 to agree with the units per s2 (since they are used in the planner)
+  reset_acceleration_rates();
+}
+
+void Planner::set_max_feedrate(const uint8_t axis, float targetValue) {
+  #if ENABLED(LIMITED_MAX_FR_EDITING)
+    #ifdef MAX_FEEDRATE_EDIT_VALUES
+      constexpr xyze_float_t max_fr_edit = MAX_FEEDRATE_EDIT_VALUES;
+      const xyze_float_t &max_fr_edit_scaled = max_fr_edit;
+    #else
+      constexpr xyze_float_t max_fr_edit = DEFAULT_MAX_FEEDRATE;
+      const xyze_float_t max_fr_edit_scaled = max_fr_edit * 2;
+    #endif
+    limit_and_warn(targetValue, axis, PSTR("Feedrate"), max_fr_edit_scaled);
+  #endif
+  settings.max_feedrate_mm_s[axis] = targetValue;
+}
+
+void Planner::set_max_jerk(const AxisEnum axis, float targetValue) {
+  #if HAS_CLASSIC_JERK
+    #if ENABLED(LIMITED_JERK_EDITING)
+      constexpr xyze_float_t max_jerk_edit =
+        #ifdef MAX_JERK_EDIT_VALUES
+          MAX_JERK_EDIT_VALUES
+        #else
+          { (DEFAULT_XJERK) * 2, (DEFAULT_YJERK) * 2,
+            (DEFAULT_ZJERK) * 2, (DEFAULT_EJERK) * 2 }
+        #endif
+      ;
+      limit_and_warn(targetValue, axis, PSTR("Jerk"), max_jerk_edit);
+    #endif
+    max_jerk[axis] = targetValue;
+  #else
+    UNUSED(axis); UNUSED(targetValue);
+  #endif
+}
+
+#if HAS_WIRED_LCD
+
+  uint16_t Planner::block_buffer_runtime() {
+    #ifdef __AVR__
+      // Protect the access to the variable. Only required for AVR, as
+      //  any 32bit CPU offers atomic access to 32bit variables
+      const bool was_enabled = stepper.suspend();
+    #endif
+
+    uint32_t bbru = block_buffer_runtime_us;
+
+    #ifdef __AVR__
+      // Reenable Stepper ISR
+      if (was_enabled) stepper.wake_up();
+    #endif
+
+    // To translate µs to ms a division by 1000 would be required.
+    // We introduce 2.4% error here by dividing by 1024.
+    // Doesn't matter because block_buffer_runtime_us is already too small an estimation.
+    bbru >>= 10;
+    // limit to about a minute.
+    NOMORE(bbru, 0x0000FFFFUL);
+    return bbru;
+  }
+
+  void Planner::clear_block_buffer_runtime() {
+    #ifdef __AVR__
+      // Protect the access to the variable. Only required for AVR, as
+      //  any 32bit CPU offers atomic access to 32bit variables
+      const bool was_enabled = stepper.suspend();
+    #endif
+
+    block_buffer_runtime_us = 0;
+
+    #ifdef __AVR__
+      // Reenable Stepper ISR
+      if (was_enabled) stepper.wake_up();
+    #endif
+  }
+
+#endif
+
+#if ENABLED(AUTOTEMP)
+
+void Planner::autotemp_update() {
+  #if ENABLED(AUTOTEMP_PROPORTIONAL)
+    const int16_t target = thermalManager.degTargetHotend(active_extruder);
+    autotemp_min = target + AUTOTEMP_MIN_P;
+    autotemp_max = target + AUTOTEMP_MAX_P;
+  #endif
+  autotemp_factor = TERN(AUTOTEMP_PROPORTIONAL, AUTOTEMP_FACTOR_P, 0);
+  autotemp_enabled = autotemp_factor != 0;
+}
+
+  void Planner::autotemp_M104_M109() {
+
+    #if ENABLED(AUTOTEMP_PROPORTIONAL)
+      const int16_t target = thermalManager.degTargetHotend(active_extruder);
+      autotemp_min = target + AUTOTEMP_MIN_P;
+      autotemp_max = target + AUTOTEMP_MAX_P;
+    #endif
+
+    if (parser.seenval('S')) autotemp_min = parser.value_celsius();
+    if (parser.seenval('B')) autotemp_max = parser.value_celsius();
+
+    // When AUTOTEMP_PROPORTIONAL is enabled, F0 disables autotemp.
+    // Normally, leaving off F also disables autotemp.
+    autotemp_factor = parser.seen('F') ? parser.value_float() : TERN(AUTOTEMP_PROPORTIONAL, AUTOTEMP_FACTOR_P, 0);
+    autotemp_enabled = autotemp_factor != 0;
+  }
+#endif
diff --git a/Marlin/src/module/planner.h b/Marlin/src/module/planner.h
new file mode 100644
index 0000000..cd906c5
--- /dev/null
+++ b/Marlin/src/module/planner.h
@@ -0,0 +1,983 @@
+/**
+ * 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
+
+/**
+ * planner.h
+ *
+ * Buffer movement commands and manage the acceleration profile plan
+ *
+ * Derived from Grbl
+ * Copyright (c) 2009-2011 Simen Svale Skogsrud
+ */
+
+#include "../MarlinCore.h"
+
+#if ENABLED(JD_HANDLE_SMALL_SEGMENTS)
+  // Enable this option for perfect accuracy but maximum
+  // computation. Should be fine on ARM processors.
+  //#define JD_USE_MATH_ACOS
+
+  // Disable this option to save 120 bytes of PROGMEM,
+  // but incur increased computation and a reduction
+  // in accuracy.
+  #define JD_USE_LOOKUP_TABLE
+#endif
+
+#include "motion.h"
+#include "../gcode/queue.h"
+
+#if ENABLED(DELTA)
+  #include "delta.h"
+#endif
+
+#if ABL_PLANAR
+  #include "../libs/vector_3.h" // for matrix_3x3
+#endif
+
+#if ENABLED(FWRETRACT)
+  #include "../feature/fwretract.h"
+#endif
+
+#if ENABLED(MIXING_EXTRUDER)
+  #include "../feature/mixing.h"
+#endif
+
+#if HAS_CUTTER
+  #include "../feature/spindle_laser_types.h"
+#endif
+
+#if ENABLED(DIRECT_STEPPING)
+  #include "../feature/direct_stepping.h"
+  #define IS_PAGE(B) TEST(B->flag, BLOCK_BIT_IS_PAGE)
+#else
+  #define IS_PAGE(B) false
+#endif
+
+// Feedrate for manual moves
+#ifdef MANUAL_FEEDRATE
+  constexpr xyze_feedrate_t _mf = MANUAL_FEEDRATE,
+                            manual_feedrate_mm_s { _mf.x / 60.0f, _mf.y / 60.0f, _mf.z / 60.0f, _mf.e / 60.0f };
+#endif
+
+#if IS_KINEMATIC && HAS_JUNCTION_DEVIATION
+  #define HAS_DIST_MM_ARG 1
+#endif
+
+enum BlockFlagBit : char {
+  // Recalculate trapezoids on entry junction. For optimization.
+  BLOCK_BIT_RECALCULATE,
+
+  // Nominal speed always reached.
+  // i.e., The segment is long enough, so the nominal speed is reachable if accelerating
+  // from a safe speed (in consideration of jerking from zero speed).
+  BLOCK_BIT_NOMINAL_LENGTH,
+
+  // The block is segment 2+ of a longer move
+  BLOCK_BIT_CONTINUED,
+
+  // Sync the stepper counts from the block
+  BLOCK_BIT_SYNC_POSITION
+
+  // Direct stepping page
+  #if ENABLED(DIRECT_STEPPING)
+    , BLOCK_BIT_IS_PAGE
+  #endif
+};
+
+enum BlockFlag : char {
+    BLOCK_FLAG_RECALCULATE          = _BV(BLOCK_BIT_RECALCULATE)
+  , BLOCK_FLAG_NOMINAL_LENGTH       = _BV(BLOCK_BIT_NOMINAL_LENGTH)
+  , BLOCK_FLAG_CONTINUED            = _BV(BLOCK_BIT_CONTINUED)
+  , BLOCK_FLAG_SYNC_POSITION        = _BV(BLOCK_BIT_SYNC_POSITION)
+  #if ENABLED(DIRECT_STEPPING)
+    , BLOCK_FLAG_IS_PAGE            = _BV(BLOCK_BIT_IS_PAGE)
+  #endif
+};
+
+#if ENABLED(LASER_POWER_INLINE)
+
+  typedef struct {
+    bool isPlanned:1;
+    bool isEnabled:1;
+    bool dir:1;
+    bool Reserved:6;
+  } power_status_t;
+
+  typedef struct {
+    power_status_t status;    // See planner settings for meaning
+    uint8_t power;            // Ditto; When in trapezoid mode this is nominal power
+    #if ENABLED(LASER_POWER_INLINE_TRAPEZOID)
+      uint8_t   power_entry;  // Entry power for the laser
+      #if DISABLED(LASER_POWER_INLINE_TRAPEZOID_CONT)
+        uint8_t   power_exit; // Exit power for the laser
+        uint32_t  entry_per,  // Steps per power increment (to avoid floats in stepper calcs)
+                  exit_per;   // Steps per power decrement
+      #endif
+    #endif
+  } block_laser_t;
+
+#endif
+
+/**
+ * struct block_t
+ *
+ * A single entry in the planner buffer.
+ * Tracks linear movement over multiple axes.
+ *
+ * The "nominal" values are as-specified by gcode, and
+ * may never actually be reached due to acceleration limits.
+ */
+typedef struct block_t {
+
+  volatile uint8_t flag;                    // Block flags (See BlockFlag enum above) - Modified by ISR and main thread!
+
+  // Fields used by the motion planner to manage acceleration
+  float nominal_speed_sqr,                  // The nominal speed for this block in (mm/sec)^2
+        entry_speed_sqr,                    // Entry speed at previous-current junction in (mm/sec)^2
+        max_entry_speed_sqr,                // Maximum allowable junction entry speed in (mm/sec)^2
+        millimeters,                        // The total travel of this block in mm
+        acceleration;                       // acceleration mm/sec^2
+
+  union {
+    abce_ulong_t steps;                     // Step count along each axis
+    abce_long_t position;                   // New position to force when this sync block is executed
+  };
+  uint32_t step_event_count;                // The number of step events required to complete this block
+
+  #if HAS_MULTI_EXTRUDER
+    uint8_t extruder;                       // The extruder to move (if E move)
+  #else
+    static constexpr uint8_t extruder = 0;
+  #endif
+
+  TERN_(MIXING_EXTRUDER, MIXER_BLOCK_FIELD); // Normalized color for the mixing steppers
+
+  // Settings for the trapezoid generator
+  uint32_t accelerate_until,                // The index of the step event on which to stop acceleration
+           decelerate_after;                // The index of the step event on which to start decelerating
+
+  #if ENABLED(S_CURVE_ACCELERATION)
+    uint32_t cruise_rate,                   // The actual cruise rate to use, between end of the acceleration phase and start of deceleration phase
+             acceleration_time,             // Acceleration time and deceleration time in STEP timer counts
+             deceleration_time,
+             acceleration_time_inverse,     // Inverse of acceleration and deceleration periods, expressed as integer. Scale depends on CPU being used
+             deceleration_time_inverse;
+  #else
+    uint32_t acceleration_rate;             // The acceleration rate used for acceleration calculation
+  #endif
+
+  uint8_t direction_bits;                   // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h)
+
+  // Advance extrusion
+  #if ENABLED(LIN_ADVANCE)
+    bool use_advance_lead;
+    uint16_t advance_speed,                 // STEP timer value for extruder speed offset ISR
+             max_adv_steps,                 // max. advance steps to get cruising speed pressure (not always nominal_speed!)
+             final_adv_steps;               // advance steps due to exit speed
+    float e_D_ratio;
+  #endif
+
+  uint32_t nominal_rate,                    // The nominal step rate for this block in step_events/sec
+           initial_rate,                    // The jerk-adjusted step rate at start of block
+           final_rate,                      // The minimal rate at exit
+           acceleration_steps_per_s2;       // acceleration steps/sec^2
+
+  #if ENABLED(DIRECT_STEPPING)
+    page_idx_t page_idx;                    // Page index used for direct stepping
+  #endif
+
+  #if HAS_CUTTER
+    cutter_power_t cutter_power;            // Power level for Spindle, Laser, etc.
+  #endif
+
+  #if HAS_FAN
+    uint8_t fan_speed[FAN_COUNT];
+  #endif
+
+  #if ENABLED(BARICUDA)
+    uint8_t valve_pressure, e_to_p_pressure;
+  #endif
+
+  #if HAS_WIRED_LCD
+    uint32_t segment_time_us;
+  #endif
+
+  #if ENABLED(POWER_LOSS_RECOVERY)
+    uint32_t sdpos;
+  #endif
+
+  #if ENABLED(LASER_POWER_INLINE)
+    block_laser_t laser;
+  #endif
+
+} block_t;
+
+#if ANY(LIN_ADVANCE, SCARA_FEEDRATE_SCALING, GRADIENT_MIX, LCD_SHOW_E_TOTAL)
+  #define HAS_POSITION_FLOAT 1
+#endif
+
+#define BLOCK_MOD(n) ((n)&(BLOCK_BUFFER_SIZE-1))
+
+#if ENABLED(LASER_POWER_INLINE)
+  typedef struct {
+    /**
+     * Laser status flags
+     */
+    power_status_t status;
+    /**
+     * Laser power: 0 or 255 in case of PWM-less laser,
+     * or the OCR (oscillator count register) value;
+     *
+     * Using OCR instead of raw power, because it avoids
+     * floating point operations during the move loop.
+     */
+    uint8_t power;
+  } laser_state_t;
+#endif
+
+typedef struct {
+   uint32_t max_acceleration_mm_per_s2[XYZE_N], // (mm/s^2) M201 XYZE
+            min_segment_time_us;                // (µs) M205 B
+      float axis_steps_per_mm[XYZE_N];          // (steps) M92 XYZE - Steps per millimeter
+ feedRate_t max_feedrate_mm_s[XYZE_N];          // (mm/s) M203 XYZE - Max speeds
+      float acceleration,                       // (mm/s^2) M204 S - Normal acceleration. DEFAULT ACCELERATION for all printing moves.
+            retract_acceleration,               // (mm/s^2) M204 R - Retract acceleration. Filament pull-back and push-forward while standing still in the other axes
+            travel_acceleration;                // (mm/s^2) M204 T - Travel acceleration. DEFAULT ACCELERATION for all NON printing moves.
+ feedRate_t min_feedrate_mm_s,                  // (mm/s) M205 S - Minimum linear feedrate
+            min_travel_feedrate_mm_s;           // (mm/s) M205 T - Minimum travel feedrate
+} planner_settings_t;
+
+#if DISABLED(SKEW_CORRECTION)
+  #define XY_SKEW_FACTOR 0
+  #define XZ_SKEW_FACTOR 0
+  #define YZ_SKEW_FACTOR 0
+#endif
+
+typedef struct {
+  #if ENABLED(SKEW_CORRECTION_GCODE)
+    float xy;
+    #if ENABLED(SKEW_CORRECTION_FOR_Z)
+      float xz, yz;
+    #else
+      const float xz = XZ_SKEW_FACTOR, yz = YZ_SKEW_FACTOR;
+    #endif
+  #else
+    const float xy = XY_SKEW_FACTOR,
+                xz = XZ_SKEW_FACTOR, yz = YZ_SKEW_FACTOR;
+  #endif
+} skew_factor_t;
+
+#if ENABLED(DISABLE_INACTIVE_EXTRUDER)
+  typedef IF<(BLOCK_BUFFER_SIZE > 64), uint16_t, uint8_t>::type last_move_t;
+#endif
+
+class Planner {
+  public:
+
+    /**
+     * The move buffer, calculated in stepper steps
+     *
+     * block_buffer is a ring buffer...
+     *
+     *             head,tail : indexes for write,read
+     *            head==tail : the buffer is empty
+     *            head!=tail : blocks are in the buffer
+     *   head==(tail-1)%size : the buffer is full
+     *
+     *  Writer of head is Planner::buffer_segment().
+     *  Reader of tail is Stepper::isr(). Always consider tail busy / read-only
+     */
+    static block_t block_buffer[BLOCK_BUFFER_SIZE];
+    static volatile uint8_t block_buffer_head,      // Index of the next block to be pushed
+                            block_buffer_nonbusy,   // Index of the first non busy block
+                            block_buffer_planned,   // Index of the optimally planned block
+                            block_buffer_tail;      // Index of the busy block, if any
+    static uint16_t cleaning_buffer_counter;        // A counter to disable queuing of blocks
+    static uint8_t delay_before_delivering;         // This counter delays delivery of blocks when queue becomes empty to allow the opportunity of merging blocks
+
+
+    #if ENABLED(DISTINCT_E_FACTORS)
+      static uint8_t last_extruder;                 // Respond to extruder change
+    #endif
+
+    #if ENABLED(DIRECT_STEPPING)
+      static uint32_t last_page_step_rate;          // Last page step rate given
+      static xyze_bool_t last_page_dir;             // Last page direction given
+    #endif
+
+    #if EXTRUDERS
+      static int16_t flow_percentage[EXTRUDERS];    // Extrusion factor for each extruder
+      static float e_factor[EXTRUDERS];             // The flow percentage and volumetric multiplier combine to scale E movement
+    #endif
+
+    #if DISABLED(NO_VOLUMETRICS)
+      static float filament_size[EXTRUDERS],          // diameter of filament (in millimeters), typically around 1.75 or 2.85, 0 disables the volumetric calculations for the extruder
+                   volumetric_area_nominal,           // Nominal cross-sectional area
+                   volumetric_multiplier[EXTRUDERS];  // Reciprocal of cross-sectional area of filament (in mm^2). Pre-calculated to reduce computation in the planner
+                                                      // May be auto-adjusted by a filament width sensor
+    #endif
+
+    #if ENABLED(VOLUMETRIC_EXTRUDER_LIMIT)
+      static float volumetric_extruder_limit[EXTRUDERS],          // Maximum mm^3/sec the extruder can handle
+                   volumetric_extruder_feedrate_limit[EXTRUDERS]; // Feedrate limit (mm/s) calculated from volume limit
+    #endif
+
+    static planner_settings_t settings;
+
+    #if ENABLED(LASER_POWER_INLINE)
+      static laser_state_t laser_inline;
+    #endif
+
+    static uint32_t max_acceleration_steps_per_s2[XYZE_N]; // (steps/s^2) Derived from mm_per_s2
+    static float steps_to_mm[XYZE_N];           // Millimeters per step
+
+    #if HAS_JUNCTION_DEVIATION
+      static float junction_deviation_mm;       // (mm) M205 J
+      #if HAS_LINEAR_E_JERK
+        static float max_e_jerk[DISTINCT_E];    // Calculated from junction_deviation_mm
+      #endif
+    #endif
+
+    #if HAS_CLASSIC_JERK
+      // (mm/s^2) M205 XYZ(E) - The largest speed change requiring no acceleration.
+      static TERN(HAS_LINEAR_E_JERK, xyz_pos_t, xyze_pos_t) max_jerk;
+    #endif
+
+    #if HAS_LEVELING
+      static bool leveling_active;          // Flag that bed leveling is enabled
+      #if ABL_PLANAR
+        static matrix_3x3 bed_level_matrix; // Transform to compensate for bed level
+      #endif
+      #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
+        static float z_fade_height, inverse_z_fade_height;
+      #endif
+    #else
+      static constexpr bool leveling_active = false;
+    #endif
+
+    #if ENABLED(LIN_ADVANCE)
+      static float extruder_advance_K[EXTRUDERS];
+    #endif
+
+    /**
+     * The current position of the tool in absolute steps
+     * Recalculated if any axis_steps_per_mm are changed by gcode
+     */
+    static xyze_long_t position;
+
+    #if HAS_POSITION_FLOAT
+      static xyze_pos_t position_float;
+    #endif
+
+    #if IS_KINEMATIC
+      static xyze_pos_t position_cart;
+    #endif
+
+    static skew_factor_t skew_factor;
+
+    #if ENABLED(SD_ABORT_ON_ENDSTOP_HIT)
+      static bool abort_on_endstop_hit;
+    #endif
+    #ifdef XY_FREQUENCY_LIMIT
+      static int8_t xy_freq_limit_hz;         // Minimum XY frequency setting
+      static float xy_freq_min_speed_factor;  // Minimum speed factor setting
+      static int32_t xy_freq_min_interval_us; // Minimum segment time based on xy_freq_limit_hz
+      static inline void refresh_frequency_limit() {
+        //xy_freq_min_interval_us = xy_freq_limit_hz ?: LROUND(1000000.0f / xy_freq_limit_hz);
+        if (xy_freq_limit_hz)
+          xy_freq_min_interval_us = LROUND(1000000.0f / xy_freq_limit_hz);
+      }
+      static inline void set_min_speed_factor_u8(const uint8_t v255) {
+        xy_freq_min_speed_factor = float(ui8_to_percent(v255)) / 100;
+      }
+      static inline void set_frequency_limit(const uint8_t hz) {
+        xy_freq_limit_hz = constrain(hz, 0, 100);
+        refresh_frequency_limit();
+      }
+    #endif
+
+  private:
+
+    /**
+     * Speed of previous path line segment
+     */
+    static xyze_float_t previous_speed;
+
+    /**
+     * Nominal speed of previous path line segment (mm/s)^2
+     */
+    static float previous_nominal_speed_sqr;
+
+    /**
+     * Limit where 64bit math is necessary for acceleration calculation
+     */
+    static uint32_t cutoff_long;
+
+    #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
+      static float last_fade_z;
+    #endif
+
+    #if ENABLED(DISABLE_INACTIVE_EXTRUDER)
+       // Counters to manage disabling inactive extruders
+      static last_move_t g_uc_extruder_last_move[EXTRUDERS];
+    #endif
+
+    #if HAS_WIRED_LCD
+      volatile static uint32_t block_buffer_runtime_us; // Theoretical block buffer runtime in µs
+    #endif
+
+  public:
+
+    /**
+     * Instance Methods
+     */
+
+    Planner();
+
+    void init();
+
+    /**
+     * Static (class) Methods
+     */
+
+    static void reset_acceleration_rates();
+    static void refresh_positioning();
+    static void set_max_acceleration(const uint8_t axis, float targetValue);
+    static void set_max_feedrate(const uint8_t axis, float targetValue);
+    static void set_max_jerk(const AxisEnum axis, float targetValue);
+
+
+    #if EXTRUDERS
+      FORCE_INLINE static void refresh_e_factor(const uint8_t e) {
+        e_factor[e] = flow_percentage[e] * 0.01f * TERN(NO_VOLUMETRICS, 1.0f, volumetric_multiplier[e]);
+      }
+
+      static inline void set_flow(const uint8_t e, const int16_t flow) {
+        flow_percentage[e] = flow;
+        refresh_e_factor(e);
+      }
+
+    #endif
+
+    // Manage fans, paste pressure, etc.
+    static void check_axes_activity();
+
+    #if ENABLED(FILAMENT_WIDTH_SENSOR)
+      void apply_filament_width_sensor(const int8_t encoded_ratio);
+
+      static inline float volumetric_percent(const bool vol) {
+        return 100.0f * (vol
+            ? volumetric_area_nominal / volumetric_multiplier[FILAMENT_SENSOR_EXTRUDER_NUM]
+            : volumetric_multiplier[FILAMENT_SENSOR_EXTRUDER_NUM]
+        );
+      }
+    #endif
+
+    #if DISABLED(NO_VOLUMETRICS)
+
+      // Update multipliers based on new diameter measurements
+      static void calculate_volumetric_multipliers();
+
+      #if ENABLED(VOLUMETRIC_EXTRUDER_LIMIT)
+        // Update pre calculated extruder feedrate limits based on volumetric values
+        static void calculate_volumetric_extruder_limit(const uint8_t e);
+        static void calculate_volumetric_extruder_limits();
+      #endif
+
+      FORCE_INLINE static void set_filament_size(const uint8_t e, const float &v) {
+        filament_size[e] = v;
+        if (v > 0) volumetric_area_nominal = CIRCLE_AREA(v * 0.5); //TODO: should it be per extruder
+        // make sure all extruders have some sane value for the filament size
+        LOOP_L_N(i, COUNT(filament_size))
+          if (!filament_size[i]) filament_size[i] = DEFAULT_NOMINAL_FILAMENT_DIA;
+      }
+
+    #endif
+
+    #if ENABLED(VOLUMETRIC_EXTRUDER_LIMIT)
+      FORCE_INLINE static void set_volumetric_extruder_limit(const uint8_t e, const float &v) {
+        volumetric_extruder_limit[e] = v;
+        calculate_volumetric_extruder_limit(e);
+      }
+    #endif
+
+    #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
+
+      /**
+       * Get the Z leveling fade factor based on the given Z height,
+       * re-calculating only when needed.
+       *
+       *  Returns 1.0 if planner.z_fade_height is 0.0.
+       *  Returns 0.0 if Z is past the specified 'Fade Height'.
+       */
+      static inline float fade_scaling_factor_for_z(const float &rz) {
+        static float z_fade_factor = 1;
+        if (!z_fade_height) return 1;
+        if (rz >= z_fade_height) return 0;
+        if (last_fade_z != rz) {
+          last_fade_z = rz;
+          z_fade_factor = 1 - rz * inverse_z_fade_height;
+        }
+        return z_fade_factor;
+      }
+
+      FORCE_INLINE static void force_fade_recalc() { last_fade_z = -999.999f; }
+
+      FORCE_INLINE static void set_z_fade_height(const float &zfh) {
+        z_fade_height = zfh > 0 ? zfh : 0;
+        inverse_z_fade_height = RECIPROCAL(z_fade_height);
+        force_fade_recalc();
+      }
+
+      FORCE_INLINE static bool leveling_active_at_z(const float &rz) {
+        return !z_fade_height || rz < z_fade_height;
+      }
+
+    #else
+
+      FORCE_INLINE static float fade_scaling_factor_for_z(const float&) { return 1; }
+
+      FORCE_INLINE static bool leveling_active_at_z(const float&) { return true; }
+
+    #endif
+
+    #if ENABLED(SKEW_CORRECTION)
+
+      FORCE_INLINE static void skew(float &cx, float &cy, const float &cz) {
+        if (WITHIN(cx, X_MIN_POS + 1, X_MAX_POS) && WITHIN(cy, Y_MIN_POS + 1, Y_MAX_POS)) {
+          const float sx = cx - cy * skew_factor.xy - cz * (skew_factor.xz - (skew_factor.xy * skew_factor.yz)),
+                      sy = cy - cz * skew_factor.yz;
+          if (WITHIN(sx, X_MIN_POS, X_MAX_POS) && WITHIN(sy, Y_MIN_POS, Y_MAX_POS)) {
+            cx = sx; cy = sy;
+          }
+        }
+      }
+      FORCE_INLINE static void skew(xyz_pos_t &raw) { skew(raw.x, raw.y, raw.z); }
+
+      FORCE_INLINE static void unskew(float &cx, float &cy, const float &cz) {
+        if (WITHIN(cx, X_MIN_POS, X_MAX_POS) && WITHIN(cy, Y_MIN_POS, Y_MAX_POS)) {
+          const float sx = cx + cy * skew_factor.xy + cz * skew_factor.xz,
+                      sy = cy + cz * skew_factor.yz;
+          if (WITHIN(sx, X_MIN_POS, X_MAX_POS) && WITHIN(sy, Y_MIN_POS, Y_MAX_POS)) {
+            cx = sx; cy = sy;
+          }
+        }
+      }
+      FORCE_INLINE static void unskew(xyz_pos_t &raw) { unskew(raw.x, raw.y, raw.z); }
+
+    #endif // SKEW_CORRECTION
+
+    #if HAS_LEVELING
+      /**
+       * Apply leveling to transform a cartesian position
+       * as it will be given to the planner and steppers.
+       */
+      static void apply_leveling(xyz_pos_t &raw);
+      static void unapply_leveling(xyz_pos_t &raw);
+      FORCE_INLINE static void force_unapply_leveling(xyz_pos_t &raw) {
+        leveling_active = true;
+        unapply_leveling(raw);
+        leveling_active = false;
+      }
+    #else
+      FORCE_INLINE static void apply_leveling(xyz_pos_t&) {}
+      FORCE_INLINE static void unapply_leveling(xyz_pos_t&) {}
+    #endif
+
+    #if ENABLED(FWRETRACT)
+      static void apply_retract(float &rz, float &e);
+      FORCE_INLINE static void apply_retract(xyze_pos_t &raw) { apply_retract(raw.z, raw.e); }
+      static void unapply_retract(float &rz, float &e);
+      FORCE_INLINE static void unapply_retract(xyze_pos_t &raw) { unapply_retract(raw.z, raw.e); }
+    #endif
+
+    #if HAS_POSITION_MODIFIERS
+      FORCE_INLINE static void apply_modifiers(xyze_pos_t &pos, bool leveling=ENABLED(PLANNER_LEVELING)) {
+        TERN_(SKEW_CORRECTION, skew(pos));
+        if (leveling) apply_leveling(pos);
+        TERN_(FWRETRACT, apply_retract(pos));
+      }
+
+      FORCE_INLINE static void unapply_modifiers(xyze_pos_t &pos, bool leveling=ENABLED(PLANNER_LEVELING)) {
+        TERN_(FWRETRACT, unapply_retract(pos));
+        if (leveling) unapply_leveling(pos);
+        TERN_(SKEW_CORRECTION, unskew(pos));
+      }
+    #endif // HAS_POSITION_MODIFIERS
+
+    // Number of moves currently in the planner including the busy block, if any
+    FORCE_INLINE static uint8_t movesplanned() { return BLOCK_MOD(block_buffer_head - block_buffer_tail); }
+
+    // Number of nonbusy moves currently in the planner
+    FORCE_INLINE static uint8_t nonbusy_movesplanned() { return BLOCK_MOD(block_buffer_head - block_buffer_nonbusy); }
+
+    // Remove all blocks from the buffer
+    FORCE_INLINE static void clear_block_buffer() { block_buffer_nonbusy = block_buffer_planned = block_buffer_head = block_buffer_tail = 0; }
+
+    // Check if movement queue is full
+    FORCE_INLINE static bool is_full() { return block_buffer_tail == next_block_index(block_buffer_head); }
+
+    // Get count of movement slots free
+    FORCE_INLINE static uint8_t moves_free() { return BLOCK_BUFFER_SIZE - 1 - movesplanned(); }
+
+    /**
+     * Planner::get_next_free_block
+     *
+     * - Get the next head indices (passed by reference)
+     * - Wait for the number of spaces to open up in the planner
+     * - Return the first head block
+     */
+    FORCE_INLINE static block_t* get_next_free_block(uint8_t &next_buffer_head, const uint8_t count=1) {
+
+      // Wait until there are enough slots free
+      while (moves_free() < count) { idle(); }
+
+      // Return the first available block
+      next_buffer_head = next_block_index(block_buffer_head);
+      return &block_buffer[block_buffer_head];
+    }
+
+    /**
+     * Planner::_buffer_steps
+     *
+     * Add a new linear movement to the buffer (in terms of steps).
+     *
+     *  target      - target position in steps units
+     *  fr_mm_s     - (target) speed of the move
+     *  extruder    - target extruder
+     *  millimeters - the length of the movement, if known
+     *
+     * Returns true if movement was buffered, false otherwise
+     */
+    static bool _buffer_steps(const xyze_long_t &target
+      #if HAS_POSITION_FLOAT
+        , const xyze_pos_t &target_float
+      #endif
+      #if HAS_DIST_MM_ARG
+        , const xyze_float_t &cart_dist_mm
+      #endif
+      , feedRate_t fr_mm_s, const uint8_t extruder, const float &millimeters=0.0
+    );
+
+    /**
+     * Planner::_populate_block
+     *
+     * Fills a new linear movement in the block (in terms of steps).
+     *
+     *  target      - target position in steps units
+     *  fr_mm_s     - (target) speed of the move
+     *  extruder    - target extruder
+     *  millimeters - the length of the movement, if known
+     *
+     * Returns true is movement is acceptable, false otherwise
+     */
+    static bool _populate_block(block_t * const block, bool split_move,
+        const xyze_long_t &target
+      #if HAS_POSITION_FLOAT
+        , const xyze_pos_t &target_float
+      #endif
+      #if HAS_DIST_MM_ARG
+        , const xyze_float_t &cart_dist_mm
+      #endif
+      , feedRate_t fr_mm_s, const uint8_t extruder, const float &millimeters=0.0
+    );
+
+    /**
+     * Planner::buffer_sync_block
+     * Add a block to the buffer that just updates the position
+     */
+    static void buffer_sync_block();
+
+  #if IS_KINEMATIC
+    private:
+
+      // Allow do_homing_move to access internal functions, such as buffer_segment.
+      friend void do_homing_move(const AxisEnum, const float, const feedRate_t, const bool);
+  #endif
+
+    /**
+     * Planner::buffer_segment
+     *
+     * Add a new linear movement to the buffer in axis units.
+     *
+     * Leveling and kinematics should be applied ahead of calling this.
+     *
+     *  a,b,c,e     - target positions in mm and/or degrees
+     *  fr_mm_s     - (target) speed of the move
+     *  extruder    - target extruder
+     *  millimeters - the length of the movement, if known
+     */
+    static bool buffer_segment(const float &a, const float &b, const float &c, const float &e
+      #if HAS_DIST_MM_ARG
+        , const xyze_float_t &cart_dist_mm
+      #endif
+      , const feedRate_t &fr_mm_s, const uint8_t extruder, const float &millimeters=0.0
+    );
+
+    FORCE_INLINE static bool buffer_segment(abce_pos_t &abce
+      #if HAS_DIST_MM_ARG
+        , const xyze_float_t &cart_dist_mm
+      #endif
+      , const feedRate_t &fr_mm_s, const uint8_t extruder, const float &millimeters=0.0
+    ) {
+      return buffer_segment(abce.a, abce.b, abce.c, abce.e
+        #if HAS_DIST_MM_ARG
+          , cart_dist_mm
+        #endif
+        , fr_mm_s, extruder, millimeters);
+    }
+
+  public:
+
+    /**
+     * Add a new linear movement to the buffer.
+     * The target is cartesian. It's translated to
+     * delta/scara if needed.
+     *
+     *  rx,ry,rz,e   - target position in mm or degrees
+     *  fr_mm_s      - (target) speed of the move (mm/s)
+     *  extruder     - target extruder
+     *  millimeters  - the length of the movement, if known
+     *  inv_duration - the reciprocal if the duration of the movement, if known (kinematic only if feeedrate scaling is enabled)
+     */
+    static bool buffer_line(const float &rx, const float &ry, const float &rz, const float &e, const feedRate_t &fr_mm_s, const uint8_t extruder, const float millimeters=0.0
+      #if ENABLED(SCARA_FEEDRATE_SCALING)
+        , const float &inv_duration=0.0
+      #endif
+    );
+
+    FORCE_INLINE static bool buffer_line(const xyze_pos_t &cart, const feedRate_t &fr_mm_s, const uint8_t extruder, const float millimeters=0.0
+      #if ENABLED(SCARA_FEEDRATE_SCALING)
+        , const float &inv_duration=0.0
+      #endif
+    ) {
+      return buffer_line(cart.x, cart.y, cart.z, cart.e, fr_mm_s, extruder, millimeters
+        #if ENABLED(SCARA_FEEDRATE_SCALING)
+          , inv_duration
+        #endif
+      );
+    }
+
+    #if ENABLED(DIRECT_STEPPING)
+      static void buffer_page(const page_idx_t page_idx, const uint8_t extruder, const uint16_t num_steps);
+    #endif
+
+    /**
+     * Set the planner.position and individual stepper positions.
+     * Used by G92, G28, G29, and other procedures.
+     *
+     * The supplied position is in the cartesian coordinate space and is
+     * translated in to machine space as needed. Modifiers such as leveling
+     * and skew are also applied.
+     *
+     * Multiplies by axis_steps_per_mm[] and does necessary conversion
+     * for COREXY / COREXZ / COREYZ to set the corresponding stepper positions.
+     *
+     * Clears previous speed values.
+     */
+    static void set_position_mm(const float &rx, const float &ry, const float &rz, const float &e);
+    FORCE_INLINE static void set_position_mm(const xyze_pos_t &cart) { set_position_mm(cart.x, cart.y, cart.z, cart.e); }
+    static void set_e_position_mm(const float &e);
+
+    /**
+     * Set the planner.position and individual stepper positions.
+     *
+     * The supplied position is in machine space, and no additional
+     * conversions are applied.
+     */
+    static void set_machine_position_mm(const float &a, const float &b, const float &c, const float &e);
+    FORCE_INLINE static void set_machine_position_mm(const abce_pos_t &abce) { set_machine_position_mm(abce.a, abce.b, abce.c, abce.e); }
+
+    /**
+     * Get an axis position according to stepper position(s)
+     * For CORE machines apply translation from ABC to XYZ.
+     */
+    static float get_axis_position_mm(const AxisEnum axis);
+
+    static inline abce_pos_t get_axis_positions_mm() {
+      const abce_pos_t out = {
+        get_axis_position_mm(A_AXIS),
+        get_axis_position_mm(B_AXIS),
+        get_axis_position_mm(C_AXIS),
+        get_axis_position_mm(E_AXIS)
+      };
+      return out;
+    }
+
+    // SCARA AB axes are in degrees, not mm
+    #if IS_SCARA
+      FORCE_INLINE static float get_axis_position_degrees(const AxisEnum axis) { return get_axis_position_mm(axis); }
+    #endif
+
+    // Called to force a quick stop of the machine (for example, when
+    // a Full Shutdown is required, or when endstops are hit)
+    static void quick_stop();
+
+    // Called when an endstop is triggered. Causes the machine to stop inmediately
+    static void endstop_triggered(const AxisEnum axis);
+
+    // Triggered position of an axis in mm (not core-savvy)
+    static float triggered_position_mm(const AxisEnum axis);
+
+    // Block until all buffered steps are executed / cleaned
+    static void synchronize();
+
+    // Wait for moves to finish and disable all steppers
+    static void finish_and_disable();
+
+    // Periodic tick to handle cleaning timeouts
+    // Called from the Temperature ISR at ~1kHz
+    static void tick() {
+      if (cleaning_buffer_counter) --cleaning_buffer_counter;
+    }
+
+    /**
+     * Does the buffer have any blocks queued?
+     */
+    FORCE_INLINE static bool has_blocks_queued() { return (block_buffer_head != block_buffer_tail); }
+
+    /**
+     * Get the current block for processing
+     * and mark the block as busy.
+     * Return nullptr if the buffer is empty
+     * or if there is a first-block delay.
+     *
+     * WARNING: Called from Stepper ISR context!
+     */
+    static block_t* get_current_block();
+
+    /**
+     * "Release" the current block so its slot can be reused.
+     * Called when the current block is no longer needed.
+     */
+    FORCE_INLINE static void release_current_block() {
+      if (has_blocks_queued())
+        block_buffer_tail = next_block_index(block_buffer_tail);
+    }
+
+    #if HAS_WIRED_LCD
+      static uint16_t block_buffer_runtime();
+      static void clear_block_buffer_runtime();
+    #endif
+
+    #if ENABLED(AUTOTEMP)
+      static float autotemp_min, autotemp_max, autotemp_factor;
+      static bool autotemp_enabled;
+      static void getHighESpeed();
+      static void autotemp_M104_M109();
+      static void autotemp_update();
+    #endif
+
+    #if HAS_LINEAR_E_JERK
+      FORCE_INLINE static void recalculate_max_e_jerk() {
+        const float prop = junction_deviation_mm * SQRT(0.5) / (1.0f - SQRT(0.5));
+        LOOP_L_N(i, EXTRUDERS)
+          max_e_jerk[E_INDEX_N(i)] = SQRT(prop * settings.max_acceleration_mm_per_s2[E_INDEX_N(i)]);
+      }
+    #endif
+
+  private:
+
+    /**
+     * Get the index of the next / previous block in the ring buffer
+     */
+    static constexpr uint8_t next_block_index(const uint8_t block_index) { return BLOCK_MOD(block_index + 1); }
+    static constexpr uint8_t prev_block_index(const uint8_t block_index) { return BLOCK_MOD(block_index - 1); }
+
+    /**
+     * Calculate the distance (not time) it takes to accelerate
+     * from initial_rate to target_rate using the given acceleration:
+     */
+    static float estimate_acceleration_distance(const float &initial_rate, const float &target_rate, const float &accel) {
+      if (accel == 0) return 0; // accel was 0, set acceleration distance to 0
+      return (sq(target_rate) - sq(initial_rate)) / (accel * 2);
+    }
+
+    /**
+     * Return the point at which you must start braking (at the rate of -'accel') if
+     * you start at 'initial_rate', accelerate (until reaching the point), and want to end at
+     * 'final_rate' after traveling 'distance'.
+     *
+     * This is used to compute the intersection point between acceleration and deceleration
+     * in cases where the "trapezoid" has no plateau (i.e., never reaches maximum speed)
+     */
+    static float intersection_distance(const float &initial_rate, const float &final_rate, const float &accel, const float &distance) {
+      if (accel == 0) return 0; // accel was 0, set intersection distance to 0
+      return (accel * 2 * distance - sq(initial_rate) + sq(final_rate)) / (accel * 4);
+    }
+
+    /**
+     * Calculate the maximum allowable speed squared at this point, in order
+     * to reach 'target_velocity_sqr' using 'acceleration' within a given
+     * 'distance'.
+     */
+    static float max_allowable_speed_sqr(const float &accel, const float &target_velocity_sqr, const float &distance) {
+      return target_velocity_sqr - 2 * accel * distance;
+    }
+
+    #if ENABLED(S_CURVE_ACCELERATION)
+      /**
+       * Calculate the speed reached given initial speed, acceleration and distance
+       */
+      static float final_speed(const float &initial_velocity, const float &accel, const float &distance) {
+        return SQRT(sq(initial_velocity) + 2 * accel * distance);
+      }
+    #endif
+
+    static void calculate_trapezoid_for_block(block_t* const block, const float &entry_factor, const float &exit_factor);
+
+    static void reverse_pass_kernel(block_t* const current, const block_t * const next);
+    static void forward_pass_kernel(const block_t * const previous, block_t* const current, uint8_t block_index);
+
+    static void reverse_pass();
+    static void forward_pass();
+
+    static void recalculate_trapezoids();
+
+    static void recalculate();
+
+    #if HAS_JUNCTION_DEVIATION
+
+      FORCE_INLINE static void normalize_junction_vector(xyze_float_t &vector) {
+        float magnitude_sq = 0;
+        LOOP_XYZE(idx) if (vector[idx]) magnitude_sq += sq(vector[idx]);
+        vector *= RSQRT(magnitude_sq);
+      }
+
+      FORCE_INLINE static float limit_value_by_axis_maximum(const float &max_value, xyze_float_t &unit_vec) {
+        float limit_value = max_value;
+        LOOP_XYZE(idx) {
+          if (unit_vec[idx]) {
+            if (limit_value * ABS(unit_vec[idx]) > settings.max_acceleration_mm_per_s2[idx])
+              limit_value = ABS(settings.max_acceleration_mm_per_s2[idx] / unit_vec[idx]);
+          }
+        }
+        return limit_value;
+      }
+
+    #endif // !CLASSIC_JERK
+};
+
+#define PLANNER_XY_FEEDRATE() _MIN(planner.settings.max_feedrate_mm_s[X_AXIS], planner.settings.max_feedrate_mm_s[Y_AXIS])
+
+extern Planner planner;
diff --git a/Marlin/src/module/planner_bezier.cpp b/Marlin/src/module/planner_bezier.cpp
new file mode 100644
index 0000000..02d878d
--- /dev/null
+++ b/Marlin/src/module/planner_bezier.cpp
@@ -0,0 +1,204 @@
+/**
+ * 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/>.
+ *
+ */
+
+/**
+ * planner_bezier.cpp
+ *
+ * Compute and buffer movement commands for bezier curves
+ */
+
+#include "../inc/MarlinConfig.h"
+
+#if ENABLED(BEZIER_CURVE_SUPPORT)
+
+#include "planner.h"
+#include "motion.h"
+#include "temperature.h"
+
+#include "../MarlinCore.h"
+#include "../gcode/queue.h"
+
+// See the meaning in the documentation of cubic_b_spline().
+#define MIN_STEP 0.002f
+#define MAX_STEP 0.1f
+#define SIGMA 0.1f
+
+// Compute the linear interpolation between two real numbers.
+static inline float interp(const float &a, const float &b, const float &t) { return (1 - t) * a + t * b; }
+
+/**
+ * Compute a Bézier curve using the De Casteljau's algorithm (see
+ * https://en.wikipedia.org/wiki/De_Casteljau%27s_algorithm), which is
+ * easy to code and has good numerical stability (very important,
+ * since Arudino works with limited precision real numbers).
+ */
+static inline float eval_bezier(const float &a, const float &b, const float &c, const float &d, const float &t) {
+  const float iab = interp(a, b, t),
+              ibc = interp(b, c, t),
+              icd = interp(c, d, t),
+              iabc = interp(iab, ibc, t),
+              ibcd = interp(ibc, icd, t);
+  return interp(iabc, ibcd, t);
+}
+
+/**
+ * We approximate Euclidean distance with the sum of the coordinates
+ * offset (so-called "norm 1"), which is quicker to compute.
+ */
+static inline float dist1(const float &x1, const float &y1, const float &x2, const float &y2) { return ABS(x1 - x2) + ABS(y1 - y2); }
+
+/**
+ * The algorithm for computing the step is loosely based on the one in Kig
+ * (See https://sources.debian.net/src/kig/4:15.08.3-1/misc/kigpainter.cpp/#L759)
+ * However, we do not use the stack.
+ *
+ * The algorithm goes as it follows: the parameters t runs from 0.0 to
+ * 1.0 describing the curve, which is evaluated by eval_bezier(). At
+ * each iteration we have to choose a step, i.e., the increment of the
+ * t variable. By default the step of the previous iteration is taken,
+ * and then it is enlarged or reduced depending on how straight the
+ * curve locally is. The step is always clamped between MIN_STEP/2 and
+ * 2*MAX_STEP. MAX_STEP is taken at the first iteration.
+ *
+ * For some t, the step value is considered acceptable if the curve in
+ * the interval [t, t+step] is sufficiently straight, i.e.,
+ * sufficiently close to linear interpolation. In practice the
+ * following test is performed: the distance between eval_bezier(...,
+ * t+step/2) is evaluated and compared with 0.5*(eval_bezier(...,
+ * t)+eval_bezier(..., t+step)). If it is smaller than SIGMA, then the
+ * step value is considered acceptable, otherwise it is not. The code
+ * seeks to find the larger step value which is considered acceptable.
+ *
+ * At every iteration the recorded step value is considered and then
+ * iteratively halved until it becomes acceptable. If it was already
+ * acceptable in the beginning (i.e., no halving were done), then
+ * maybe it was necessary to enlarge it; then it is iteratively
+ * doubled while it remains acceptable. The last acceptable value
+ * found is taken, provided that it is between MIN_STEP and MAX_STEP
+ * and does not bring t over 1.0.
+ *
+ * Caveat: this algorithm is not perfect, since it can happen that a
+ * step is considered acceptable even when the curve is not linear at
+ * all in the interval [t, t+step] (but its mid point coincides "by
+ * chance" with the midpoint according to the parametrization). This
+ * kind of glitches can be eliminated with proper first derivative
+ * estimates; however, given the improbability of such configurations,
+ * the mitigation offered by MIN_STEP and the small computational
+ * power available on Arduino, I think it is not wise to implement it.
+ */
+void cubic_b_spline(
+  const xyze_pos_t &position,       // current position
+  const xyze_pos_t &target,         // target position
+  const xy_pos_t (&offsets)[2],     // a pair of offsets
+  const feedRate_t &scaled_fr_mm_s, // mm/s scaled by feedrate %
+  const uint8_t extruder
+) {
+  // Absolute first and second control points are recovered.
+  const xy_pos_t first = position + offsets[0], second = target + offsets[1];
+
+  xyze_pos_t bez_target;
+  bez_target.set(position.x, position.y);
+  float step = MAX_STEP;
+
+  millis_t next_idle_ms = millis() + 200UL;
+
+  for (float t = 0; t < 1;) {
+
+    thermalManager.manage_heater();
+    millis_t now = millis();
+    if (ELAPSED(now, next_idle_ms)) {
+      next_idle_ms = now + 200UL;
+      idle();
+    }
+
+    // First try to reduce the step in order to make it sufficiently
+    // close to a linear interpolation.
+    bool did_reduce = false;
+    float new_t = t + step;
+    NOMORE(new_t, 1);
+    float new_pos0 = eval_bezier(position.x, first.x, second.x, target.x, new_t),
+          new_pos1 = eval_bezier(position.y, first.y, second.y, target.y, new_t);
+    for (;;) {
+      if (new_t - t < (MIN_STEP)) break;
+      const float candidate_t = 0.5f * (t + new_t),
+                  candidate_pos0 = eval_bezier(position.x, first.x, second.x, target.x, candidate_t),
+                  candidate_pos1 = eval_bezier(position.y, first.y, second.y, target.y, candidate_t),
+                  interp_pos0 = 0.5f * (bez_target.x + new_pos0),
+                  interp_pos1 = 0.5f * (bez_target.y + new_pos1);
+      if (dist1(candidate_pos0, candidate_pos1, interp_pos0, interp_pos1) <= (SIGMA)) break;
+      new_t = candidate_t;
+      new_pos0 = candidate_pos0;
+      new_pos1 = candidate_pos1;
+      did_reduce = true;
+    }
+
+    // If we did not reduce the step, maybe we should enlarge it.
+    if (!did_reduce) for (;;) {
+      if (new_t - t > MAX_STEP) break;
+      const float candidate_t = t + 2 * (new_t - t);
+      if (candidate_t >= 1) break;
+      const float candidate_pos0 = eval_bezier(position.x, first.x, second.x, target.x, candidate_t),
+                  candidate_pos1 = eval_bezier(position.y, first.y, second.y, target.y, candidate_t),
+                  interp_pos0 = 0.5f * (bez_target.x + candidate_pos0),
+                  interp_pos1 = 0.5f * (bez_target.y + candidate_pos1);
+      if (dist1(new_pos0, new_pos1, interp_pos0, interp_pos1) > (SIGMA)) break;
+      new_t = candidate_t;
+      new_pos0 = candidate_pos0;
+      new_pos1 = candidate_pos1;
+    }
+
+    // Check some postcondition; they are disabled in the actual
+    // Marlin build, but if you test the same code on a computer you
+    // may want to check they are respect.
+    /*
+      assert(new_t <= 1.0);
+      if (new_t < 1.0) {
+        assert(new_t - t >= (MIN_STEP) / 2.0);
+        assert(new_t - t <= (MAX_STEP) * 2.0);
+      }
+    */
+
+    step = new_t - t;
+    t = new_t;
+
+    // Compute and send new position
+    xyze_pos_t new_bez = {
+      new_pos0, new_pos1,
+      interp(position.z, target.z, t),   // FIXME. These two are wrong, since the parameter t is
+      interp(position.e, target.e, t)    // not linear in the distance.
+    };
+    apply_motion_limits(new_bez);
+    bez_target = new_bez;
+
+    #if HAS_LEVELING && !PLANNER_LEVELING
+      xyze_pos_t pos = bez_target;
+      planner.apply_leveling(pos);
+    #else
+      const xyze_pos_t &pos = bez_target;
+    #endif
+
+    if (!planner.buffer_line(pos, scaled_fr_mm_s, active_extruder, step))
+      break;
+  }
+}
+
+#endif // BEZIER_CURVE_SUPPORT
diff --git a/Marlin/src/module/planner_bezier.h b/Marlin/src/module/planner_bezier.h
new file mode 100644
index 0000000..72048c4
--- /dev/null
+++ b/Marlin/src/module/planner_bezier.h
@@ -0,0 +1,38 @@
+/**
+ * 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
+
+/**
+ * planner_bezier.h
+ *
+ * Compute and buffer movement commands for Bézier curves
+ */
+
+#include "../core/types.h"
+
+void cubic_b_spline(
+  const xyze_pos_t &position,       // current position
+  const xyze_pos_t &target,         // target position
+  const xy_pos_t (&offsets)[2],     // a pair of offsets
+  const feedRate_t &scaled_fr_mm_s, // mm/s scaled by feedrate %
+  const uint8_t extruder
+);
diff --git a/Marlin/src/module/printcounter.cpp b/Marlin/src/module/printcounter.cpp
new file mode 100644
index 0000000..45072c8
--- /dev/null
+++ b/Marlin/src/module/printcounter.cpp
@@ -0,0 +1,349 @@
+/**
+ * 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 "../inc/MarlinConfig.h"
+
+#if DISABLED(PRINTCOUNTER)
+
+#include "../libs/stopwatch.h"
+Stopwatch print_job_timer;      // Global Print Job Timer instance
+
+#else // PRINTCOUNTER
+
+#if ENABLED(EXTENSIBLE_UI)
+  #include "../lcd/extui/ui_api.h"
+#endif
+
+#include "printcounter.h"
+#include "../MarlinCore.h"
+#include "../HAL/shared/eeprom_api.h"
+
+#if HAS_BUZZER && SERVICE_WARNING_BUZZES > 0
+  #include "../libs/buzzer.h"
+#endif
+
+#if PRINTCOUNTER_SYNC
+  #include "../module/planner.h"
+#endif
+
+// Service intervals
+#if HAS_SERVICE_INTERVALS
+  #if SERVICE_INTERVAL_1 > 0
+    #define SERVICE_INTERVAL_SEC_1   (3600UL * SERVICE_INTERVAL_1)
+  #else
+    #define SERVICE_INTERVAL_SEC_1   (3600UL * 100)
+  #endif
+  #if SERVICE_INTERVAL_2 > 0
+    #define SERVICE_INTERVAL_SEC_2   (3600UL * SERVICE_INTERVAL_2)
+  #else
+    #define SERVICE_INTERVAL_SEC_2   (3600UL * 100)
+  #endif
+  #if SERVICE_INTERVAL_3 > 0
+    #define SERVICE_INTERVAL_SEC_3   (3600UL * SERVICE_INTERVAL_3)
+  #else
+    #define SERVICE_INTERVAL_SEC_3   (3600UL * 100)
+  #endif
+#endif
+
+PrintCounter print_job_timer;   // Global Print Job Timer instance
+
+printStatistics PrintCounter::data;
+
+const PrintCounter::eeprom_address_t PrintCounter::address = STATS_EEPROM_ADDRESS;
+
+millis_t PrintCounter::lastDuration;
+bool PrintCounter::loaded = false;
+
+millis_t PrintCounter::deltaDuration() {
+  TERN_(DEBUG_PRINTCOUNTER, debug(PSTR("deltaDuration")));
+  millis_t tmp = lastDuration;
+  lastDuration = duration();
+  return lastDuration - tmp;
+}
+
+void PrintCounter::incFilamentUsed(float const &amount) {
+  TERN_(DEBUG_PRINTCOUNTER, debug(PSTR("incFilamentUsed")));
+
+  // Refuses to update data if object is not loaded
+  if (!isLoaded()) return;
+
+  data.filamentUsed += amount; // mm
+}
+
+void PrintCounter::initStats() {
+  TERN_(DEBUG_PRINTCOUNTER, debug(PSTR("initStats")));
+
+  loaded = true;
+  data = { 0, 0, 0, 0, 0.0
+    #if HAS_SERVICE_INTERVALS
+      #if SERVICE_INTERVAL_1 > 0
+        , SERVICE_INTERVAL_SEC_1
+      #endif
+      #if SERVICE_INTERVAL_2 > 0
+        , SERVICE_INTERVAL_SEC_2
+      #endif
+      #if SERVICE_INTERVAL_3 > 0
+        , SERVICE_INTERVAL_SEC_3
+      #endif
+    #endif
+  };
+
+  saveStats();
+  persistentStore.access_start();
+  persistentStore.write_data(address, (uint8_t)0x16);
+  persistentStore.access_finish();
+}
+
+#if HAS_SERVICE_INTERVALS
+  inline void _print_divider() { SERIAL_ECHO_MSG("============================================="); }
+  inline bool _service_warn(const char * const msg) {
+    _print_divider();
+    SERIAL_ECHO_START();
+    serialprintPGM(msg);
+    SERIAL_ECHOLNPGM("!");
+    _print_divider();
+    return true;
+  }
+#endif
+
+void PrintCounter::loadStats() {
+  TERN_(DEBUG_PRINTCOUNTER, debug(PSTR("loadStats")));
+
+  // Check if the EEPROM block is initialized
+  uint8_t value = 0;
+  persistentStore.access_start();
+  persistentStore.read_data(address, &value, sizeof(uint8_t));
+  if (value != 0x16)
+    initStats();
+  else
+    persistentStore.read_data(address + sizeof(uint8_t), (uint8_t*)&data, sizeof(printStatistics));
+  persistentStore.access_finish();
+  loaded = true;
+
+  #if HAS_SERVICE_INTERVALS
+    bool doBuzz = false;
+    #if SERVICE_INTERVAL_1 > 0
+      if (data.nextService1 == 0) doBuzz = _service_warn(PSTR(" " SERVICE_NAME_1));
+    #endif
+    #if SERVICE_INTERVAL_2 > 0
+      if (data.nextService2 == 0) doBuzz = _service_warn(PSTR(" " SERVICE_NAME_2));
+    #endif
+    #if SERVICE_INTERVAL_3 > 0
+      if (data.nextService3 == 0) doBuzz = _service_warn(PSTR(" " SERVICE_NAME_3));
+    #endif
+    #if HAS_BUZZER && SERVICE_WARNING_BUZZES > 0
+      if (doBuzz) for (int i = 0; i < SERVICE_WARNING_BUZZES; i++) BUZZ(200, 404);
+    #else
+      UNUSED(doBuzz);
+    #endif
+  #endif // HAS_SERVICE_INTERVALS
+}
+
+void PrintCounter::saveStats() {
+  TERN_(DEBUG_PRINTCOUNTER, debug(PSTR("saveStats")));
+
+  // Refuses to save data if object is not loaded
+  if (!isLoaded()) return;
+
+  TERN_(PRINTCOUNTER_SYNC, planner.synchronize());
+
+  // Saves the struct to EEPROM
+  persistentStore.access_start();
+  persistentStore.write_data(address + sizeof(uint8_t), (uint8_t*)&data, sizeof(printStatistics));
+  persistentStore.access_finish();
+
+  TERN_(EXTENSIBLE_UI, ExtUI::onConfigurationStoreWritten(true));
+}
+
+#if HAS_SERVICE_INTERVALS
+  inline void _service_when(char buffer[], const char * const msg, const uint32_t when) {
+    SERIAL_ECHOPGM(STR_STATS);
+    serialprintPGM(msg);
+    SERIAL_ECHOLNPAIR(" in ", duration_t(when).toString(buffer));
+  }
+#endif
+
+void PrintCounter::showStats() {
+  char buffer[22];
+
+  SERIAL_ECHOPGM(STR_STATS);
+  SERIAL_ECHOLNPAIR(
+    "Prints: ", data.totalPrints,
+    ", Finished: ", data.finishedPrints,
+    ", Failed: ", data.totalPrints - data.finishedPrints
+                    - ((isRunning() || isPaused()) ? 1 : 0) // Remove 1 from failures with an active counter
+  );
+
+  SERIAL_ECHOPGM(STR_STATS);
+  duration_t elapsed = data.printTime;
+  elapsed.toString(buffer);
+  SERIAL_ECHOPAIR("Total time: ", buffer);
+  #if ENABLED(DEBUG_PRINTCOUNTER)
+    SERIAL_ECHOPAIR(" (", data.printTime);
+    SERIAL_CHAR(')');
+  #endif
+
+  elapsed = data.longestPrint;
+  elapsed.toString(buffer);
+  SERIAL_ECHOPAIR(", Longest job: ", buffer);
+  #if ENABLED(DEBUG_PRINTCOUNTER)
+    SERIAL_ECHOPAIR(" (", data.longestPrint);
+    SERIAL_CHAR(')');
+  #endif
+
+  SERIAL_ECHOPAIR("\n" STR_STATS "Filament used: ", data.filamentUsed / 1000);
+  SERIAL_CHAR('m');
+  SERIAL_EOL();
+
+  #if SERVICE_INTERVAL_1 > 0
+    _service_when(buffer, PSTR(SERVICE_NAME_1), data.nextService1);
+  #endif
+  #if SERVICE_INTERVAL_2 > 0
+    _service_when(buffer, PSTR(SERVICE_NAME_2), data.nextService2);
+  #endif
+  #if SERVICE_INTERVAL_3 > 0
+    _service_when(buffer, PSTR(SERVICE_NAME_3), data.nextService3);
+  #endif
+}
+
+void PrintCounter::tick() {
+  if (!isRunning()) return;
+
+  millis_t now = millis();
+
+  static millis_t update_next; // = 0
+  if (ELAPSED(now, update_next)) {
+    update_next = now + updateInterval;
+
+    TERN_(DEBUG_PRINTCOUNTER, debug(PSTR("tick")));
+
+    millis_t delta = deltaDuration();
+    data.printTime += delta;
+
+    #if SERVICE_INTERVAL_1 > 0
+      data.nextService1 -= _MIN(delta, data.nextService1);
+    #endif
+    #if SERVICE_INTERVAL_2 > 0
+      data.nextService2 -= _MIN(delta, data.nextService2);
+    #endif
+    #if SERVICE_INTERVAL_3 > 0
+      data.nextService3 -= _MIN(delta, data.nextService3);
+    #endif
+  }
+
+  #if PRINTCOUNTER_SAVE_INTERVAL > 0
+    static millis_t eeprom_next; // = 0
+    if (ELAPSED(now, eeprom_next)) {
+      eeprom_next = now + saveInterval;
+      saveStats();
+    }
+  #endif
+}
+
+// @Override
+bool PrintCounter::start() {
+  TERN_(DEBUG_PRINTCOUNTER, debug(PSTR("start")));
+
+  bool paused = isPaused();
+
+  if (super::start()) {
+    if (!paused) {
+      data.totalPrints++;
+      lastDuration = 0;
+    }
+    return true;
+  }
+
+  return false;
+}
+
+bool PrintCounter::_stop(const bool completed) {
+  TERN_(DEBUG_PRINTCOUNTER, debug(PSTR("stop")));
+
+  const bool did_stop = super::stop();
+  if (did_stop) {
+    data.printTime += deltaDuration();
+    if (completed) {
+      data.finishedPrints++;
+      if (duration() > data.longestPrint)
+        data.longestPrint = duration();
+    }
+  }
+  saveStats();
+  return did_stop;
+}
+
+// @Override
+void PrintCounter::reset() {
+  TERN_(DEBUG_PRINTCOUNTER, debug(PSTR("stop")));
+
+  super::reset();
+  lastDuration = 0;
+}
+
+#if HAS_SERVICE_INTERVALS
+
+  void PrintCounter::resetServiceInterval(const int index) {
+    switch (index) {
+      #if SERVICE_INTERVAL_1 > 0
+        case 1: data.nextService1 = SERVICE_INTERVAL_SEC_1;
+      #endif
+      #if SERVICE_INTERVAL_2 > 0
+        case 2: data.nextService2 = SERVICE_INTERVAL_SEC_2;
+      #endif
+      #if SERVICE_INTERVAL_3 > 0
+        case 3: data.nextService3 = SERVICE_INTERVAL_SEC_3;
+      #endif
+    }
+    saveStats();
+  }
+
+  bool PrintCounter::needsService(const int index) {
+    switch (index) {
+      #if SERVICE_INTERVAL_1 > 0
+        case 1: return data.nextService1 == 0;
+      #endif
+      #if SERVICE_INTERVAL_2 > 0
+        case 2: return data.nextService2 == 0;
+      #endif
+      #if SERVICE_INTERVAL_3 > 0
+        case 3: return data.nextService3 == 0;
+      #endif
+      default: return false;
+    }
+  }
+
+#endif // HAS_SERVICE_INTERVALS
+
+#if ENABLED(DEBUG_PRINTCOUNTER)
+
+  void PrintCounter::debug(const char func[]) {
+    if (DEBUGGING(INFO)) {
+      SERIAL_ECHOPGM("PrintCounter::");
+      serialprintPGM(func);
+      SERIAL_ECHOLNPGM("()");
+    }
+  }
+
+#endif
+
+#endif // PRINTCOUNTER
diff --git a/Marlin/src/module/printcounter.h b/Marlin/src/module/printcounter.h
new file mode 100644
index 0000000..4deae45
--- /dev/null
+++ b/Marlin/src/module/printcounter.h
@@ -0,0 +1,205 @@
+/**
+ * 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 "../libs/stopwatch.h"
+#include "../libs/duration_t.h"
+#include "../inc/MarlinConfig.h"
+
+// Print debug messages with M111 S2
+//#define DEBUG_PRINTCOUNTER
+
+// Round up I2C / SPI address to next page boundary (assuming 32 byte pages)
+#define STATS_EEPROM_ADDRESS TERN(USE_WIRED_EEPROM, 0x40, 0x32)
+
+struct printStatistics {    // 16 bytes
+  //const uint8_t magic;    // Magic header, it will always be 0x16
+  uint16_t totalPrints;     // Number of prints
+  uint16_t finishedPrints;  // Number of complete prints
+  uint32_t printTime;       // Accumulated printing time
+  uint32_t longestPrint;    // Longest successful print job
+  float    filamentUsed;    // Accumulated filament consumed in mm
+  #if SERVICE_INTERVAL_1 > 0
+    uint32_t nextService1;  // Service intervals (or placeholders)
+  #endif
+  #if SERVICE_INTERVAL_2 > 0
+    uint32_t nextService2;
+  #endif
+  #if SERVICE_INTERVAL_3 > 0
+    uint32_t nextService3;
+  #endif
+};
+
+class PrintCounter: public Stopwatch {
+  private:
+    typedef Stopwatch super;
+
+    #if EITHER(USE_WIRED_EEPROM, CPU_32_BIT)
+      typedef uint32_t eeprom_address_t;
+    #else
+      typedef uint16_t eeprom_address_t;
+    #endif
+
+    static printStatistics data;
+
+    /**
+     * @brief EEPROM address
+     * @details Defines the start offset address where the data is stored.
+     */
+    static const eeprom_address_t address;
+
+    /**
+     * @brief Interval in seconds between counter updates
+     * @details This const value defines what will be the time between each
+     * accumulator update. This is different from the EEPROM save interval.
+     */
+    static constexpr millis_t updateInterval = SEC_TO_MS(10);
+
+    #if PRINTCOUNTER_SAVE_INTERVAL > 0
+      /**
+       * @brief Interval in seconds between EEPROM saves
+       * @details This const value defines what will be the time between each
+       * EEPROM save cycle, the development team recommends to set this value
+       * no lower than 3600 secs (1 hour).
+       */
+      static constexpr millis_t saveInterval = MIN_TO_MS(PRINTCOUNTER_SAVE_INTERVAL);
+    #endif
+
+    /**
+     * @brief Timestamp of the last call to deltaDuration()
+     * @details Store the timestamp of the last deltaDuration(), this is
+     * required due to the updateInterval cycle.
+     */
+    static millis_t lastDuration;
+
+    /**
+     * @brief Stats were loaded from EEPROM
+     * @details If set to true it indicates if the statistical data was already
+     * loaded from the EEPROM.
+     */
+    static bool loaded;
+
+  protected:
+    /**
+     * @brief dT since the last call
+     * @details Return the elapsed time in seconds since the last call, this is
+     * used internally for print statistics accounting is not intended to be a
+     * user callable function.
+     */
+    static millis_t deltaDuration();
+
+  public:
+
+    /**
+     * @brief Initialize the print counter
+     */
+    static inline void init() {
+      super::init();
+      loadStats();
+    }
+
+    /**
+     * @brief Check if Print Statistics has been loaded
+     * @details Return true if the statistical data has been loaded.
+     * @return bool
+     */
+    FORCE_INLINE static bool isLoaded() { return loaded; }
+
+    /**
+     * @brief Increment the total filament used
+     * @details The total filament used counter will be incremented by "amount".
+     *
+     * @param amount The amount of filament used in mm
+     */
+    static void incFilamentUsed(float const &amount);
+
+    /**
+     * @brief Reset the Print Statistics
+     * @details Reset the statistics to zero and saves them to EEPROM creating
+     * also the magic header.
+     */
+    static void initStats();
+
+    /**
+     * @brief Load the Print Statistics
+     * @details Load the statistics from EEPROM
+     */
+    static void loadStats();
+
+    /**
+     * @brief Save the Print Statistics
+     * @details Save the statistics to EEPROM
+     */
+    static void saveStats();
+
+    /**
+     * @brief Serial output the Print Statistics
+     * @details This function may change in the future, for now it directly
+     * prints the statistical data to serial.
+     */
+    static void showStats();
+
+    /**
+     * @brief Return the currently loaded statistics
+     * @details Return the raw data, in the same structure used internally
+     */
+    static printStatistics getStats() { return data; }
+
+    /**
+     * @brief Loop function
+     * @details This function should be called at loop, it will take care of
+     * periodically save the statistical data to EEPROM and do time keeping.
+     */
+    static void tick();
+
+    /**
+     * The following functions are being overridden
+     */
+    static bool start();
+    static bool _stop(const bool completed);
+    static inline bool stop()  { return _stop(true);  }
+    static inline bool abort() { return _stop(false); }
+
+    static void reset();
+
+    #if HAS_SERVICE_INTERVALS
+      static void resetServiceInterval(const int index);
+      static bool needsService(const int index);
+    #endif
+
+    #if ENABLED(DEBUG_PRINTCOUNTER)
+
+      /**
+       * @brief Print a debug message
+       * @details Print a simple debug message
+       */
+      static void debug(const char func[]);
+
+    #endif
+};
+
+// Global Print Job Timer instance
+#if ENABLED(PRINTCOUNTER)
+  extern PrintCounter print_job_timer;
+#else
+  extern Stopwatch print_job_timer;
+#endif
diff --git a/Marlin/src/module/probe.cpp b/Marlin/src/module/probe.cpp
new file mode 100644
index 0000000..94c409e
--- /dev/null
+++ b/Marlin/src/module/probe.cpp
@@ -0,0 +1,786 @@
+/**
+ * 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/>.
+ *
+ */
+
+/**
+ * module/probe.cpp
+ */
+
+#include "../inc/MarlinConfig.h"
+
+#if HAS_BED_PROBE
+
+#include "probe.h"
+
+#include "../libs/buzzer.h"
+#include "motion.h"
+#include "temperature.h"
+#include "endstops.h"
+
+#include "../gcode/gcode.h"
+#include "../lcd/marlinui.h"
+
+#include "../MarlinCore.h" // for stop(), disable_e_steppers(), wait_for_user_response()
+
+#if HAS_LEVELING
+  #include "../feature/bedlevel/bedlevel.h"
+#endif
+
+#if ENABLED(DELTA)
+  #include "delta.h"
+#endif
+
+#if ENABLED(BABYSTEP_ZPROBE_OFFSET)
+  #include "planner.h"
+#endif
+
+#if ENABLED(MEASURE_BACKLASH_WHEN_PROBING)
+  #include "../feature/backlash.h"
+#endif
+
+#if ENABLED(BLTOUCH)
+  #include "../feature/bltouch.h"
+#endif
+
+#if ENABLED(HOST_PROMPT_SUPPORT)
+  #include "../feature/host_actions.h" // for PROMPT_USER_CONTINUE
+#endif
+
+#if HAS_Z_SERVO_PROBE
+  #include "servo.h"
+#endif
+
+#if ENABLED(SENSORLESS_PROBING)
+  #include "stepper.h"
+  #include "../feature/tmc_util.h"
+#endif
+
+#if HAS_QUIET_PROBING
+  #include "stepper/indirection.h"
+#endif
+
+#if ENABLED(EXTENSIBLE_UI)
+  #include "../lcd/extui/ui_api.h"
+#endif
+
+#define DEBUG_OUT ENABLED(DEBUG_LEVELING_FEATURE)
+#include "../core/debug_out.h"
+
+Probe probe;
+
+xyz_pos_t Probe::offset; // Initialized by settings.load()
+
+#if HAS_PROBE_XY_OFFSET
+  const xy_pos_t &Probe::offset_xy = Probe::offset;
+#endif
+
+#if ENABLED(Z_PROBE_SLED)
+
+  #ifndef SLED_DOCKING_OFFSET
+    #define SLED_DOCKING_OFFSET 0
+  #endif
+
+  /**
+   * Method to dock/undock a sled designed by Charles Bell.
+   *
+   * stow[in]     If false, move to MAX_X and engage the solenoid
+   *              If true, move to MAX_X and release the solenoid
+   */
+  static void dock_sled(const bool stow) {
+    if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("dock_sled(", stow, ")");
+
+    // Dock sled a bit closer to ensure proper capturing
+    do_blocking_move_to_x(X_MAX_POS + SLED_DOCKING_OFFSET - ((stow) ? 1 : 0));
+
+    #if HAS_SOLENOID_1 && DISABLED(EXT_SOLENOID)
+      WRITE(SOL1_PIN, !stow); // switch solenoid
+    #endif
+  }
+
+#elif ENABLED(TOUCH_MI_PROBE)
+
+  // Move to the magnet to unlock the probe
+  inline void run_deploy_moves_script() {
+    #ifndef TOUCH_MI_DEPLOY_XPOS
+      #define TOUCH_MI_DEPLOY_XPOS X_MIN_POS
+    #elif TOUCH_MI_DEPLOY_XPOS > X_MAX_BED
+      TemporaryGlobalEndstopsState unlock_x(false);
+    #endif
+    #if TOUCH_MI_DEPLOY_YPOS > Y_MAX_BED
+      TemporaryGlobalEndstopsState unlock_y(false);
+    #endif
+
+    #if ENABLED(TOUCH_MI_MANUAL_DEPLOY)
+
+      const screenFunc_t prev_screen = ui.currentScreen;
+      LCD_MESSAGEPGM(MSG_MANUAL_DEPLOY_TOUCHMI);
+      ui.return_to_status();
+
+      TERN_(HOST_PROMPT_SUPPORT, host_prompt_do(PROMPT_USER_CONTINUE, PSTR("Deploy TouchMI"), CONTINUE_STR));
+      wait_for_user_response();
+      ui.reset_status();
+      ui.goto_screen(prev_screen);
+
+    #elif defined(TOUCH_MI_DEPLOY_XPOS) && defined(TOUCH_MI_DEPLOY_YPOS)
+      do_blocking_move_to_xy(TOUCH_MI_DEPLOY_XPOS, TOUCH_MI_DEPLOY_YPOS);
+    #elif defined(TOUCH_MI_DEPLOY_XPOS)
+      do_blocking_move_to_x(TOUCH_MI_DEPLOY_XPOS);
+    #elif defined(TOUCH_MI_DEPLOY_YPOS)
+      do_blocking_move_to_y(TOUCH_MI_DEPLOY_YPOS);
+    #endif
+  }
+
+  // Move down to the bed to stow the probe
+  inline void run_stow_moves_script() {
+    const xyz_pos_t oldpos = current_position;
+    endstops.enable_z_probe(false);
+    do_blocking_move_to_z(TOUCH_MI_RETRACT_Z, homing_feedrate(Z_AXIS));
+    do_blocking_move_to(oldpos, homing_feedrate(Z_AXIS));
+  }
+
+#elif ENABLED(Z_PROBE_ALLEN_KEY)
+
+  inline void run_deploy_moves_script() {
+    #ifdef Z_PROBE_ALLEN_KEY_DEPLOY_1
+      #ifndef Z_PROBE_ALLEN_KEY_DEPLOY_1_FEEDRATE
+        #define Z_PROBE_ALLEN_KEY_DEPLOY_1_FEEDRATE 0.0
+      #endif
+      constexpr xyz_pos_t deploy_1 = Z_PROBE_ALLEN_KEY_DEPLOY_1;
+      do_blocking_move_to(deploy_1, MMM_TO_MMS(Z_PROBE_ALLEN_KEY_DEPLOY_1_FEEDRATE));
+    #endif
+    #ifdef Z_PROBE_ALLEN_KEY_DEPLOY_2
+      #ifndef Z_PROBE_ALLEN_KEY_DEPLOY_2_FEEDRATE
+        #define Z_PROBE_ALLEN_KEY_DEPLOY_2_FEEDRATE 0.0
+      #endif
+      constexpr xyz_pos_t deploy_2 = Z_PROBE_ALLEN_KEY_DEPLOY_2;
+      do_blocking_move_to(deploy_2, MMM_TO_MMS(Z_PROBE_ALLEN_KEY_DEPLOY_2_FEEDRATE));
+    #endif
+    #ifdef Z_PROBE_ALLEN_KEY_DEPLOY_3
+      #ifndef Z_PROBE_ALLEN_KEY_DEPLOY_3_FEEDRATE
+        #define Z_PROBE_ALLEN_KEY_DEPLOY_3_FEEDRATE 0.0
+      #endif
+      constexpr xyz_pos_t deploy_3 = Z_PROBE_ALLEN_KEY_DEPLOY_3;
+      do_blocking_move_to(deploy_3, MMM_TO_MMS(Z_PROBE_ALLEN_KEY_DEPLOY_3_FEEDRATE));
+    #endif
+    #ifdef Z_PROBE_ALLEN_KEY_DEPLOY_4
+      #ifndef Z_PROBE_ALLEN_KEY_DEPLOY_4_FEEDRATE
+        #define Z_PROBE_ALLEN_KEY_DEPLOY_4_FEEDRATE 0.0
+      #endif
+      constexpr xyz_pos_t deploy_4 = Z_PROBE_ALLEN_KEY_DEPLOY_4;
+      do_blocking_move_to(deploy_4, MMM_TO_MMS(Z_PROBE_ALLEN_KEY_DEPLOY_4_FEEDRATE));
+    #endif
+    #ifdef Z_PROBE_ALLEN_KEY_DEPLOY_5
+      #ifndef Z_PROBE_ALLEN_KEY_DEPLOY_5_FEEDRATE
+        #define Z_PROBE_ALLEN_KEY_DEPLOY_5_FEEDRATE 0.0
+      #endif
+      constexpr xyz_pos_t deploy_5 = Z_PROBE_ALLEN_KEY_DEPLOY_5;
+      do_blocking_move_to(deploy_5, MMM_TO_MMS(Z_PROBE_ALLEN_KEY_DEPLOY_5_FEEDRATE));
+    #endif
+  }
+
+  inline void run_stow_moves_script() {
+    #ifdef Z_PROBE_ALLEN_KEY_STOW_1
+      #ifndef Z_PROBE_ALLEN_KEY_STOW_1_FEEDRATE
+        #define Z_PROBE_ALLEN_KEY_STOW_1_FEEDRATE 0.0
+      #endif
+      constexpr xyz_pos_t stow_1 = Z_PROBE_ALLEN_KEY_STOW_1;
+      do_blocking_move_to(stow_1, MMM_TO_MMS(Z_PROBE_ALLEN_KEY_STOW_1_FEEDRATE));
+    #endif
+    #ifdef Z_PROBE_ALLEN_KEY_STOW_2
+      #ifndef Z_PROBE_ALLEN_KEY_STOW_2_FEEDRATE
+        #define Z_PROBE_ALLEN_KEY_STOW_2_FEEDRATE 0.0
+      #endif
+      constexpr xyz_pos_t stow_2 = Z_PROBE_ALLEN_KEY_STOW_2;
+      do_blocking_move_to(stow_2, MMM_TO_MMS(Z_PROBE_ALLEN_KEY_STOW_2_FEEDRATE));
+    #endif
+    #ifdef Z_PROBE_ALLEN_KEY_STOW_3
+      #ifndef Z_PROBE_ALLEN_KEY_STOW_3_FEEDRATE
+        #define Z_PROBE_ALLEN_KEY_STOW_3_FEEDRATE 0.0
+      #endif
+      constexpr xyz_pos_t stow_3 = Z_PROBE_ALLEN_KEY_STOW_3;
+      do_blocking_move_to(stow_3, MMM_TO_MMS(Z_PROBE_ALLEN_KEY_STOW_3_FEEDRATE));
+    #endif
+    #ifdef Z_PROBE_ALLEN_KEY_STOW_4
+      #ifndef Z_PROBE_ALLEN_KEY_STOW_4_FEEDRATE
+        #define Z_PROBE_ALLEN_KEY_STOW_4_FEEDRATE 0.0
+      #endif
+      constexpr xyz_pos_t stow_4 = Z_PROBE_ALLEN_KEY_STOW_4;
+      do_blocking_move_to(stow_4, MMM_TO_MMS(Z_PROBE_ALLEN_KEY_STOW_4_FEEDRATE));
+    #endif
+    #ifdef Z_PROBE_ALLEN_KEY_STOW_5
+      #ifndef Z_PROBE_ALLEN_KEY_STOW_5_FEEDRATE
+        #define Z_PROBE_ALLEN_KEY_STOW_5_FEEDRATE 0.0
+      #endif
+      constexpr xyz_pos_t stow_5 = Z_PROBE_ALLEN_KEY_STOW_5;
+      do_blocking_move_to(stow_5, MMM_TO_MMS(Z_PROBE_ALLEN_KEY_STOW_5_FEEDRATE));
+    #endif
+  }
+
+#endif // Z_PROBE_ALLEN_KEY
+
+#if HAS_QUIET_PROBING
+
+  void Probe::set_probing_paused(const bool p) {
+    TERN_(PROBING_HEATERS_OFF, thermalManager.pause(p));
+    TERN_(PROBING_FANS_OFF, thermalManager.set_fans_paused(p));
+    #if ENABLED(PROBING_STEPPERS_OFF)
+      disable_e_steppers();
+      #if NONE(DELTA, HOME_AFTER_DEACTIVATE)
+        DISABLE_AXIS_X(); DISABLE_AXIS_Y();
+      #endif
+    #endif
+    if (p) safe_delay(25
+      #if DELAY_BEFORE_PROBING > 25
+        - 25 + DELAY_BEFORE_PROBING
+      #endif
+    );
+  }
+
+#endif // HAS_QUIET_PROBING
+
+/**
+ * Raise Z to a minimum height to make room for a probe to move
+ */
+void Probe::do_z_raise(const float z_raise) {
+  if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("Probe::do_z_raise(", z_raise, ")");
+  float z_dest = z_raise;
+  if (offset.z < 0) z_dest -= offset.z;
+  do_z_clearance(z_dest);
+}
+
+FORCE_INLINE void probe_specific_action(const bool deploy) {
+  #if ENABLED(PAUSE_BEFORE_DEPLOY_STOW)
+    do {
+      #if ENABLED(PAUSE_PROBE_DEPLOY_WHEN_TRIGGERED)
+        if (deploy == PROBE_TRIGGERED()) break;
+      #endif
+
+      BUZZ(100, 659);
+      BUZZ(100, 698);
+
+      PGM_P const ds_str = deploy ? GET_TEXT(MSG_MANUAL_DEPLOY) : GET_TEXT(MSG_MANUAL_STOW);
+      ui.return_to_status();       // To display the new status message
+      ui.set_status_P(ds_str, 99);
+      serialprintPGM(ds_str);
+      SERIAL_EOL();
+
+      TERN_(HOST_PROMPT_SUPPORT, host_prompt_do(PROMPT_USER_CONTINUE, PSTR("Stow Probe"), CONTINUE_STR));
+      TERN_(EXTENSIBLE_UI, ExtUI::onUserConfirmRequired_P(PSTR("Stow Probe")));
+
+      wait_for_user_response();
+      ui.reset_status();
+
+    } while (ENABLED(PAUSE_PROBE_DEPLOY_WHEN_TRIGGERED));
+
+  #endif // PAUSE_BEFORE_DEPLOY_STOW
+
+  #if ENABLED(SOLENOID_PROBE)
+
+    #if HAS_SOLENOID_1
+      WRITE(SOL1_PIN, deploy);
+    #endif
+
+  #elif ENABLED(Z_PROBE_SLED)
+
+    dock_sled(!deploy);
+
+  #elif ENABLED(BLTOUCH)
+
+    deploy ? bltouch.deploy() : bltouch.stow();
+
+  #elif HAS_Z_SERVO_PROBE
+
+    MOVE_SERVO(Z_PROBE_SERVO_NR, servo_angles[Z_PROBE_SERVO_NR][deploy ? 0 : 1]);
+
+  #elif EITHER(TOUCH_MI_PROBE, Z_PROBE_ALLEN_KEY)
+
+    deploy ? run_deploy_moves_script() : run_stow_moves_script();
+
+  #elif ENABLED(RACK_AND_PINION_PROBE)
+
+    do_blocking_move_to_x(deploy ? Z_PROBE_DEPLOY_X : Z_PROBE_RETRACT_X);
+
+  #elif DISABLED(PAUSE_BEFORE_DEPLOY_STOW)
+
+    UNUSED(deploy);
+
+  #endif
+}
+
+#if EITHER(PREHEAT_BEFORE_PROBING, PREHEAT_BEFORE_LEVELING)
+
+  /**
+   * Do preheating as required before leveling or probing
+   */
+  void Probe::preheat_for_probing(const uint16_t hotend_temp, const uint16_t bed_temp) {
+    #if PROBING_NOZZLE_TEMP || LEVELING_NOZZLE_TEMP
+      #define WAIT_FOR_NOZZLE_HEAT
+    #endif
+    #if PROBING_BED_TEMP || LEVELING_BED_TEMP
+      #define WAIT_FOR_BED_HEAT
+    #endif
+    const uint16_t hotendPreheat = TERN0(WAIT_FOR_NOZZLE_HEAT, thermalManager.degHotend(0) < hotend_temp) ? hotend_temp : 0,
+                      bedPreheat = TERN0(WAIT_FOR_BED_HEAT,    thermalManager.degBed()     < bed_temp)    ? bed_temp    : 0;
+    DEBUG_ECHOPGM("Preheating ");
+    if (hotendPreheat) {
+      DEBUG_ECHOPAIR("hotend (", hotendPreheat, ") ");
+      if (bedPreheat) DEBUG_ECHOPGM("and ");
+    }
+    if (bedPreheat) DEBUG_ECHOPAIR("bed (", bedPreheat, ") ");
+    DEBUG_EOL();
+
+    TERN_(WAIT_FOR_NOZZLE_HEAT, if (hotendPreheat) thermalManager.setTargetHotend(hotendPreheat, 0));
+    TERN_(WAIT_FOR_BED_HEAT,    if (bedPreheat)    thermalManager.setTargetBed(bedPreheat));
+    TERN_(WAIT_FOR_NOZZLE_HEAT, if (hotendPreheat) thermalManager.wait_for_hotend(0));
+    TERN_(WAIT_FOR_BED_HEAT,    if (bedPreheat)    thermalManager.wait_for_bed_heating());
+  }
+
+#endif
+
+/**
+ * Attempt to deploy or stow the probe
+ *
+ * Return TRUE if the probe could not be deployed/stowed
+ */
+bool Probe::set_deployed(const bool deploy) {
+
+  if (DEBUGGING(LEVELING)) {
+    DEBUG_POS("Probe::set_deployed", current_position);
+    DEBUG_ECHOLNPAIR("deploy: ", deploy);
+  }
+
+  if (endstops.z_probe_enabled == deploy) return false;
+
+  // Make room for probe to deploy (or stow)
+  // Fix-mounted probe should only raise for deploy
+  // unless PAUSE_BEFORE_DEPLOY_STOW is enabled
+  #if EITHER(FIX_MOUNTED_PROBE, NOZZLE_AS_PROBE) && DISABLED(PAUSE_BEFORE_DEPLOY_STOW)
+    const bool z_raise_wanted = deploy;
+  #else
+    constexpr bool z_raise_wanted = true;
+  #endif
+
+  // For beds that fall when Z is powered off only raise for trusted Z
+  #if ENABLED(UNKNOWN_Z_NO_RAISE)
+    const bool z_is_trusted = axis_is_trusted(Z_AXIS);
+  #else
+    constexpr float z_is_trusted = true;
+  #endif
+
+  if (z_is_trusted && z_raise_wanted)
+    do_z_raise(_MAX(Z_CLEARANCE_BETWEEN_PROBES, Z_CLEARANCE_DEPLOY_PROBE));
+
+  #if EITHER(Z_PROBE_SLED, Z_PROBE_ALLEN_KEY)
+    if (homing_needed_error(TERN_(Z_PROBE_SLED, _BV(X_AXIS)))) {
+      SERIAL_ERROR_MSG(STR_STOP_UNHOMED);
+      stop();
+      return true;
+    }
+  #endif
+
+  const xy_pos_t old_xy = current_position;
+
+  #if ENABLED(PROBE_TRIGGERED_WHEN_STOWED_TEST)
+
+    // Only deploy/stow if needed
+    if (PROBE_TRIGGERED() == deploy) {
+      if (!deploy) endstops.enable_z_probe(false); // Switch off triggered when stowed probes early
+                                                   // otherwise an Allen-Key probe can't be stowed.
+      probe_specific_action(deploy);
+    }
+
+    if (PROBE_TRIGGERED() == deploy) {             // Unchanged after deploy/stow action?
+      if (IsRunning()) {
+        SERIAL_ERROR_MSG("Z-Probe failed");
+        LCD_ALERTMESSAGEPGM_P(PSTR("Err: ZPROBE"));
+      }
+      stop();
+      return true;
+    }
+
+  #else
+
+    probe_specific_action(deploy);
+
+  #endif
+
+  // If preheating is required before any probing...
+  TERN_(PREHEAT_BEFORE_PROBING, if (deploy) preheat_for_probing(PROBING_NOZZLE_TEMP, PROBING_BED_TEMP));
+
+  do_blocking_move_to(old_xy);
+  endstops.enable_z_probe(deploy);
+  return false;
+}
+
+/**
+ * @brief Used by run_z_probe to do a single Z probe move.
+ *
+ * @param  z        Z destination
+ * @param  fr_mm_s  Feedrate in mm/s
+ * @return true to indicate an error
+ */
+
+/**
+ * @brief Move down until the probe triggers or the low limit is reached
+ *
+ * @details Used by run_z_probe to get each bed Z height measurement.
+ *          Sets current_position.z to the height where the probe triggered
+ *          (according to the Z stepper count). The float Z is propagated
+ *          back to the planner.position to preempt any rounding error.
+ *
+ * @return TRUE if the probe failed to trigger.
+ */
+bool Probe::probe_down_to_z(const float z, const feedRate_t fr_mm_s) {
+  DEBUG_SECTION(log_probe, "Probe::probe_down_to_z", DEBUGGING(LEVELING));
+
+  #if BOTH(HAS_HEATED_BED, WAIT_FOR_BED_HEATER)
+    thermalManager.wait_for_bed_heating();
+  #endif
+
+  if (TERN0(BLTOUCH_SLOW_MODE, bltouch.deploy())) return true; // Deploy in LOW SPEED MODE on every probe action
+
+  // Disable stealthChop if used. Enable diag1 pin on driver.
+  #if ENABLED(SENSORLESS_PROBING)
+    sensorless_t stealth_states { false };
+    #if ENABLED(DELTA)
+      stealth_states.x = tmc_enable_stallguard(stepperX);
+      stealth_states.y = tmc_enable_stallguard(stepperY);
+    #endif
+    stealth_states.z = tmc_enable_stallguard(stepperZ);
+    endstops.enable(true);
+  #endif
+
+  TERN_(HAS_QUIET_PROBING, set_probing_paused(true));
+
+  // Move down until the probe is triggered
+  do_blocking_move_to_z(z, fr_mm_s);
+
+  // Check to see if the probe was triggered
+  const bool probe_triggered =
+    #if BOTH(DELTA, SENSORLESS_PROBING)
+      endstops.trigger_state() & (_BV(X_MIN) | _BV(Y_MIN) | _BV(Z_MIN))
+    #else
+      TEST(endstops.trigger_state(), TERN(Z_MIN_PROBE_USES_Z_MIN_ENDSTOP_PIN, Z_MIN, Z_MIN_PROBE))
+    #endif
+  ;
+
+  TERN_(HAS_QUIET_PROBING, set_probing_paused(false));
+
+  // Re-enable stealthChop if used. Disable diag1 pin on driver.
+  #if ENABLED(SENSORLESS_PROBING)
+    endstops.not_homing();
+    #if ENABLED(DELTA)
+      tmc_disable_stallguard(stepperX, stealth_states.x);
+      tmc_disable_stallguard(stepperY, stealth_states.y);
+    #endif
+    tmc_disable_stallguard(stepperZ, stealth_states.z);
+  #endif
+
+  if (probe_triggered && TERN0(BLTOUCH_SLOW_MODE, bltouch.stow())) // Stow in LOW SPEED MODE on every trigger
+    return true;
+
+  // Clear endstop flags
+  endstops.hit_on_purpose();
+
+  // Get Z where the steppers were interrupted
+  set_current_from_steppers_for_axis(Z_AXIS);
+
+  // Tell the planner where we actually are
+  sync_plan_position();
+
+  return !probe_triggered;
+}
+
+#if ENABLED(PROBE_TARE)
+
+  /**
+   * @brief Init the tare pin
+   *
+   * @details Init tare pin to ON state for a strain gauge, otherwise OFF
+   */
+  void Probe::tare_init() {
+    OUT_WRITE(PROBE_TARE_PIN, !PROBE_TARE_STATE);
+  }
+
+  /**
+   * @brief Tare the Z probe
+   *
+   * @details Signal to the probe to tare itself
+   *
+   * @return TRUE if the tare cold not be completed
+   */
+  bool Probe::tare() {
+    #if BOTH(PROBE_ACTIVATION_SWITCH, PROBE_TARE_ONLY_WHILE_INACTIVE)
+      if (endstops.probe_switch_activated()) {
+        SERIAL_ECHOLNPGM("Cannot tare an active probe");
+        return true;
+      }
+    #endif
+
+    SERIAL_ECHOLNPGM("Taring probe");
+    WRITE(PROBE_TARE_PIN, PROBE_TARE_STATE);
+    delay(PROBE_TARE_TIME);
+    WRITE(PROBE_TARE_PIN, !PROBE_TARE_STATE);
+    delay(PROBE_TARE_DELAY);
+
+    endstops.hit_on_purpose();
+    return false;
+  }
+#endif
+
+/**
+ * @brief Probe at the current XY (possibly more than once) to find the bed Z.
+ *
+ * @details Used by probe_at_point to get the bed Z height at the current XY.
+ *          Leaves current_position.z at the height where the probe triggered.
+ *
+ * @return The Z position of the bed at the current XY or NAN on error.
+ */
+float Probe::run_z_probe(const bool sanity_check/*=true*/) {
+  DEBUG_SECTION(log_probe, "Probe::run_z_probe", DEBUGGING(LEVELING));
+
+  auto try_to_probe = [&](PGM_P const plbl, const float &z_probe_low_point, const feedRate_t fr_mm_s, const bool scheck, const float clearance) -> bool {
+    // Tare the probe, if supported
+    if (TERN0(PROBE_TARE, tare())) return true;
+
+    // Do a first probe at the fast speed
+    const bool probe_fail = probe_down_to_z(z_probe_low_point, fr_mm_s),            // No probe trigger?
+               early_fail = (scheck && current_position.z > -offset.z + clearance); // Probe triggered too high?
+    #if ENABLED(DEBUG_LEVELING_FEATURE)
+      if (DEBUGGING(LEVELING) && (probe_fail || early_fail)) {
+        DEBUG_PRINT_P(plbl);
+        DEBUG_ECHOPGM(" Probe fail! -");
+        if (probe_fail) DEBUG_ECHOPGM(" No trigger.");
+        if (early_fail) DEBUG_ECHOPGM(" Triggered early.");
+        DEBUG_EOL();
+      }
+    #else
+      UNUSED(plbl);
+    #endif
+    return probe_fail || early_fail;
+  };
+
+  // Stop the probe before it goes too low to prevent damage.
+  // If Z isn't known then probe to -10mm.
+  const float z_probe_low_point = axis_is_trusted(Z_AXIS) ? -offset.z + Z_PROBE_LOW_POINT : -10.0;
+
+  // Double-probing does a fast probe followed by a slow probe
+  #if TOTAL_PROBING == 2
+
+    // Attempt to tare the probe
+    if (TERN0(PROBE_TARE, tare())) return NAN;
+
+    // Do a first probe at the fast speed
+    if (try_to_probe(PSTR("FAST"), z_probe_low_point, z_probe_fast_mm_s,
+                     sanity_check, Z_CLEARANCE_BETWEEN_PROBES) ) return NAN;
+
+    const float first_probe_z = current_position.z;
+
+    if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("1st Probe Z:", first_probe_z);
+
+    // Raise to give the probe clearance
+    do_blocking_move_to_z(current_position.z + Z_CLEARANCE_MULTI_PROBE, z_probe_fast_mm_s);
+
+  #elif Z_PROBE_SPEED_FAST != Z_PROBE_SPEED_SLOW
+
+    // If the nozzle is well over the travel height then
+    // move down quickly before doing the slow probe
+    const float z = Z_CLEARANCE_DEPLOY_PROBE + 5.0 + (offset.z < 0 ? -offset.z : 0);
+    if (current_position.z > z) {
+      // Probe down fast. If the probe never triggered, raise for probe clearance
+      if (!probe_down_to_z(z, z_probe_fast_mm_s))
+        do_blocking_move_to_z(current_position.z + Z_CLEARANCE_BETWEEN_PROBES, z_probe_fast_mm_s);
+    }
+  #endif
+
+  #if EXTRA_PROBING > 0
+    float probes[TOTAL_PROBING];
+  #endif
+
+  #if TOTAL_PROBING > 2
+    float probes_z_sum = 0;
+    for (
+      #if EXTRA_PROBING > 0
+        uint8_t p = 0; p < TOTAL_PROBING; p++
+      #else
+        uint8_t p = TOTAL_PROBING; p--;
+      #endif
+    )
+  #endif
+    {
+      // If the probe won't tare, return
+      if (TERN0(PROBE_TARE, tare())) return true;
+
+      // Probe downward slowly to find the bed
+      if (try_to_probe(PSTR("SLOW"), z_probe_low_point, MMM_TO_MMS(Z_PROBE_SPEED_SLOW),
+                       sanity_check, Z_CLEARANCE_MULTI_PROBE) ) return NAN;
+
+      TERN_(MEASURE_BACKLASH_WHEN_PROBING, backlash.measure_with_probe());
+
+      const float z = current_position.z;
+
+      #if EXTRA_PROBING > 0
+        // Insert Z measurement into probes[]. Keep it sorted ascending.
+        LOOP_LE_N(i, p) {                            // Iterate the saved Zs to insert the new Z
+          if (i == p || probes[i] > z) {                              // Last index or new Z is smaller than this Z
+            for (int8_t m = p; --m >= i;) probes[m + 1] = probes[m];  // Shift items down after the insertion point
+            probes[i] = z;                                            // Insert the new Z measurement
+            break;                                                    // Only one to insert. Done!
+          }
+        }
+      #elif TOTAL_PROBING > 2
+        probes_z_sum += z;
+      #else
+        UNUSED(z);
+      #endif
+
+      #if TOTAL_PROBING > 2
+        // Small Z raise after all but the last probe
+        if (p
+          #if EXTRA_PROBING > 0
+            < TOTAL_PROBING - 1
+          #endif
+        ) do_blocking_move_to_z(z + Z_CLEARANCE_MULTI_PROBE, z_probe_fast_mm_s);
+      #endif
+    }
+
+  #if TOTAL_PROBING > 2
+
+    #if EXTRA_PROBING > 0
+      // Take the center value (or average the two middle values) as the median
+      static constexpr int PHALF = (TOTAL_PROBING - 1) / 2;
+      const float middle = probes[PHALF],
+                  median = ((TOTAL_PROBING) & 1) ? middle : (middle + probes[PHALF + 1]) * 0.5f;
+
+      // Remove values farthest from the median
+      uint8_t min_avg_idx = 0, max_avg_idx = TOTAL_PROBING - 1;
+      for (uint8_t i = EXTRA_PROBING; i--;)
+        if (ABS(probes[max_avg_idx] - median) > ABS(probes[min_avg_idx] - median))
+          max_avg_idx--; else min_avg_idx++;
+
+      // Return the average value of all remaining probes.
+      LOOP_S_LE_N(i, min_avg_idx, max_avg_idx)
+        probes_z_sum += probes[i];
+
+    #endif
+
+    const float measured_z = probes_z_sum * RECIPROCAL(MULTIPLE_PROBING);
+
+  #elif TOTAL_PROBING == 2
+
+    const float z2 = current_position.z;
+
+    if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("2nd Probe Z:", z2, " Discrepancy:", first_probe_z - z2);
+
+    // Return a weighted average of the fast and slow probes
+    const float measured_z = (z2 * 3.0 + first_probe_z * 2.0) * 0.2;
+
+  #else
+
+    // Return the single probe result
+    const float measured_z = current_position.z;
+
+  #endif
+
+  return measured_z;
+}
+
+/**
+ * - Move to the given XY
+ * - Deploy the probe, if not already deployed
+ * - Probe the bed, get the Z position
+ * - Depending on the 'stow' flag
+ *   - Stow the probe, or
+ *   - Raise to the BETWEEN height
+ * - Return the probed Z position
+ */
+float Probe::probe_at_point(const float &rx, const float &ry, const ProbePtRaise raise_after/*=PROBE_PT_NONE*/, const uint8_t verbose_level/*=0*/, const bool probe_relative/*=true*/, const bool sanity_check/*=true*/) {
+  DEBUG_SECTION(log_probe, "Probe::probe_at_point", DEBUGGING(LEVELING));
+
+  if (DEBUGGING(LEVELING)) {
+    DEBUG_ECHOLNPAIR(
+      "...(", LOGICAL_X_POSITION(rx), ", ", LOGICAL_Y_POSITION(ry),
+      ", ", raise_after == PROBE_PT_RAISE ? "raise" : raise_after == PROBE_PT_STOW ? "stow" : "none",
+      ", ", int(verbose_level),
+      ", ", probe_relative ? "probe" : "nozzle", "_relative)"
+    );
+    DEBUG_POS("", current_position);
+  }
+
+  #if BOTH(BLTOUCH, BLTOUCH_HS_MODE)
+    if (bltouch.triggered()) bltouch._reset();
+  #endif
+
+  // On delta keep Z below clip height or do_blocking_move_to will abort
+  xyz_pos_t npos = { rx, ry, _MIN(TERN(DELTA, delta_clip_start_height, current_position.z), current_position.z) };
+  if (probe_relative) {                                     // The given position is in terms of the probe
+    if (!can_reach(npos)) {
+      if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Position Not Reachable");
+      return NAN;
+    }
+    npos -= offset_xy;                                      // Get the nozzle position
+  }
+  else if (!position_is_reachable(npos)) return NAN;        // The given position is in terms of the nozzle
+
+  // Move the probe to the starting XYZ
+  do_blocking_move_to(npos, feedRate_t(XY_PROBE_FEEDRATE_MM_S));
+
+  float measured_z = NAN;
+  if (!deploy()) measured_z = run_z_probe(sanity_check) + offset.z;
+  if (!isnan(measured_z)) {
+    const bool big_raise = raise_after == PROBE_PT_BIG_RAISE;
+    if (big_raise || raise_after == PROBE_PT_RAISE)
+      do_blocking_move_to_z(current_position.z + (big_raise ? 25 : Z_CLEARANCE_BETWEEN_PROBES), z_probe_fast_mm_s);
+    else if (raise_after == PROBE_PT_STOW)
+      if (stow()) measured_z = NAN;   // Error on stow?
+
+    if (verbose_level > 2)
+      SERIAL_ECHOLNPAIR("Bed X: ", LOGICAL_X_POSITION(rx), " Y: ", LOGICAL_Y_POSITION(ry), " Z: ", measured_z);
+  }
+
+  if (isnan(measured_z)) {
+    stow();
+    LCD_MESSAGEPGM(MSG_LCD_PROBING_FAILED);
+    #if DISABLED(G29_RETRY_AND_RECOVER)
+      SERIAL_ERROR_MSG(STR_ERR_PROBING_FAILED);
+    #endif
+  }
+
+  return measured_z;
+}
+
+#if HAS_Z_SERVO_PROBE
+
+  void Probe::servo_probe_init() {
+    /**
+     * Set position of Z Servo Endstop
+     *
+     * The servo might be deployed and positioned too low to stow
+     * when starting up the machine or rebooting the board.
+     * There's no way to know where the nozzle is positioned until
+     * homing has been done - no homing with z-probe without init!
+     */
+    STOW_Z_SERVO();
+  }
+
+#endif // HAS_Z_SERVO_PROBE
+
+#endif // HAS_BED_PROBE
diff --git a/Marlin/src/module/probe.h b/Marlin/src/module/probe.h
new file mode 100644
index 0000000..d28cdff
--- /dev/null
+++ b/Marlin/src/module/probe.h
@@ -0,0 +1,256 @@
+/**
+ * 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
+
+/**
+ * module/probe.h - Move, deploy, enable, etc.
+ */
+
+#include "../inc/MarlinConfig.h"
+
+#include "motion.h"
+
+#if HAS_BED_PROBE
+  enum ProbePtRaise : uint8_t {
+    PROBE_PT_NONE,      // No raise or stow after run_z_probe
+    PROBE_PT_STOW,      // Do a complete stow after run_z_probe
+    PROBE_PT_RAISE,     // Raise to "between" clearance after run_z_probe
+    PROBE_PT_BIG_RAISE  // Raise to big clearance after run_z_probe
+  };
+#endif
+
+#if HAS_CUSTOM_PROBE_PIN
+  #define PROBE_TRIGGERED() (READ(Z_MIN_PROBE_PIN) != Z_MIN_PROBE_ENDSTOP_INVERTING)
+#else
+  #define PROBE_TRIGGERED() (READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING)
+#endif
+
+class Probe {
+public:
+
+  #if HAS_BED_PROBE
+
+    static xyz_pos_t offset;
+
+    #if EITHER(PREHEAT_BEFORE_PROBING, PREHEAT_BEFORE_LEVELING)
+      static void preheat_for_probing(const uint16_t hotend_temp, const uint16_t bed_temp);
+    #endif
+
+    static bool set_deployed(const bool deploy);
+
+    #if IS_KINEMATIC
+
+      #if HAS_PROBE_XY_OFFSET
+        // Return true if the both nozzle and the probe can reach the given point.
+        // Note: This won't work on SCARA since the probe offset rotates with the arm.
+        static bool can_reach(const float &rx, const float &ry) {
+          return position_is_reachable(rx - offset_xy.x, ry - offset_xy.y) // The nozzle can go where it needs to go?
+              && position_is_reachable(rx, ry, ABS(PROBING_MARGIN));       // Can the nozzle also go near there?
+        }
+      #else
+        static bool can_reach(const float &rx, const float &ry) {
+          return position_is_reachable(rx, ry, PROBING_MARGIN);
+        }
+      #endif
+
+    #else
+
+      /**
+       * Return whether the given position is within the bed, and whether the nozzle
+       * can reach the position required to put the probe at the given position.
+       *
+       * Example: For a probe offset of -10,+10, then for the probe to reach 0,0 the
+       *          nozzle must be be able to reach +10,-10.
+       */
+      static bool can_reach(const float &rx, const float &ry) {
+        return position_is_reachable(rx - offset_xy.x, ry - offset_xy.y)
+            && WITHIN(rx, min_x() - fslop, max_x() + fslop)
+            && WITHIN(ry, min_y() - fslop, max_y() + fslop);
+      }
+
+    #endif
+
+    static void move_z_after_probing() {
+      #ifdef Z_AFTER_PROBING
+        do_z_clearance(Z_AFTER_PROBING, true, true, true); // Move down still permitted
+      #endif
+    }
+    static float probe_at_point(const float &rx, const float &ry, const ProbePtRaise raise_after=PROBE_PT_NONE, const uint8_t verbose_level=0, const bool probe_relative=true, const bool sanity_check=true);
+    static float probe_at_point(const xy_pos_t &pos, const ProbePtRaise raise_after=PROBE_PT_NONE, const uint8_t verbose_level=0, const bool probe_relative=true, const bool sanity_check=true) {
+      return probe_at_point(pos.x, pos.y, raise_after, verbose_level, probe_relative, sanity_check);
+    }
+
+  #else
+
+    static constexpr xyz_pos_t offset = xyz_pos_t({ 0, 0, 0 }); // See #16767
+
+    static bool set_deployed(const bool) { return false; }
+
+    static bool can_reach(const float &rx, const float &ry) { return position_is_reachable(rx, ry); }
+
+  #endif
+
+  static void move_z_after_homing() {
+    #ifdef Z_AFTER_HOMING
+      do_z_clearance(Z_AFTER_HOMING, true, true, true);
+    #elif BOTH(Z_AFTER_PROBING, HAS_BED_PROBE)
+      move_z_after_probing();
+    #endif
+  }
+
+  static bool can_reach(const xy_pos_t &pos) { return can_reach(pos.x, pos.y); }
+
+  static bool good_bounds(const xy_pos_t &lf, const xy_pos_t &rb) {
+    return (
+      #if IS_KINEMATIC
+        can_reach(lf.x, 0) && can_reach(rb.x, 0) && can_reach(0, lf.y) && can_reach(0, rb.y)
+      #else
+        can_reach(lf) && can_reach(rb)
+      #endif
+    );
+  }
+
+  // Use offset_xy for read only access
+  // More optimal the XY offset is known to always be zero.
+  #if HAS_PROBE_XY_OFFSET
+    static const xy_pos_t &offset_xy;
+  #else
+    static constexpr xy_pos_t offset_xy = xy_pos_t({ 0, 0 });   // See #16767
+  #endif
+
+  static bool deploy() { return set_deployed(true); }
+  static bool stow()   { return set_deployed(false); }
+
+  #if HAS_BED_PROBE || HAS_LEVELING
+    #if IS_KINEMATIC
+      static constexpr float printable_radius = (
+        TERN_(DELTA, DELTA_PRINTABLE_RADIUS)
+        TERN_(IS_SCARA, SCARA_PRINTABLE_RADIUS)
+      );
+      static constexpr float probe_radius(const xy_pos_t &probe_offset_xy = offset_xy) {
+        return printable_radius - _MAX(PROBING_MARGIN, HYPOT(probe_offset_xy.x, probe_offset_xy.y));
+      }
+    #endif
+
+    static constexpr float _min_x(const xy_pos_t &probe_offset_xy = offset_xy) {
+      return TERN(IS_KINEMATIC,
+        (X_CENTER) - probe_radius(probe_offset_xy),
+        _MAX((X_MIN_BED) + (PROBING_MARGIN_LEFT), (X_MIN_POS) + probe_offset_xy.x)
+      );
+    }
+    static constexpr float _max_x(const xy_pos_t &probe_offset_xy = offset_xy) {
+      return TERN(IS_KINEMATIC,
+        (X_CENTER) + probe_radius(probe_offset_xy),
+        _MIN((X_MAX_BED) - (PROBING_MARGIN_RIGHT), (X_MAX_POS) + probe_offset_xy.x)
+      );
+    }
+    static constexpr float _min_y(const xy_pos_t &probe_offset_xy = offset_xy) {
+      return TERN(IS_KINEMATIC,
+        (Y_CENTER) - probe_radius(probe_offset_xy),
+        _MAX((Y_MIN_BED) + (PROBING_MARGIN_FRONT), (Y_MIN_POS) + probe_offset_xy.y)
+      );
+    }
+    static constexpr float _max_y(const xy_pos_t &probe_offset_xy = offset_xy) {
+      return TERN(IS_KINEMATIC,
+        (Y_CENTER) + probe_radius(probe_offset_xy),
+        _MIN((Y_MAX_BED) - (PROBING_MARGIN_BACK), (Y_MAX_POS) + probe_offset_xy.y)
+      );
+    }
+
+    static float min_x() { return _min_x() - TERN0(NOZZLE_AS_PROBE, TERN0(HAS_HOME_OFFSET, home_offset.x)); }
+    static float max_x() { return _max_x() - TERN0(NOZZLE_AS_PROBE, TERN0(HAS_HOME_OFFSET, home_offset.x)); }
+    static float min_y() { return _min_y() - TERN0(NOZZLE_AS_PROBE, TERN0(HAS_HOME_OFFSET, home_offset.y)); }
+    static float max_y() { return _max_y() - TERN0(NOZZLE_AS_PROBE, TERN0(HAS_HOME_OFFSET, home_offset.y)); }
+
+    // constexpr helpers used in build-time static_asserts, relying on default probe offsets.
+    class build_time {
+      static constexpr xyz_pos_t default_probe_xyz_offset =
+        #if HAS_BED_PROBE
+          NOZZLE_TO_PROBE_OFFSET
+        #else
+          { 0 }
+        #endif
+      ;
+      static constexpr xy_pos_t default_probe_xy_offset = { default_probe_xyz_offset.x,  default_probe_xyz_offset.y };
+
+    public:
+      static constexpr bool can_reach(float x, float y) {
+        #if IS_KINEMATIC
+          return HYPOT2(x, y) <= sq(probe_radius(default_probe_xy_offset));
+        #else
+          return WITHIN(x, _min_x(default_probe_xy_offset) - fslop, _max_x(default_probe_xy_offset) + fslop)
+              && WITHIN(y, _min_y(default_probe_xy_offset) - fslop, _max_y(default_probe_xy_offset) + fslop);
+        #endif
+      }
+
+      static constexpr bool can_reach(const xy_pos_t &point) { return can_reach(point.x, point.y); }
+    };
+
+    #if NEEDS_THREE_PROBE_POINTS
+      // Retrieve three points to probe the bed. Any type exposing set(X,Y) may be used.
+      template <typename T>
+      static void get_three_points(T points[3]) {
+        #if HAS_FIXED_3POINT
+          #define VALIDATE_PROBE_PT(N) static_assert(Probe::build_time::can_reach(xy_pos_t{PROBE_PT_##N##_X, PROBE_PT_##N##_Y}), \
+            "PROBE_PT_" STRINGIFY(N) "_(X|Y) is unreachable using default NOZZLE_TO_PROBE_OFFSET and PROBING_MARGIN");
+          VALIDATE_PROBE_PT(1); VALIDATE_PROBE_PT(2); VALIDATE_PROBE_PT(3);
+          points[0].set(PROBE_PT_1_X, PROBE_PT_1_Y);
+          points[1].set(PROBE_PT_2_X, PROBE_PT_2_Y);
+          points[2].set(PROBE_PT_3_X, PROBE_PT_3_Y);
+        #else
+          #if IS_KINEMATIC
+            constexpr float SIN0 = 0.0, SIN120 = 0.866025, SIN240 = -0.866025,
+                            COS0 = 1.0, COS120 = -0.5    , COS240 = -0.5;
+            points[0].set((X_CENTER) + probe_radius() * COS0,   (Y_CENTER) + probe_radius() * SIN0);
+            points[1].set((X_CENTER) + probe_radius() * COS120, (Y_CENTER) + probe_radius() * SIN120);
+            points[2].set((X_CENTER) + probe_radius() * COS240, (Y_CENTER) + probe_radius() * SIN240);
+          #else
+            points[0].set(min_x(), min_y());
+            points[1].set(max_x(), min_y());
+            points[2].set((min_x() + max_x()) / 2, max_y());
+          #endif
+        #endif
+      }
+    #endif
+
+  #endif // HAS_BED_PROBE
+
+  #if HAS_Z_SERVO_PROBE
+    static void servo_probe_init();
+  #endif
+
+  #if HAS_QUIET_PROBING
+    static void set_probing_paused(const bool p);
+  #endif
+
+  #if ENABLED(PROBE_TARE)
+    static void tare_init();
+    static bool tare();
+  #endif
+
+private:
+  static bool probe_down_to_z(const float z, const feedRate_t fr_mm_s);
+  static void do_z_raise(const float z_raise);
+  static float run_z_probe(const bool sanity_check=true);
+};
+
+extern Probe probe;
diff --git a/Marlin/src/module/scara.cpp b/Marlin/src/module/scara.cpp
new file mode 100644
index 0000000..e4b2f0b
--- /dev/null
+++ b/Marlin/src/module/scara.cpp
@@ -0,0 +1,164 @@
+/**
+ * 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/>.
+ *
+ */
+
+/**
+ * scara.cpp
+ */
+
+#include "../inc/MarlinConfig.h"
+
+#if IS_SCARA
+
+#include "scara.h"
+#include "motion.h"
+#include "planner.h"
+
+float delta_segments_per_second = SCARA_SEGMENTS_PER_SECOND;
+
+void scara_set_axis_is_at_home(const AxisEnum axis) {
+  if (axis == Z_AXIS)
+    current_position.z = Z_HOME_POS;
+  else {
+
+    /**
+     * SCARA homes XY at the same time
+     */
+    xyz_pos_t homeposition;
+    LOOP_XYZ(i) homeposition[i] = base_home_pos((AxisEnum)i);
+
+    #if ENABLED(MORGAN_SCARA)
+      // MORGAN_SCARA uses arm angles for AB home position
+      // SERIAL_ECHOLNPAIR("homeposition A:", homeposition.a, " B:", homeposition.b);
+      inverse_kinematics(homeposition);
+      forward_kinematics_SCARA(delta.a, delta.b);
+      current_position[axis] = cartes[axis];
+    #else
+      // MP_SCARA uses a Cartesian XY home position
+      // SERIAL_ECHOPGM("homeposition");
+      // SERIAL_ECHOLNPAIR_P(SP_X_LBL, homeposition.x, SP_Y_LBL, homeposition.y);
+      current_position[axis] = homeposition[axis];
+    #endif
+
+    // SERIAL_ECHOPGM("Cartesian");
+    // SERIAL_ECHOLNPAIR_P(SP_X_LBL, current_position.x, SP_Y_LBL, current_position.y);
+    update_software_endstops(axis);
+  }
+}
+
+static constexpr xy_pos_t scara_offset = { SCARA_OFFSET_X, SCARA_OFFSET_Y };
+
+/**
+ * Morgan SCARA Forward Kinematics. Results in 'cartes'.
+ * Maths and first version by QHARLEY.
+ * Integrated into Marlin and slightly restructured by Joachim Cerny.
+ */
+void forward_kinematics_SCARA(const float &a, const float &b) {
+
+  const float a_sin = sin(RADIANS(a)) * L1,
+              a_cos = cos(RADIANS(a)) * L1,
+              b_sin = sin(RADIANS(b)) * L2,
+              b_cos = cos(RADIANS(b)) * L2;
+
+  cartes.set(a_cos + b_cos + scara_offset.x,  // theta
+             a_sin + b_sin + scara_offset.y); // theta+phi
+
+  /*
+    SERIAL_ECHOLNPAIR(
+      "SCARA FK Angle a=", a,
+      " b=", b,
+      " a_sin=", a_sin,
+      " a_cos=", a_cos,
+      " b_sin=", b_sin,
+      " b_cos=", b_cos
+    );
+    SERIAL_ECHOLNPAIR(" cartes (X,Y) = "(cartes.x, ", ", cartes.y, ")");
+  //*/
+}
+
+void inverse_kinematics(const xyz_pos_t &raw) {
+
+  #if ENABLED(MORGAN_SCARA)
+    /**
+     * Morgan SCARA Inverse Kinematics. Results in 'delta'.
+     *
+     * See https://reprap.org/forum/read.php?185,283327
+     *
+     * Maths and first version by QHARLEY.
+     * Integrated into Marlin and slightly restructured by Joachim Cerny.
+     */
+    float C2, S2, SK1, SK2, THETA, PSI;
+
+    // Translate SCARA to standard XY with scaling factor
+    const xy_pos_t spos = raw - scara_offset;
+
+    const float H2 = HYPOT2(spos.x, spos.y);
+    if (L1 == L2)
+      C2 = H2 / L1_2_2 - 1;
+    else
+      C2 = (H2 - (L1_2 + L2_2)) / (2.0f * L1 * L2);
+
+    S2 = SQRT(1.0f - sq(C2));
+
+    // Unrotated Arm1 plus rotated Arm2 gives the distance from Center to End
+    SK1 = L1 + L2 * C2;
+
+    // Rotated Arm2 gives the distance from Arm1 to Arm2
+    SK2 = L2 * S2;
+
+    // Angle of Arm1 is the difference between Center-to-End angle and the Center-to-Elbow
+    THETA = ATAN2(SK1, SK2) - ATAN2(spos.x, spos.y);
+
+    // Angle of Arm2
+    PSI = ATAN2(S2, C2);
+
+    delta.set(DEGREES(THETA), DEGREES(THETA + PSI), raw.z);
+
+    /*
+      DEBUG_POS("SCARA IK", raw);
+      DEBUG_POS("SCARA IK", delta);
+      SERIAL_ECHOLNPAIR("  SCARA (x,y) ", sx, ",", sy, " C2=", C2, " S2=", S2, " Theta=", THETA, " Phi=", PHI);
+    //*/
+
+  #else // MP_SCARA
+
+    const float x = raw.x, y = raw.y, c = HYPOT(x, y),
+                THETA3 = ATAN2(y, x),
+                THETA1 = THETA3 + ACOS((sq(c) + sq(L1) - sq(L2)) / (2.0f * c * L1)),
+                THETA2 = THETA3 - ACOS((sq(c) + sq(L2) - sq(L1)) / (2.0f * c * L2));
+
+    delta.set(DEGREES(THETA1), DEGREES(THETA2), raw.z);
+
+    /*
+      DEBUG_POS("SCARA IK", raw);
+      DEBUG_POS("SCARA IK", delta);
+      SERIAL_ECHOLNPAIR("  SCARA (x,y) ", x, ",", y," Theta1=", THETA1, " Theta2=", THETA2);
+    //*/
+
+  #endif // MP_SCARA
+}
+
+void scara_report_positions() {
+  SERIAL_ECHOLNPAIR("SCARA Theta:", planner.get_axis_position_degrees(A_AXIS), "  Psi+Theta:", planner.get_axis_position_degrees(B_AXIS));
+  SERIAL_EOL();
+}
+
+#endif // IS_SCARA
diff --git a/Marlin/src/module/scara.h b/Marlin/src/module/scara.h
new file mode 100644
index 0000000..e2acaf3
--- /dev/null
+++ b/Marlin/src/module/scara.h
@@ -0,0 +1,42 @@
+/**
+ * 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
+
+/**
+ * scara.h - SCARA-specific functions
+ */
+
+#include "../core/macros.h"
+
+extern float delta_segments_per_second;
+
+// Float constants for SCARA calculations
+float constexpr L1 = SCARA_LINKAGE_1, L2 = SCARA_LINKAGE_2,
+                L1_2 = sq(float(L1)), L1_2_2 = 2.0 * L1_2,
+                L2_2 = sq(float(L2));
+
+void scara_set_axis_is_at_home(const AxisEnum axis);
+
+void inverse_kinematics(const xyz_pos_t &raw);
+void forward_kinematics_SCARA(const float &a, const float &b);
+
+void scara_report_positions();
diff --git a/Marlin/src/module/servo.cpp b/Marlin/src/module/servo.cpp
new file mode 100644
index 0000000..27e5a2a
--- /dev/null
+++ b/Marlin/src/module/servo.cpp
@@ -0,0 +1,56 @@
+/**
+ * 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/>.
+ *
+ */
+
+/**
+ * module/servo.cpp
+ */
+
+#include "../inc/MarlinConfig.h"
+
+#if HAS_SERVOS
+
+#include "servo.h"
+
+HAL_SERVO_LIB servo[NUM_SERVOS];
+
+TERN_(EDITABLE_SERVO_ANGLES, uint16_t servo_angles[NUM_SERVOS][2]);
+
+void servo_init() {
+  #if NUM_SERVOS >= 1 && HAS_SERVO_0
+    servo[0].attach(SERVO0_PIN);
+    servo[0].detach(); // Just set up the pin. We don't have a position yet. Don't move to a random position.
+  #endif
+  #if NUM_SERVOS >= 2 && HAS_SERVO_1
+    servo[1].attach(SERVO1_PIN);
+    servo[1].detach();
+  #endif
+  #if NUM_SERVOS >= 3 && HAS_SERVO_2
+    servo[2].attach(SERVO2_PIN);
+    servo[2].detach();
+  #endif
+  #if NUM_SERVOS >= 4 && HAS_SERVO_3
+    servo[3].attach(SERVO3_PIN);
+    servo[3].detach();
+  #endif
+}
+
+#endif // HAS_SERVOS
diff --git a/Marlin/src/module/servo.h b/Marlin/src/module/servo.h
new file mode 100644
index 0000000..29bd3b8
--- /dev/null
+++ b/Marlin/src/module/servo.h
@@ -0,0 +1,115 @@
+/**
+ * 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
+
+/**
+ * module/servo.h
+ */
+
+#include "../inc/MarlinConfig.h"
+#include "../HAL/shared/servo.h"
+
+#if HAS_SERVO_ANGLES
+
+  #if ENABLED(SWITCHING_EXTRUDER)
+    // Switching extruder can have 2 or 4 angles
+    #if EXTRUDERS > 3
+      #define REQ_ANGLES 4
+    #else
+      #define REQ_ANGLES 2
+    #endif
+    constexpr uint16_t sase[] = SWITCHING_EXTRUDER_SERVO_ANGLES;
+    static_assert(COUNT(sase) == REQ_ANGLES, "SWITCHING_EXTRUDER_SERVO_ANGLES needs " STRINGIFY(REQ_ANGLES) " angles.");
+  #else
+    constexpr uint16_t sase[4] = { 0 };
+  #endif
+
+  #if ENABLED(SWITCHING_NOZZLE)
+    constexpr uint16_t sasn[] = SWITCHING_NOZZLE_SERVO_ANGLES;
+    static_assert(COUNT(sasn) == 2, "SWITCHING_NOZZLE_SERVO_ANGLES needs 2 angles.");
+  #else
+    constexpr uint16_t sasn[2] = { 0 };
+  #endif
+
+  #ifdef Z_PROBE_SERVO_NR
+    #if ENABLED(BLTOUCH)
+      #include "../feature/bltouch.h"
+      #undef Z_SERVO_ANGLES
+      #define Z_SERVO_ANGLES { BLTOUCH_DEPLOY, BLTOUCH_STOW }
+    #endif
+    constexpr uint16_t sazp[] = Z_SERVO_ANGLES;
+    static_assert(COUNT(sazp) == 2, "Z_SERVO_ANGLES needs 2 angles.");
+  #else
+    constexpr uint16_t sazp[2] = { 0 };
+  #endif
+
+  #ifndef SWITCHING_EXTRUDER_SERVO_NR
+    #define SWITCHING_EXTRUDER_SERVO_NR -1
+  #endif
+  #ifndef SWITCHING_EXTRUDER_E23_SERVO_NR
+    #define SWITCHING_EXTRUDER_E23_SERVO_NR -1
+  #endif
+  #ifndef SWITCHING_NOZZLE_SERVO_NR
+    #define SWITCHING_NOZZLE_SERVO_NR -1
+  #endif
+  #ifndef Z_PROBE_SERVO_NR
+    #define Z_PROBE_SERVO_NR -1
+  #endif
+
+  #define ASRC(N,I) (                                  \
+      N == SWITCHING_EXTRUDER_SERVO_NR     ? sase[I]   \
+    : N == SWITCHING_EXTRUDER_E23_SERVO_NR ? sase[I+2] \
+    : N == SWITCHING_NOZZLE_SERVO_NR       ? sasn[I]   \
+    : N == Z_PROBE_SERVO_NR                ? sazp[I]   \
+    : 0                                                )
+
+  #if ENABLED(EDITABLE_SERVO_ANGLES)
+    extern uint16_t servo_angles[NUM_SERVOS][2];
+    #define CONST_SERVO_ANGLES base_servo_angles
+  #else
+    #define CONST_SERVO_ANGLES servo_angles
+  #endif
+
+  constexpr uint16_t CONST_SERVO_ANGLES [NUM_SERVOS][2] = {
+      { ASRC(0,0), ASRC(0,1) }
+    #if NUM_SERVOS > 1
+      , { ASRC(1,0), ASRC(1,1) }
+      #if NUM_SERVOS > 2
+        , { ASRC(2,0), ASRC(2,1) }
+        #if NUM_SERVOS > 3
+          , { ASRC(3,0), ASRC(3,1) }
+        #endif
+      #endif
+    #endif
+  };
+
+  #if HAS_Z_SERVO_PROBE
+    #define DEPLOY_Z_SERVO() MOVE_SERVO(Z_PROBE_SERVO_NR, servo_angles[Z_PROBE_SERVO_NR][0])
+    #define STOW_Z_SERVO() MOVE_SERVO(Z_PROBE_SERVO_NR, servo_angles[Z_PROBE_SERVO_NR][1])
+  #endif
+
+#endif // HAS_SERVO_ANGLES
+
+#define MOVE_SERVO(I, P) servo[I].move(P)
+
+extern HAL_SERVO_LIB servo[NUM_SERVOS];
+extern void servo_init();
diff --git a/Marlin/src/module/settings.cpp b/Marlin/src/module/settings.cpp
new file mode 100644
index 0000000..6908635
--- /dev/null
+++ b/Marlin/src/module/settings.cpp
@@ -0,0 +1,3880 @@
+/**
+ * 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/>.
+ *
+ */
+
+/**
+ * settings.cpp
+ *
+ * Settings and EEPROM storage
+ *
+ * IMPORTANT:  Whenever there are changes made to the variables stored in EEPROM
+ * in the functions below, also increment the version number. This makes sure that
+ * the default values are used whenever there is a change to the data, to prevent
+ * wrong data being written to the variables.
+ *
+ * ALSO: Variables in the Store and Retrieve sections must be in the same order.
+ *       If a feature is disabled, some data must still be written that, when read,
+ *       either sets a Sane Default, or results in No Change to the existing value.
+ */
+
+// Change EEPROM version if the structure changes
+#define EEPROM_VERSION "V83"
+#define EEPROM_OFFSET 100
+
+// Check the integrity of data offsets.
+// Can be disabled for production build.
+//#define DEBUG_EEPROM_READWRITE
+
+#include "settings.h"
+
+#include "endstops.h"
+#include "planner.h"
+#include "stepper.h"
+#include "temperature.h"
+
+#include "../lcd/marlinui.h"
+#include "../libs/vector_3.h"   // for matrix_3x3
+#include "../gcode/gcode.h"
+#include "../MarlinCore.h"
+
+#if EITHER(EEPROM_SETTINGS, SD_FIRMWARE_UPDATE)
+  #include "../HAL/shared/eeprom_api.h"
+#endif
+
+#include "probe.h"
+
+#if HAS_LEVELING
+  #include "../feature/bedlevel/bedlevel.h"
+#endif
+
+#if ENABLED(Z_STEPPER_AUTO_ALIGN)
+  #include "../feature/z_stepper_align.h"
+#endif
+
+#if ENABLED(EXTENSIBLE_UI)
+  #include "../lcd/extui/ui_api.h"
+#endif
+
+#if HAS_SERVOS
+  #include "servo.h"
+#endif
+
+#if HAS_SERVOS && HAS_SERVO_ANGLES
+  #define EEPROM_NUM_SERVOS NUM_SERVOS
+#else
+  #define EEPROM_NUM_SERVOS NUM_SERVO_PLUGS
+#endif
+
+#include "../feature/fwretract.h"
+
+#if ENABLED(POWER_LOSS_RECOVERY)
+  #include "../feature/powerloss.h"
+#endif
+
+#if HAS_POWER_MONITOR
+  #include "../feature/power_monitor.h"
+#endif
+
+#include "../feature/pause.h"
+
+#if ENABLED(BACKLASH_COMPENSATION)
+  #include "../feature/backlash.h"
+#endif
+
+#if HAS_FILAMENT_SENSOR
+  #include "../feature/runout.h"
+  #ifndef FIL_RUNOUT_ENABLED_DEFAULT
+    #define FIL_RUNOUT_ENABLED_DEFAULT true
+  #endif
+#endif
+
+#if ENABLED(EXTRA_LIN_ADVANCE_K)
+  extern float other_extruder_advance_K[EXTRUDERS];
+#endif
+
+#if HAS_MULTI_EXTRUDER
+  #include "tool_change.h"
+  void M217_report(const bool eeprom);
+#endif
+
+#if ENABLED(BLTOUCH)
+  #include "../feature/bltouch.h"
+#endif
+
+#if HAS_TRINAMIC_CONFIG
+  #include "stepper/indirection.h"
+  #include "../feature/tmc_util.h"
+#endif
+
+#if ENABLED(PROBE_TEMP_COMPENSATION)
+  #include "../feature/probe_temp_comp.h"
+#endif
+
+#include "../feature/controllerfan.h"
+#if ENABLED(CONTROLLER_FAN_EDITABLE)
+  void M710_report(const bool forReplay);
+#endif
+
+#if ENABLED(CASE_LIGHT_ENABLE)
+  #include "../feature/caselight.h"
+#endif
+
+#if ENABLED(PASSWORD_FEATURE)
+  #include "../feature/password/password.h"
+#endif
+
+#if ENABLED(TOUCH_SCREEN_CALIBRATION)
+  #include "../lcd/tft_io/touch_calibration.h"
+#endif
+
+#if HAS_ETHERNET
+  #include "../feature/ethernet.h"
+#endif
+
+#if ENABLED(SOUND_MENU_ITEM)
+  #include "../libs/buzzer.h"
+#endif
+
+#pragma pack(push, 1) // No padding between variables
+
+#if HAS_ETHERNET
+  void ETH0_report();
+  void MAC_report();
+  void M552_report();
+  void M553_report();
+  void M554_report();
+#endif
+
+typedef struct { uint16_t X, Y, Z, X2, Y2, Z2, Z3, Z4, E0, E1, E2, E3, E4, E5, E6, E7; } tmc_stepper_current_t;
+typedef struct { uint32_t X, Y, Z, X2, Y2, Z2, Z3, Z4, E0, E1, E2, E3, E4, E5, E6, E7; } tmc_hybrid_threshold_t;
+typedef struct {  int16_t X, Y, Z, X2, Y2, Z2, Z3, Z4;                                 } tmc_sgt_t;
+typedef struct {     bool X, Y, Z, X2, Y2, Z2, Z3, Z4, E0, E1, E2, E3, E4, E5, E6, E7; } tmc_stealth_enabled_t;
+
+// Limit an index to an array size
+#define ALIM(I,ARR) _MIN(I, (signed)COUNT(ARR) - 1)
+
+// Defaults for reset / fill in on load
+static const uint32_t   _DMA[] PROGMEM = DEFAULT_MAX_ACCELERATION;
+static const float     _DASU[] PROGMEM = DEFAULT_AXIS_STEPS_PER_UNIT;
+static const feedRate_t _DMF[] PROGMEM = DEFAULT_MAX_FEEDRATE;
+
+/**
+ * Current EEPROM Layout
+ *
+ * Keep this data structure up to date so
+ * EEPROM size is known at compile time!
+ */
+typedef struct SettingsDataStruct {
+  char      version[4];                                 // Vnn\0
+  uint16_t  crc;                                        // Data Checksum
+
+  //
+  // DISTINCT_E_FACTORS
+  //
+  uint8_t   esteppers;                                  // XYZE_N - XYZ
+
+  planner_settings_t planner_settings;
+
+  xyze_float_t planner_max_jerk;                        // M205 XYZE  planner.max_jerk
+  float planner_junction_deviation_mm;                  // M205 J     planner.junction_deviation_mm
+
+  xyz_pos_t home_offset;                                // M206 XYZ / M665 TPZ
+
+  #if HAS_HOTEND_OFFSET
+    xyz_pos_t hotend_offset[HOTENDS - 1];               // M218 XYZ
+  #endif
+
+  //
+  // FILAMENT_RUNOUT_SENSOR
+  //
+  bool runout_sensor_enabled;                           // M412 S
+  float runout_distance_mm;                             // M412 D
+
+  //
+  // ENABLE_LEVELING_FADE_HEIGHT
+  //
+  float planner_z_fade_height;                          // M420 Zn  planner.z_fade_height
+
+  //
+  // MESH_BED_LEVELING
+  //
+  float mbl_z_offset;                                   // mbl.z_offset
+  uint8_t mesh_num_x, mesh_num_y;                       // GRID_MAX_POINTS_X, GRID_MAX_POINTS_Y
+  float mbl_z_values[TERN(MESH_BED_LEVELING, GRID_MAX_POINTS_X, 3)]   // mbl.z_values
+                    [TERN(MESH_BED_LEVELING, GRID_MAX_POINTS_Y, 3)];
+
+  //
+  // HAS_BED_PROBE
+  //
+
+  xyz_pos_t probe_offset;
+
+  //
+  // ABL_PLANAR
+  //
+  matrix_3x3 planner_bed_level_matrix;                  // planner.bed_level_matrix
+
+  //
+  // AUTO_BED_LEVELING_BILINEAR
+  //
+  uint8_t grid_max_x, grid_max_y;                       // GRID_MAX_POINTS_X, GRID_MAX_POINTS_Y
+  xy_pos_t bilinear_grid_spacing, bilinear_start;       // G29 L F
+  #if ENABLED(AUTO_BED_LEVELING_BILINEAR)
+    bed_mesh_t z_values;                                // G29
+  #else
+    float z_values[3][3];
+  #endif
+
+  //
+  // AUTO_BED_LEVELING_UBL
+  //
+  bool planner_leveling_active;                         // M420 S  planner.leveling_active
+  int8_t ubl_storage_slot;                              // ubl.storage_slot
+
+  //
+  // SERVO_ANGLES
+  //
+  uint16_t servo_angles[EEPROM_NUM_SERVOS][2];          // M281 P L U
+
+  //
+  // Temperature first layer compensation values
+  //
+  #if ENABLED(PROBE_TEMP_COMPENSATION)
+    int16_t z_offsets_probe[COUNT(temp_comp.z_offsets_probe)], // M871 P I V
+            z_offsets_bed[COUNT(temp_comp.z_offsets_bed)]      // M871 B I V
+            #if ENABLED(USE_TEMP_EXT_COMPENSATION)
+              , z_offsets_ext[COUNT(temp_comp.z_offsets_ext)]  // M871 E I V
+            #endif
+            ;
+  #endif
+
+  //
+  // BLTOUCH
+  //
+  bool bltouch_last_written_mode;
+
+  //
+  // DELTA / [XYZ]_DUAL_ENDSTOPS
+  //
+  #if ENABLED(DELTA)
+    float delta_height;                                 // M666 H
+    abc_float_t delta_endstop_adj;                      // M666 X Y Z
+    float delta_radius,                                 // M665 R
+          delta_diagonal_rod,                           // M665 L
+          delta_segments_per_second;                    // M665 S
+    abc_float_t delta_tower_angle_trim,                 // M665 X Y Z
+                delta_diagonal_rod_trim;                // M665 A B C
+  #elif HAS_EXTRA_ENDSTOPS
+    float x2_endstop_adj,                               // M666 X
+          y2_endstop_adj,                               // M666 Y
+          z2_endstop_adj,                               // M666 (S2) Z
+          z3_endstop_adj,                               // M666 (S3) Z
+          z4_endstop_adj;                               // M666 (S4) Z
+  #endif
+
+  //
+  // Z_STEPPER_AUTO_ALIGN, Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS
+  //
+  #if ENABLED(Z_STEPPER_AUTO_ALIGN)
+    xy_pos_t z_stepper_align_xy[NUM_Z_STEPPER_DRIVERS];             // M422 S X Y
+    #if ENABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS)
+      xy_pos_t z_stepper_align_stepper_xy[NUM_Z_STEPPER_DRIVERS];   // M422 W X Y
+    #endif
+  #endif
+
+  //
+  // Material Presets
+  //
+  #if PREHEAT_COUNT
+    preheat_t ui_material_preset[PREHEAT_COUNT];        // M145 S0 H B F
+  #endif
+
+  //
+  // PIDTEMP
+  //
+  PIDCF_t hotendPID[HOTENDS];                           // M301 En PIDCF / M303 En U
+  int16_t lpq_len;                                      // M301 L
+
+  //
+  // PIDTEMPBED
+  //
+  PID_t bedPID;                                         // M304 PID / M303 E-1 U
+
+  //
+  // User-defined Thermistors
+  //
+  #if HAS_USER_THERMISTORS
+    user_thermistor_t user_thermistor[USER_THERMISTORS]; // M305 P0 R4700 T100000 B3950
+  #endif
+
+  //
+  // Power monitor
+  //
+  uint8_t power_monitor_flags;                          // M430 I V W
+
+  //
+  // HAS_LCD_CONTRAST
+  //
+  int16_t lcd_contrast;                                 // M250 C
+
+  //
+  // Controller fan settings
+  //
+  controllerFan_settings_t controllerFan_settings;      // M710
+
+  //
+  // POWER_LOSS_RECOVERY
+  //
+  bool recovery_enabled;                                // M413 S
+
+  //
+  // FWRETRACT
+  //
+  fwretract_settings_t fwretract_settings;              // M207 S F Z W, M208 S F W R
+  bool autoretract_enabled;                             // M209 S
+
+  //
+  // !NO_VOLUMETRIC
+  //
+  bool parser_volumetric_enabled;                       // M200 S  parser.volumetric_enabled
+  float planner_filament_size[EXTRUDERS];               // M200 T D  planner.filament_size[]
+  float planner_volumetric_extruder_limit[EXTRUDERS];   // M200 T L  planner.volumetric_extruder_limit[]
+
+  //
+  // HAS_TRINAMIC_CONFIG
+  //
+  tmc_stepper_current_t tmc_stepper_current;            // M906 X Y Z X2 Y2 Z2 Z3 Z4 E0 E1 E2 E3 E4 E5
+  tmc_hybrid_threshold_t tmc_hybrid_threshold;          // M913 X Y Z X2 Y2 Z2 Z3 Z4 E0 E1 E2 E3 E4 E5
+  tmc_sgt_t tmc_sgt;                                    // M914 X Y Z X2 Y2 Z2 Z3 Z4
+  tmc_stealth_enabled_t tmc_stealth_enabled;            // M569 X Y Z X2 Y2 Z2 Z3 Z4 E0 E1 E2 E3 E4 E5
+
+  //
+  // LIN_ADVANCE
+  //
+  float planner_extruder_advance_K[_MAX(EXTRUDERS, 1)]; // M900 K  planner.extruder_advance_K
+
+  //
+  // HAS_MOTOR_CURRENT_PWM
+  //
+  #ifndef MOTOR_CURRENT_COUNT
+    #define MOTOR_CURRENT_COUNT 3
+  #endif
+  uint32_t motor_current_setting[MOTOR_CURRENT_COUNT];  // M907 X Z E
+
+  //
+  // CNC_COORDINATE_SYSTEMS
+  //
+  xyz_pos_t coordinate_system[MAX_COORDINATE_SYSTEMS];  // G54-G59.3
+
+  //
+  // SKEW_CORRECTION
+  //
+  skew_factor_t planner_skew_factor;                    // M852 I J K  planner.skew_factor
+
+  //
+  // ADVANCED_PAUSE_FEATURE
+  //
+  #if EXTRUDERS
+    fil_change_settings_t fc_settings[EXTRUDERS];       // M603 T U L
+  #endif
+
+  //
+  // Tool-change settings
+  //
+  #if HAS_MULTI_EXTRUDER
+    toolchange_settings_t toolchange_settings;          // M217 S P R
+  #endif
+
+  //
+  // BACKLASH_COMPENSATION
+  //
+  xyz_float_t backlash_distance_mm;                     // M425 X Y Z
+  uint8_t backlash_correction;                          // M425 F
+  float backlash_smoothing_mm;                          // M425 S
+
+  //
+  // EXTENSIBLE_UI
+  //
+  #if ENABLED(EXTENSIBLE_UI)
+    // This is a significant hardware change; don't reserve space when not present
+    uint8_t extui_data[ExtUI::eeprom_data_size];
+  #endif
+
+  //
+  // CASELIGHT_USES_BRIGHTNESS
+  //
+  #if CASELIGHT_USES_BRIGHTNESS
+    uint8_t caselight_brightness;                        // M355 P
+  #endif
+
+  //
+  // PASSWORD_FEATURE
+  //
+  #if ENABLED(PASSWORD_FEATURE)
+    bool password_is_set;
+    uint32_t password_value;
+  #endif
+
+  //
+  // TOUCH_SCREEN_CALIBRATION
+  //
+  #if ENABLED(TOUCH_SCREEN_CALIBRATION)
+    touch_calibration_t touch_calibration_data;
+  #endif
+
+  // Ethernet settings
+  #if HAS_ETHERNET
+    bool ethernet_hardware_enabled;                     // M552 S
+    uint32_t ethernet_ip,                               // M552 P
+             ethernet_dns,
+             ethernet_gateway,                          // M553 P
+             ethernet_subnet;                           // M554 P
+  #endif
+
+  //
+  // Buzzer enable/disable
+  //
+  #if ENABLED(SOUND_MENU_ITEM)
+    bool buzzer_enabled;
+  #endif
+
+  #if HAS_MULTI_LANGUAGE
+    uint8_t ui_language;                                // M414 S
+  #endif
+
+} SettingsData;
+
+//static_assert(sizeof(SettingsData) <= MARLIN_EEPROM_SIZE, "EEPROM too small to contain SettingsData!");
+
+MarlinSettings settings;
+
+uint16_t MarlinSettings::datasize() { return sizeof(SettingsData); }
+
+/**
+ * Post-process after Retrieve or Reset
+ */
+
+#if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
+  float new_z_fade_height;
+#endif
+
+void MarlinSettings::postprocess() {
+  xyze_pos_t oldpos = current_position;
+
+  // steps per s2 needs to be updated to agree with units per s2
+  planner.reset_acceleration_rates();
+
+  // Make sure delta kinematics are updated before refreshing the
+  // planner position so the stepper counts will be set correctly.
+  TERN_(DELTA, recalc_delta_settings());
+
+  TERN_(PIDTEMP, thermalManager.updatePID());
+
+  #if DISABLED(NO_VOLUMETRICS)
+    planner.calculate_volumetric_multipliers();
+  #elif EXTRUDERS
+    for (uint8_t i = COUNT(planner.e_factor); i--;)
+      planner.refresh_e_factor(i);
+  #endif
+
+  // Software endstops depend on home_offset
+  LOOP_XYZ(i) {
+    update_workspace_offset((AxisEnum)i);
+    update_software_endstops((AxisEnum)i);
+  }
+
+  TERN_(ENABLE_LEVELING_FADE_HEIGHT, set_z_fade_height(new_z_fade_height, false)); // false = no report
+
+  TERN_(AUTO_BED_LEVELING_BILINEAR, refresh_bed_level());
+
+  TERN_(HAS_MOTOR_CURRENT_PWM, stepper.refresh_motor_power());
+
+  TERN_(FWRETRACT, fwretract.refresh_autoretract());
+
+  TERN_(HAS_LINEAR_E_JERK, planner.recalculate_max_e_jerk());
+
+  TERN_(CASELIGHT_USES_BRIGHTNESS, caselight.update_brightness());
+
+  // Refresh steps_to_mm with the reciprocal of axis_steps_per_mm
+  // and init stepper.count[], planner.position[] with current_position
+  planner.refresh_positioning();
+
+  // Various factors can change the current position
+  if (oldpos != current_position)
+    report_current_position();
+}
+
+#if BOTH(PRINTCOUNTER, EEPROM_SETTINGS)
+  #include "printcounter.h"
+  static_assert(
+    !WITHIN(STATS_EEPROM_ADDRESS, EEPROM_OFFSET, EEPROM_OFFSET + sizeof(SettingsData)) &&
+    !WITHIN(STATS_EEPROM_ADDRESS + sizeof(printStatistics), EEPROM_OFFSET, EEPROM_OFFSET + sizeof(SettingsData)),
+    "STATS_EEPROM_ADDRESS collides with EEPROM settings storage."
+  );
+#endif
+
+#if ENABLED(SD_FIRMWARE_UPDATE)
+
+  #if ENABLED(EEPROM_SETTINGS)
+    static_assert(
+      !WITHIN(SD_FIRMWARE_UPDATE_EEPROM_ADDR, EEPROM_OFFSET, EEPROM_OFFSET + sizeof(SettingsData)),
+      "SD_FIRMWARE_UPDATE_EEPROM_ADDR collides with EEPROM settings storage."
+    );
+  #endif
+
+  bool MarlinSettings::sd_update_status() {
+    uint8_t val;
+    persistentStore.read_data(SD_FIRMWARE_UPDATE_EEPROM_ADDR, &val);
+    return (val == SD_FIRMWARE_UPDATE_ACTIVE_VALUE);
+  }
+
+  bool MarlinSettings::set_sd_update_status(const bool enable) {
+    if (enable != sd_update_status())
+      persistentStore.write_data(
+        SD_FIRMWARE_UPDATE_EEPROM_ADDR,
+        enable ? SD_FIRMWARE_UPDATE_ACTIVE_VALUE : SD_FIRMWARE_UPDATE_INACTIVE_VALUE
+      );
+    return true;
+  }
+
+#endif // SD_FIRMWARE_UPDATE
+
+#ifdef ARCHIM2_SPI_FLASH_EEPROM_BACKUP_SIZE
+  static_assert(EEPROM_OFFSET + sizeof(SettingsData) < ARCHIM2_SPI_FLASH_EEPROM_BACKUP_SIZE,
+                "ARCHIM2_SPI_FLASH_EEPROM_BACKUP_SIZE is insufficient to capture all EEPROM data.");
+#endif
+
+#define DEBUG_OUT ENABLED(DEBUG_LEVELING_FEATURE)
+#include "../core/debug_out.h"
+
+#if ENABLED(EEPROM_SETTINGS)
+
+  #define EEPROM_START()          if (!persistentStore.access_start()) { SERIAL_ECHO_MSG("No EEPROM."); return false; } \
+                                  int eeprom_index = EEPROM_OFFSET
+  #define EEPROM_FINISH()         persistentStore.access_finish()
+  #define EEPROM_SKIP(VAR)        (eeprom_index += sizeof(VAR))
+  #define EEPROM_WRITE(VAR)       do{ persistentStore.write_data(eeprom_index, (uint8_t*)&VAR, sizeof(VAR), &working_crc);              }while(0)
+  #define EEPROM_READ(VAR)        do{ persistentStore.read_data(eeprom_index, (uint8_t*)&VAR, sizeof(VAR), &working_crc, !validating);  }while(0)
+  #define EEPROM_READ_ALWAYS(VAR) do{ persistentStore.read_data(eeprom_index, (uint8_t*)&VAR, sizeof(VAR), &working_crc);               }while(0)
+  #define EEPROM_ASSERT(TST,ERR)  do{ if (!(TST)) { SERIAL_ERROR_MSG(ERR); eeprom_error = true; } }while(0)
+
+  #if ENABLED(DEBUG_EEPROM_READWRITE)
+    #define _FIELD_TEST(FIELD) \
+      EEPROM_ASSERT( \
+        eeprom_error || eeprom_index == offsetof(SettingsData, FIELD) + EEPROM_OFFSET, \
+        "Field " STRINGIFY(FIELD) " mismatch." \
+      )
+  #else
+    #define _FIELD_TEST(FIELD) NOOP
+  #endif
+
+  const char version[4] = EEPROM_VERSION;
+
+  bool MarlinSettings::eeprom_error, MarlinSettings::validating;
+
+  bool MarlinSettings::size_error(const uint16_t size) {
+    if (size != datasize()) {
+      DEBUG_ERROR_MSG("EEPROM datasize error.");
+      return true;
+    }
+    return false;
+  }
+
+  /**
+   * M500 - Store Configuration
+   */
+  bool MarlinSettings::save() {
+    float dummyf = 0;
+    char ver[4] = "ERR";
+
+    uint16_t working_crc = 0;
+
+    EEPROM_START();
+
+    eeprom_error = false;
+
+    // Write or Skip version. (Flash doesn't allow rewrite without erase.)
+    TERN(FLASH_EEPROM_EMULATION, EEPROM_SKIP, EEPROM_WRITE)(ver);
+
+    EEPROM_SKIP(working_crc); // Skip the checksum slot
+
+    working_crc = 0; // clear before first "real data"
+
+    _FIELD_TEST(esteppers);
+
+    const uint8_t esteppers = COUNT(planner.settings.axis_steps_per_mm) - XYZ;
+    EEPROM_WRITE(esteppers);
+
+    //
+    // Planner Motion
+    //
+    {
+      EEPROM_WRITE(planner.settings);
+
+      #if HAS_CLASSIC_JERK
+        EEPROM_WRITE(planner.max_jerk);
+        #if HAS_LINEAR_E_JERK
+          dummyf = float(DEFAULT_EJERK);
+          EEPROM_WRITE(dummyf);
+        #endif
+      #else
+        const xyze_pos_t planner_max_jerk = { 10, 10, 0.4, float(DEFAULT_EJERK) };
+        EEPROM_WRITE(planner_max_jerk);
+      #endif
+
+      TERN_(CLASSIC_JERK, dummyf = 0.02f);
+      EEPROM_WRITE(TERN(CLASSIC_JERK, dummyf, planner.junction_deviation_mm));
+    }
+
+    //
+    // Home Offset
+    //
+    {
+      _FIELD_TEST(home_offset);
+
+      #if HAS_SCARA_OFFSET
+        EEPROM_WRITE(scara_home_offset);
+      #else
+        #if !HAS_HOME_OFFSET
+          const xyz_pos_t home_offset{0};
+        #endif
+        EEPROM_WRITE(home_offset);
+      #endif
+    }
+
+    //
+    // Hotend Offsets, if any
+    //
+    {
+      #if HAS_HOTEND_OFFSET
+        // Skip hotend 0 which must be 0
+        LOOP_S_L_N(e, 1, HOTENDS)
+          EEPROM_WRITE(hotend_offset[e]);
+      #endif
+    }
+
+    //
+    // Filament Runout Sensor
+    //
+    {
+      #if HAS_FILAMENT_SENSOR
+        const bool &runout_sensor_enabled = runout.enabled;
+      #else
+        constexpr int8_t runout_sensor_enabled = -1;
+      #endif
+      _FIELD_TEST(runout_sensor_enabled);
+      EEPROM_WRITE(runout_sensor_enabled);
+
+      #if HAS_FILAMENT_RUNOUT_DISTANCE
+        const float &runout_distance_mm = runout.runout_distance();
+      #else
+        constexpr float runout_distance_mm = 0;
+      #endif
+      EEPROM_WRITE(runout_distance_mm);
+    }
+
+    //
+    // Global Leveling
+    //
+    {
+      const float zfh = TERN(ENABLE_LEVELING_FADE_HEIGHT, planner.z_fade_height, (DEFAULT_LEVELING_FADE_HEIGHT));
+      EEPROM_WRITE(zfh);
+    }
+
+    //
+    // Mesh Bed Leveling
+    //
+    {
+      #if ENABLED(MESH_BED_LEVELING)
+        static_assert(
+          sizeof(mbl.z_values) == (GRID_MAX_POINTS) * sizeof(mbl.z_values[0][0]),
+          "MBL Z array is the wrong size."
+        );
+      #else
+        dummyf = 0;
+      #endif
+
+      const uint8_t mesh_num_x = TERN(MESH_BED_LEVELING, GRID_MAX_POINTS_X, 3),
+                    mesh_num_y = TERN(MESH_BED_LEVELING, GRID_MAX_POINTS_Y, 3);
+
+      EEPROM_WRITE(TERN(MESH_BED_LEVELING, mbl.z_offset, dummyf));
+      EEPROM_WRITE(mesh_num_x);
+      EEPROM_WRITE(mesh_num_y);
+
+      #if ENABLED(MESH_BED_LEVELING)
+        EEPROM_WRITE(mbl.z_values);
+      #else
+        for (uint8_t q = mesh_num_x * mesh_num_y; q--;) EEPROM_WRITE(dummyf);
+      #endif
+    }
+
+    //
+    // Probe XYZ Offsets
+    //
+    {
+      _FIELD_TEST(probe_offset);
+      #if HAS_BED_PROBE
+        const xyz_pos_t &zpo = probe.offset;
+      #else
+        constexpr xyz_pos_t zpo{0};
+      #endif
+      EEPROM_WRITE(zpo);
+    }
+
+    //
+    // Planar Bed Leveling matrix
+    //
+    {
+      #if ABL_PLANAR
+        EEPROM_WRITE(planner.bed_level_matrix);
+      #else
+        dummyf = 0;
+        for (uint8_t q = 9; q--;) EEPROM_WRITE(dummyf);
+      #endif
+    }
+
+    //
+    // Bilinear Auto Bed Leveling
+    //
+    {
+      #if ENABLED(AUTO_BED_LEVELING_BILINEAR)
+        static_assert(
+          sizeof(z_values) == (GRID_MAX_POINTS) * sizeof(z_values[0][0]),
+          "Bilinear Z array is the wrong size."
+        );
+      #else
+        const xy_pos_t bilinear_start{0}, bilinear_grid_spacing{0};
+      #endif
+
+      const uint8_t grid_max_x = TERN(AUTO_BED_LEVELING_BILINEAR, GRID_MAX_POINTS_X, 3),
+                    grid_max_y = TERN(AUTO_BED_LEVELING_BILINEAR, GRID_MAX_POINTS_Y, 3);
+      EEPROM_WRITE(grid_max_x);
+      EEPROM_WRITE(grid_max_y);
+      EEPROM_WRITE(bilinear_grid_spacing);
+      EEPROM_WRITE(bilinear_start);
+
+      #if ENABLED(AUTO_BED_LEVELING_BILINEAR)
+        EEPROM_WRITE(z_values);              // 9-256 floats
+      #else
+        dummyf = 0;
+        for (uint16_t q = grid_max_x * grid_max_y; q--;) EEPROM_WRITE(dummyf);
+      #endif
+    }
+
+    //
+    // Unified Bed Leveling
+    //
+    {
+      _FIELD_TEST(planner_leveling_active);
+      const bool ubl_active = TERN(AUTO_BED_LEVELING_UBL, planner.leveling_active, false);
+      const int8_t storage_slot = TERN(AUTO_BED_LEVELING_UBL, ubl.storage_slot, -1);
+      EEPROM_WRITE(ubl_active);
+      EEPROM_WRITE(storage_slot);
+    }
+
+    //
+    // Servo Angles
+    //
+    {
+      _FIELD_TEST(servo_angles);
+      #if !HAS_SERVO_ANGLES
+        uint16_t servo_angles[EEPROM_NUM_SERVOS][2] = { { 0, 0 } };
+      #endif
+      EEPROM_WRITE(servo_angles);
+    }
+
+    //
+    // Thermal first layer compensation values
+    //
+    #if ENABLED(PROBE_TEMP_COMPENSATION)
+      EEPROM_WRITE(temp_comp.z_offsets_probe);
+      EEPROM_WRITE(temp_comp.z_offsets_bed);
+      #if ENABLED(USE_TEMP_EXT_COMPENSATION)
+        EEPROM_WRITE(temp_comp.z_offsets_ext);
+      #endif
+    #else
+      // No placeholder data for this feature
+    #endif
+
+    //
+    // BLTOUCH
+    //
+    {
+      _FIELD_TEST(bltouch_last_written_mode);
+      const bool bltouch_last_written_mode = TERN(BLTOUCH, bltouch.last_written_mode, false);
+      EEPROM_WRITE(bltouch_last_written_mode);
+    }
+
+    //
+    // DELTA Geometry or Dual Endstops offsets
+    //
+    {
+      #if ENABLED(DELTA)
+
+        _FIELD_TEST(delta_height);
+
+        EEPROM_WRITE(delta_height);              // 1 float
+        EEPROM_WRITE(delta_endstop_adj);         // 3 floats
+        EEPROM_WRITE(delta_radius);              // 1 float
+        EEPROM_WRITE(delta_diagonal_rod);        // 1 float
+        EEPROM_WRITE(delta_segments_per_second); // 1 float
+        EEPROM_WRITE(delta_tower_angle_trim);    // 3 floats
+        EEPROM_WRITE(delta_diagonal_rod_trim);   // 3 floats
+
+      #elif HAS_EXTRA_ENDSTOPS
+
+        _FIELD_TEST(x2_endstop_adj);
+
+        // Write dual endstops in X, Y, Z order. Unused = 0.0
+        dummyf = 0;
+        EEPROM_WRITE(TERN(X_DUAL_ENDSTOPS, endstops.x2_endstop_adj, dummyf));   // 1 float
+        EEPROM_WRITE(TERN(Y_DUAL_ENDSTOPS, endstops.y2_endstop_adj, dummyf));   // 1 float
+        EEPROM_WRITE(TERN(Z_MULTI_ENDSTOPS, endstops.z2_endstop_adj, dummyf));  // 1 float
+
+        #if ENABLED(Z_MULTI_ENDSTOPS) && NUM_Z_STEPPER_DRIVERS >= 3
+          EEPROM_WRITE(endstops.z3_endstop_adj);   // 1 float
+        #else
+          EEPROM_WRITE(dummyf);
+        #endif
+
+        #if ENABLED(Z_MULTI_ENDSTOPS) && NUM_Z_STEPPER_DRIVERS >= 4
+          EEPROM_WRITE(endstops.z4_endstop_adj);   // 1 float
+        #else
+          EEPROM_WRITE(dummyf);
+        #endif
+
+      #endif
+    }
+
+    #if ENABLED(Z_STEPPER_AUTO_ALIGN)
+      EEPROM_WRITE(z_stepper_align.xy);
+      #if ENABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS)
+        EEPROM_WRITE(z_stepper_align.stepper_xy);
+      #endif
+    #endif
+
+    //
+    // LCD Preheat settings
+    //
+    #if PREHEAT_COUNT
+      _FIELD_TEST(ui_material_preset);
+      EEPROM_WRITE(ui.material_preset);
+    #endif
+
+    //
+    // PIDTEMP
+    //
+    {
+      _FIELD_TEST(hotendPID);
+      HOTEND_LOOP() {
+        PIDCF_t pidcf = {
+          #if DISABLED(PIDTEMP)
+            NAN, NAN, NAN,
+            NAN, NAN
+          #else
+                         PID_PARAM(Kp, e),
+            unscalePID_i(PID_PARAM(Ki, e)),
+            unscalePID_d(PID_PARAM(Kd, e)),
+                         PID_PARAM(Kc, e),
+                         PID_PARAM(Kf, e)
+          #endif
+        };
+        EEPROM_WRITE(pidcf);
+      }
+
+      _FIELD_TEST(lpq_len);
+      #if DISABLED(PID_EXTRUSION_SCALING)
+        const int16_t lpq_len = 20;
+      #endif
+      EEPROM_WRITE(TERN(PID_EXTRUSION_SCALING, thermalManager.lpq_len, lpq_len));
+    }
+
+    //
+    // PIDTEMPBED
+    //
+    {
+      _FIELD_TEST(bedPID);
+
+      const PID_t bed_pid = {
+        #if DISABLED(PIDTEMPBED)
+          NAN, NAN, NAN
+        #else
+          // Store the unscaled PID values
+          thermalManager.temp_bed.pid.Kp,
+          unscalePID_i(thermalManager.temp_bed.pid.Ki),
+          unscalePID_d(thermalManager.temp_bed.pid.Kd)
+        #endif
+      };
+      EEPROM_WRITE(bed_pid);
+    }
+
+    //
+    // User-defined Thermistors
+    //
+    #if HAS_USER_THERMISTORS
+    {
+      _FIELD_TEST(user_thermistor);
+      EEPROM_WRITE(thermalManager.user_thermistor);
+    }
+    #endif
+
+    //
+    // Power monitor
+    //
+    {
+      #if HAS_POWER_MONITOR
+        const uint8_t &power_monitor_flags = power_monitor.flags;
+      #else
+        constexpr uint8_t power_monitor_flags = 0x00;
+      #endif
+      _FIELD_TEST(power_monitor_flags);
+      EEPROM_WRITE(power_monitor_flags);
+    }
+
+    //
+    // LCD Contrast
+    //
+    {
+      _FIELD_TEST(lcd_contrast);
+
+      const int16_t lcd_contrast =
+        #if HAS_LCD_CONTRAST
+          ui.contrast
+        #else
+          127
+        #endif
+      ;
+      EEPROM_WRITE(lcd_contrast);
+    }
+
+    //
+    // Controller Fan
+    //
+    {
+      _FIELD_TEST(controllerFan_settings);
+      #if ENABLED(USE_CONTROLLER_FAN)
+        const controllerFan_settings_t &cfs = controllerFan.settings;
+      #else
+        controllerFan_settings_t cfs = controllerFan_defaults;
+      #endif
+      EEPROM_WRITE(cfs);
+    }
+
+    //
+    // Power-Loss Recovery
+    //
+    {
+      _FIELD_TEST(recovery_enabled);
+      const bool recovery_enabled = TERN(POWER_LOSS_RECOVERY, recovery.enabled, ENABLED(PLR_ENABLED_DEFAULT));
+      EEPROM_WRITE(recovery_enabled);
+    }
+
+    //
+    // Firmware Retraction
+    //
+    {
+      _FIELD_TEST(fwretract_settings);
+      #if DISABLED(FWRETRACT)
+        const fwretract_settings_t autoretract_defaults = { 3, 45, 0, 0, 0, 13, 0, 8 };
+      #endif
+      EEPROM_WRITE(TERN(FWRETRACT, fwretract.settings, autoretract_defaults));
+
+      #if DISABLED(FWRETRACT_AUTORETRACT)
+        const bool autoretract_enabled = false;
+      #endif
+      EEPROM_WRITE(TERN(FWRETRACT_AUTORETRACT, fwretract.autoretract_enabled, autoretract_enabled));
+    }
+
+    //
+    // Volumetric & Filament Size
+    //
+    {
+      _FIELD_TEST(parser_volumetric_enabled);
+
+      #if DISABLED(NO_VOLUMETRICS)
+
+        EEPROM_WRITE(parser.volumetric_enabled);
+        EEPROM_WRITE(planner.filament_size);
+        #if ENABLED(VOLUMETRIC_EXTRUDER_LIMIT)
+          EEPROM_WRITE(planner.volumetric_extruder_limit);
+        #else
+          dummyf = DEFAULT_VOLUMETRIC_EXTRUDER_LIMIT;
+          for (uint8_t q = EXTRUDERS; q--;) EEPROM_WRITE(dummyf);
+        #endif
+
+      #else
+
+        const bool volumetric_enabled = false;
+        EEPROM_WRITE(volumetric_enabled);
+        dummyf = DEFAULT_NOMINAL_FILAMENT_DIA;
+        for (uint8_t q = EXTRUDERS; q--;) EEPROM_WRITE(dummyf);
+        dummyf = DEFAULT_VOLUMETRIC_EXTRUDER_LIMIT;
+        for (uint8_t q = EXTRUDERS; q--;) EEPROM_WRITE(dummyf);
+
+      #endif
+    }
+
+    //
+    // TMC Configuration
+    //
+    {
+      _FIELD_TEST(tmc_stepper_current);
+
+      tmc_stepper_current_t tmc_stepper_current = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
+
+      #if HAS_TRINAMIC_CONFIG
+        #if AXIS_IS_TMC(X)
+          tmc_stepper_current.X = stepperX.getMilliamps();
+        #endif
+        #if AXIS_IS_TMC(Y)
+          tmc_stepper_current.Y = stepperY.getMilliamps();
+        #endif
+        #if AXIS_IS_TMC(Z)
+          tmc_stepper_current.Z = stepperZ.getMilliamps();
+        #endif
+        #if AXIS_IS_TMC(X2)
+          tmc_stepper_current.X2 = stepperX2.getMilliamps();
+        #endif
+        #if AXIS_IS_TMC(Y2)
+          tmc_stepper_current.Y2 = stepperY2.getMilliamps();
+        #endif
+        #if AXIS_IS_TMC(Z2)
+          tmc_stepper_current.Z2 = stepperZ2.getMilliamps();
+        #endif
+        #if AXIS_IS_TMC(Z3)
+          tmc_stepper_current.Z3 = stepperZ3.getMilliamps();
+        #endif
+        #if AXIS_IS_TMC(Z4)
+          tmc_stepper_current.Z4 = stepperZ4.getMilliamps();
+        #endif
+        #if AXIS_IS_TMC(E0)
+          tmc_stepper_current.E0 = stepperE0.getMilliamps();
+        #endif
+        #if AXIS_IS_TMC(E1)
+          tmc_stepper_current.E1 = stepperE1.getMilliamps();
+        #endif
+        #if AXIS_IS_TMC(E2)
+          tmc_stepper_current.E2 = stepperE2.getMilliamps();
+        #endif
+        #if AXIS_IS_TMC(E3)
+          tmc_stepper_current.E3 = stepperE3.getMilliamps();
+        #endif
+        #if AXIS_IS_TMC(E4)
+          tmc_stepper_current.E4 = stepperE4.getMilliamps();
+        #endif
+        #if AXIS_IS_TMC(E5)
+          tmc_stepper_current.E5 = stepperE5.getMilliamps();
+        #endif
+        #if AXIS_IS_TMC(E6)
+          tmc_stepper_current.E6 = stepperE6.getMilliamps();
+        #endif
+        #if AXIS_IS_TMC(E7)
+          tmc_stepper_current.E7 = stepperE7.getMilliamps();
+        #endif
+      #endif
+      EEPROM_WRITE(tmc_stepper_current);
+    }
+
+    //
+    // TMC Hybrid Threshold, and placeholder values
+    //
+    {
+      _FIELD_TEST(tmc_hybrid_threshold);
+
+      #if ENABLED(HYBRID_THRESHOLD)
+       tmc_hybrid_threshold_t tmc_hybrid_threshold = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
+        #if AXIS_HAS_STEALTHCHOP(X)
+          tmc_hybrid_threshold.X = stepperX.get_pwm_thrs();
+        #endif
+        #if AXIS_HAS_STEALTHCHOP(Y)
+          tmc_hybrid_threshold.Y = stepperY.get_pwm_thrs();
+        #endif
+        #if AXIS_HAS_STEALTHCHOP(Z)
+          tmc_hybrid_threshold.Z = stepperZ.get_pwm_thrs();
+        #endif
+        #if AXIS_HAS_STEALTHCHOP(X2)
+          tmc_hybrid_threshold.X2 = stepperX2.get_pwm_thrs();
+        #endif
+        #if AXIS_HAS_STEALTHCHOP(Y2)
+          tmc_hybrid_threshold.Y2 = stepperY2.get_pwm_thrs();
+        #endif
+        #if AXIS_HAS_STEALTHCHOP(Z2)
+          tmc_hybrid_threshold.Z2 = stepperZ2.get_pwm_thrs();
+        #endif
+        #if AXIS_HAS_STEALTHCHOP(Z3)
+          tmc_hybrid_threshold.Z3 = stepperZ3.get_pwm_thrs();
+        #endif
+        #if AXIS_HAS_STEALTHCHOP(Z4)
+          tmc_hybrid_threshold.Z4 = stepperZ4.get_pwm_thrs();
+        #endif
+        #if AXIS_HAS_STEALTHCHOP(E0)
+          tmc_hybrid_threshold.E0 = stepperE0.get_pwm_thrs();
+        #endif
+        #if AXIS_HAS_STEALTHCHOP(E1)
+          tmc_hybrid_threshold.E1 = stepperE1.get_pwm_thrs();
+        #endif
+        #if AXIS_HAS_STEALTHCHOP(E2)
+          tmc_hybrid_threshold.E2 = stepperE2.get_pwm_thrs();
+        #endif
+        #if AXIS_HAS_STEALTHCHOP(E3)
+          tmc_hybrid_threshold.E3 = stepperE3.get_pwm_thrs();
+        #endif
+        #if AXIS_HAS_STEALTHCHOP(E4)
+          tmc_hybrid_threshold.E4 = stepperE4.get_pwm_thrs();
+        #endif
+        #if AXIS_HAS_STEALTHCHOP(E5)
+          tmc_hybrid_threshold.E5 = stepperE5.get_pwm_thrs();
+        #endif
+        #if AXIS_HAS_STEALTHCHOP(E6)
+          tmc_hybrid_threshold.E6 = stepperE6.get_pwm_thrs();
+        #endif
+        #if AXIS_HAS_STEALTHCHOP(E7)
+          tmc_hybrid_threshold.E7 = stepperE7.get_pwm_thrs();
+        #endif
+      #else
+        const tmc_hybrid_threshold_t tmc_hybrid_threshold = {
+          .X  = 100, .Y  = 100, .Z  =   3,
+          .X2 = 100, .Y2 = 100, .Z2 =   3, .Z3 =   3, .Z4 = 3,
+          .E0 =  30, .E1 =  30, .E2 =  30,
+          .E3 =  30, .E4 =  30, .E5 =  30
+        };
+      #endif
+      EEPROM_WRITE(tmc_hybrid_threshold);
+    }
+
+    //
+    // TMC StallGuard threshold
+    //
+    {
+      tmc_sgt_t tmc_sgt{0};
+      #if USE_SENSORLESS
+        TERN_(X_SENSORLESS,  tmc_sgt.X  = stepperX.homing_threshold());
+        TERN_(X2_SENSORLESS, tmc_sgt.X2 = stepperX2.homing_threshold());
+        TERN_(Y_SENSORLESS,  tmc_sgt.Y  = stepperY.homing_threshold());
+        TERN_(Y2_SENSORLESS, tmc_sgt.Y2 = stepperY2.homing_threshold());
+        TERN_(Z_SENSORLESS,  tmc_sgt.Z  = stepperZ.homing_threshold());
+        TERN_(Z2_SENSORLESS, tmc_sgt.Z2 = stepperZ2.homing_threshold());
+        TERN_(Z3_SENSORLESS, tmc_sgt.Z3 = stepperZ3.homing_threshold());
+        TERN_(Z4_SENSORLESS, tmc_sgt.Z4 = stepperZ4.homing_threshold());
+      #endif
+      EEPROM_WRITE(tmc_sgt);
+    }
+
+    //
+    // TMC stepping mode
+    //
+    {
+      _FIELD_TEST(tmc_stealth_enabled);
+
+      tmc_stealth_enabled_t tmc_stealth_enabled = { false };
+      #if AXIS_HAS_STEALTHCHOP(X)
+        tmc_stealth_enabled.X = stepperX.get_stored_stealthChop();
+      #endif
+      #if AXIS_HAS_STEALTHCHOP(Y)
+        tmc_stealth_enabled.Y = stepperY.get_stored_stealthChop();
+      #endif
+      #if AXIS_HAS_STEALTHCHOP(Z)
+        tmc_stealth_enabled.Z = stepperZ.get_stored_stealthChop();
+      #endif
+      #if AXIS_HAS_STEALTHCHOP(X2)
+        tmc_stealth_enabled.X2 = stepperX2.get_stored_stealthChop();
+      #endif
+      #if AXIS_HAS_STEALTHCHOP(Y2)
+        tmc_stealth_enabled.Y2 = stepperY2.get_stored_stealthChop();
+      #endif
+      #if AXIS_HAS_STEALTHCHOP(Z2)
+        tmc_stealth_enabled.Z2 = stepperZ2.get_stored_stealthChop();
+      #endif
+      #if AXIS_HAS_STEALTHCHOP(Z3)
+        tmc_stealth_enabled.Z3 = stepperZ3.get_stored_stealthChop();
+      #endif
+      #if AXIS_HAS_STEALTHCHOP(Z4)
+        tmc_stealth_enabled.Z4 = stepperZ4.get_stored_stealthChop();
+      #endif
+      #if AXIS_HAS_STEALTHCHOP(E0)
+        tmc_stealth_enabled.E0 = stepperE0.get_stored_stealthChop();
+      #endif
+      #if AXIS_HAS_STEALTHCHOP(E1)
+        tmc_stealth_enabled.E1 = stepperE1.get_stored_stealthChop();
+      #endif
+      #if AXIS_HAS_STEALTHCHOP(E2)
+        tmc_stealth_enabled.E2 = stepperE2.get_stored_stealthChop();
+      #endif
+      #if AXIS_HAS_STEALTHCHOP(E3)
+        tmc_stealth_enabled.E3 = stepperE3.get_stored_stealthChop();
+      #endif
+      #if AXIS_HAS_STEALTHCHOP(E4)
+        tmc_stealth_enabled.E4 = stepperE4.get_stored_stealthChop();
+      #endif
+      #if AXIS_HAS_STEALTHCHOP(E5)
+        tmc_stealth_enabled.E5 = stepperE5.get_stored_stealthChop();
+      #endif
+      #if AXIS_HAS_STEALTHCHOP(E6)
+        tmc_stealth_enabled.E6 = stepperE6.get_stored_stealthChop();
+      #endif
+      #if AXIS_HAS_STEALTHCHOP(E7)
+        tmc_stealth_enabled.E7 = stepperE7.get_stored_stealthChop();
+      #endif
+      EEPROM_WRITE(tmc_stealth_enabled);
+    }
+
+    //
+    // Linear Advance
+    //
+    {
+      _FIELD_TEST(planner_extruder_advance_K);
+
+      #if ENABLED(LIN_ADVANCE)
+        EEPROM_WRITE(planner.extruder_advance_K);
+      #else
+        dummyf = 0;
+        for (uint8_t q = _MAX(EXTRUDERS, 1); q--;) EEPROM_WRITE(dummyf);
+      #endif
+    }
+
+    //
+    // Motor Current PWM
+    //
+    {
+      _FIELD_TEST(motor_current_setting);
+
+      #if HAS_MOTOR_CURRENT_SPI || HAS_MOTOR_CURRENT_PWM
+        EEPROM_WRITE(stepper.motor_current_setting);
+      #else
+        const uint32_t no_current[MOTOR_CURRENT_COUNT] = { 0 };
+        EEPROM_WRITE(no_current);
+      #endif
+    }
+
+    //
+    // CNC Coordinate Systems
+    //
+
+    _FIELD_TEST(coordinate_system);
+
+    #if DISABLED(CNC_COORDINATE_SYSTEMS)
+      const xyz_pos_t coordinate_system[MAX_COORDINATE_SYSTEMS] = { { 0 } };
+    #endif
+    EEPROM_WRITE(TERN(CNC_COORDINATE_SYSTEMS, gcode.coordinate_system, coordinate_system));
+
+    //
+    // Skew correction factors
+    //
+    _FIELD_TEST(planner_skew_factor);
+    EEPROM_WRITE(planner.skew_factor);
+
+    //
+    // Advanced Pause filament load & unload lengths
+    //
+    #if EXTRUDERS
+    {
+      #if DISABLED(ADVANCED_PAUSE_FEATURE)
+        const fil_change_settings_t fc_settings[EXTRUDERS] = { 0, 0 };
+      #endif
+      _FIELD_TEST(fc_settings);
+      EEPROM_WRITE(fc_settings);
+    }
+    #endif
+
+    //
+    // Multiple Extruders
+    //
+
+    #if HAS_MULTI_EXTRUDER
+      _FIELD_TEST(toolchange_settings);
+      EEPROM_WRITE(toolchange_settings);
+    #endif
+
+    //
+    // Backlash Compensation
+    //
+    {
+      #if ENABLED(BACKLASH_GCODE)
+        const xyz_float_t &backlash_distance_mm = backlash.distance_mm;
+        const uint8_t &backlash_correction = backlash.correction;
+      #else
+        const xyz_float_t backlash_distance_mm{0};
+        const uint8_t backlash_correction = 0;
+      #endif
+      #if ENABLED(BACKLASH_GCODE) && defined(BACKLASH_SMOOTHING_MM)
+        const float &backlash_smoothing_mm = backlash.smoothing_mm;
+      #else
+        const float backlash_smoothing_mm = 3;
+      #endif
+      _FIELD_TEST(backlash_distance_mm);
+      EEPROM_WRITE(backlash_distance_mm);
+      EEPROM_WRITE(backlash_correction);
+      EEPROM_WRITE(backlash_smoothing_mm);
+    }
+
+    //
+    // Extensible UI User Data
+    //
+    #if ENABLED(EXTENSIBLE_UI)
+      {
+        char extui_data[ExtUI::eeprom_data_size] = { 0 };
+        ExtUI::onStoreSettings(extui_data);
+        _FIELD_TEST(extui_data);
+        EEPROM_WRITE(extui_data);
+      }
+    #endif
+
+    //
+    // Case Light Brightness
+    //
+    #if CASELIGHT_USES_BRIGHTNESS
+      EEPROM_WRITE(caselight.brightness);
+    #endif
+
+    //
+    // Password feature
+    //
+    #if ENABLED(PASSWORD_FEATURE)
+      EEPROM_WRITE(password.is_set);
+      EEPROM_WRITE(password.value);
+    #endif
+
+    //
+    // TOUCH_SCREEN_CALIBRATION
+    //
+    #if ENABLED(TOUCH_SCREEN_CALIBRATION)
+      EEPROM_WRITE(touch_calibration.calibration);
+    #endif
+
+    //
+    // Ethernet network info
+    //
+    #if HAS_ETHERNET
+    {
+      _FIELD_TEST(ethernet_hardware_enabled);
+      const bool ethernet_hardware_enabled = ethernet.hardware_enabled;
+      const uint32_t ethernet_ip      = ethernet.ip,
+                     ethernet_dns     = ethernet.myDns,
+                     ethernet_gateway = ethernet.gateway,
+                     ethernet_subnet  = ethernet.subnet;
+      EEPROM_WRITE(ethernet_hardware_enabled);
+      EEPROM_WRITE(ethernet_ip);
+      EEPROM_WRITE(ethernet_dns);
+      EEPROM_WRITE(ethernet_gateway);
+      EEPROM_WRITE(ethernet_subnet);
+    }
+    #endif
+
+    //
+    // Buzzer enable/disable
+    //
+    #if ENABLED(SOUND_MENU_ITEM)
+      EEPROM_WRITE(ui.buzzer_enabled);
+    #endif
+
+    //
+    // Selected LCD language
+    //
+    #if HAS_MULTI_LANGUAGE
+      EEPROM_WRITE(ui.language);
+    #endif
+
+    //
+    // Report final CRC and Data Size
+    //
+    if (!eeprom_error) {
+      const uint16_t eeprom_size = eeprom_index - (EEPROM_OFFSET),
+                     final_crc = working_crc;
+
+      // Write the EEPROM header
+      eeprom_index = EEPROM_OFFSET;
+
+      EEPROM_WRITE(version);
+      EEPROM_WRITE(final_crc);
+
+      // Report storage size
+      DEBUG_ECHO_START();
+      DEBUG_ECHOLNPAIR("Settings Stored (", eeprom_size, " bytes; crc ", (uint32_t)final_crc, ")");
+
+      eeprom_error |= size_error(eeprom_size);
+    }
+    EEPROM_FINISH();
+
+    //
+    // UBL Mesh
+    //
+    #if ENABLED(UBL_SAVE_ACTIVE_ON_M500)
+      if (ubl.storage_slot >= 0)
+        store_mesh(ubl.storage_slot);
+    #endif
+
+    if (!eeprom_error) LCD_MESSAGEPGM(MSG_SETTINGS_STORED);
+
+    TERN_(EXTENSIBLE_UI, ExtUI::onConfigurationStoreWritten(!eeprom_error));
+
+    return !eeprom_error;
+  }
+
+  /**
+   * M501 - Retrieve Configuration
+   */
+  bool MarlinSettings::_load() {
+    uint16_t working_crc = 0;
+
+    EEPROM_START();
+
+    char stored_ver[4];
+    EEPROM_READ_ALWAYS(stored_ver);
+
+    uint16_t stored_crc;
+    EEPROM_READ_ALWAYS(stored_crc);
+
+    // Version has to match or defaults are used
+    if (strncmp(version, stored_ver, 3) != 0) {
+      if (stored_ver[3] != '\0') {
+        stored_ver[0] = '?';
+        stored_ver[1] = '\0';
+      }
+      DEBUG_ECHO_START();
+      DEBUG_ECHOLNPAIR("EEPROM version mismatch (EEPROM=", stored_ver, " Marlin=" EEPROM_VERSION ")");
+      IF_DISABLED(EEPROM_AUTO_INIT, ui.eeprom_alert_version());
+      eeprom_error = true;
+    }
+    else {
+      float dummyf = 0;
+      working_crc = 0;  // Init to 0. Accumulated by EEPROM_READ
+
+      _FIELD_TEST(esteppers);
+
+      // Number of esteppers may change
+      uint8_t esteppers;
+      EEPROM_READ_ALWAYS(esteppers);
+
+      //
+      // Planner Motion
+      //
+      {
+        // Get only the number of E stepper parameters previously stored
+        // Any steppers added later are set to their defaults
+        uint32_t tmp1[XYZ + esteppers];
+        float tmp2[XYZ + esteppers];
+        feedRate_t tmp3[XYZ + esteppers];
+        EEPROM_READ(tmp1);                         // max_acceleration_mm_per_s2
+        EEPROM_READ(planner.settings.min_segment_time_us);
+        EEPROM_READ(tmp2);                         // axis_steps_per_mm
+        EEPROM_READ(tmp3);                         // max_feedrate_mm_s
+
+        if (!validating) LOOP_XYZE_N(i) {
+          const bool in = (i < esteppers + XYZ);
+          planner.settings.max_acceleration_mm_per_s2[i] = in ? tmp1[i] : pgm_read_dword(&_DMA[ALIM(i, _DMA)]);
+          planner.settings.axis_steps_per_mm[i]          = in ? tmp2[i] : pgm_read_float(&_DASU[ALIM(i, _DASU)]);
+          planner.settings.max_feedrate_mm_s[i]          = in ? tmp3[i] : pgm_read_float(&_DMF[ALIM(i, _DMF)]);
+        }
+
+        EEPROM_READ(planner.settings.acceleration);
+        EEPROM_READ(planner.settings.retract_acceleration);
+        EEPROM_READ(planner.settings.travel_acceleration);
+        EEPROM_READ(planner.settings.min_feedrate_mm_s);
+        EEPROM_READ(planner.settings.min_travel_feedrate_mm_s);
+
+        #if HAS_CLASSIC_JERK
+          EEPROM_READ(planner.max_jerk);
+          #if HAS_LINEAR_E_JERK
+            EEPROM_READ(dummyf);
+          #endif
+        #else
+          for (uint8_t q = 4; q--;) EEPROM_READ(dummyf);
+        #endif
+
+        EEPROM_READ(TERN(CLASSIC_JERK, dummyf, planner.junction_deviation_mm));
+      }
+
+      //
+      // Home Offset (M206 / M665)
+      //
+      {
+        _FIELD_TEST(home_offset);
+
+        #if HAS_SCARA_OFFSET
+          EEPROM_READ(scara_home_offset);
+        #else
+          #if !HAS_HOME_OFFSET
+            xyz_pos_t home_offset;
+          #endif
+          EEPROM_READ(home_offset);
+        #endif
+      }
+
+      //
+      // Hotend Offsets, if any
+      //
+      {
+        #if HAS_HOTEND_OFFSET
+          // Skip hotend 0 which must be 0
+          LOOP_S_L_N(e, 1, HOTENDS)
+            EEPROM_READ(hotend_offset[e]);
+        #endif
+      }
+
+      //
+      // Filament Runout Sensor
+      //
+      {
+        int8_t runout_sensor_enabled;
+        _FIELD_TEST(runout_sensor_enabled);
+        EEPROM_READ(runout_sensor_enabled);
+        #if HAS_FILAMENT_SENSOR
+          runout.enabled = runout_sensor_enabled < 0 ? FIL_RUNOUT_ENABLED_DEFAULT : runout_sensor_enabled;
+        #endif
+
+        TERN_(HAS_FILAMENT_SENSOR, if (runout.enabled) runout.reset());
+
+        float runout_distance_mm;
+        EEPROM_READ(runout_distance_mm);
+        #if HAS_FILAMENT_RUNOUT_DISTANCE
+          if (!validating) runout.set_runout_distance(runout_distance_mm);
+        #endif
+      }
+
+      //
+      // Global Leveling
+      //
+      EEPROM_READ(TERN(ENABLE_LEVELING_FADE_HEIGHT, new_z_fade_height, dummyf));
+
+      //
+      // Mesh (Manual) Bed Leveling
+      //
+      {
+        uint8_t mesh_num_x, mesh_num_y;
+        EEPROM_READ(dummyf);
+        EEPROM_READ_ALWAYS(mesh_num_x);
+        EEPROM_READ_ALWAYS(mesh_num_y);
+
+        #if ENABLED(MESH_BED_LEVELING)
+          if (!validating) mbl.z_offset = dummyf;
+          if (mesh_num_x == GRID_MAX_POINTS_X && mesh_num_y == GRID_MAX_POINTS_Y) {
+            // EEPROM data fits the current mesh
+            EEPROM_READ(mbl.z_values);
+          }
+          else {
+            // EEPROM data is stale
+            if (!validating) mbl.reset();
+            for (uint16_t q = mesh_num_x * mesh_num_y; q--;) EEPROM_READ(dummyf);
+          }
+        #else
+          // MBL is disabled - skip the stored data
+          for (uint16_t q = mesh_num_x * mesh_num_y; q--;) EEPROM_READ(dummyf);
+        #endif // MESH_BED_LEVELING
+      }
+
+      //
+      // Probe Z Offset
+      //
+      {
+        _FIELD_TEST(probe_offset);
+        #if HAS_BED_PROBE
+          const xyz_pos_t &zpo = probe.offset;
+        #else
+          xyz_pos_t zpo;
+        #endif
+        EEPROM_READ(zpo);
+      }
+
+      //
+      // Planar Bed Leveling matrix
+      //
+      {
+        #if ABL_PLANAR
+          EEPROM_READ(planner.bed_level_matrix);
+        #else
+          for (uint8_t q = 9; q--;) EEPROM_READ(dummyf);
+        #endif
+      }
+
+      //
+      // Bilinear Auto Bed Leveling
+      //
+      {
+        uint8_t grid_max_x, grid_max_y;
+        EEPROM_READ_ALWAYS(grid_max_x);                // 1 byte
+        EEPROM_READ_ALWAYS(grid_max_y);                // 1 byte
+        #if ENABLED(AUTO_BED_LEVELING_BILINEAR)
+          if (grid_max_x == GRID_MAX_POINTS_X && grid_max_y == GRID_MAX_POINTS_Y) {
+            if (!validating) set_bed_leveling_enabled(false);
+            EEPROM_READ(bilinear_grid_spacing);        // 2 ints
+            EEPROM_READ(bilinear_start);               // 2 ints
+            EEPROM_READ(z_values);                     // 9 to 256 floats
+          }
+          else // EEPROM data is stale
+        #endif // AUTO_BED_LEVELING_BILINEAR
+          {
+            // Skip past disabled (or stale) Bilinear Grid data
+            xy_pos_t bgs, bs;
+            EEPROM_READ(bgs);
+            EEPROM_READ(bs);
+            for (uint16_t q = grid_max_x * grid_max_y; q--;) EEPROM_READ(dummyf);
+          }
+      }
+
+      //
+      // Unified Bed Leveling active state
+      //
+      {
+        _FIELD_TEST(planner_leveling_active);
+        #if ENABLED(AUTO_BED_LEVELING_UBL)
+          const bool &planner_leveling_active = planner.leveling_active;
+          const int8_t &ubl_storage_slot = ubl.storage_slot;
+        #else
+          bool planner_leveling_active;
+          int8_t ubl_storage_slot;
+        #endif
+        EEPROM_READ(planner_leveling_active);
+        EEPROM_READ(ubl_storage_slot);
+      }
+
+      //
+      // SERVO_ANGLES
+      //
+      {
+        _FIELD_TEST(servo_angles);
+        #if ENABLED(EDITABLE_SERVO_ANGLES)
+          uint16_t (&servo_angles_arr)[EEPROM_NUM_SERVOS][2] = servo_angles;
+        #else
+          uint16_t servo_angles_arr[EEPROM_NUM_SERVOS][2];
+        #endif
+        EEPROM_READ(servo_angles_arr);
+      }
+
+      //
+      // Thermal first layer compensation values
+      //
+      #if ENABLED(PROBE_TEMP_COMPENSATION)
+        EEPROM_READ(temp_comp.z_offsets_probe);
+        EEPROM_READ(temp_comp.z_offsets_bed);
+        #if ENABLED(USE_TEMP_EXT_COMPENSATION)
+          EEPROM_READ(temp_comp.z_offsets_ext);
+        #endif
+        temp_comp.reset_index();
+      #else
+        // No placeholder data for this feature
+      #endif
+
+      //
+      // BLTOUCH
+      //
+      {
+        _FIELD_TEST(bltouch_last_written_mode);
+        #if ENABLED(BLTOUCH)
+          const bool &bltouch_last_written_mode = bltouch.last_written_mode;
+        #else
+          bool bltouch_last_written_mode;
+        #endif
+        EEPROM_READ(bltouch_last_written_mode);
+      }
+
+      //
+      // DELTA Geometry or Dual Endstops offsets
+      //
+      {
+        #if ENABLED(DELTA)
+
+          _FIELD_TEST(delta_height);
+
+          EEPROM_READ(delta_height);              // 1 float
+          EEPROM_READ(delta_endstop_adj);         // 3 floats
+          EEPROM_READ(delta_radius);              // 1 float
+          EEPROM_READ(delta_diagonal_rod);        // 1 float
+          EEPROM_READ(delta_segments_per_second); // 1 float
+          EEPROM_READ(delta_tower_angle_trim);    // 3 floats
+          EEPROM_READ(delta_diagonal_rod_trim);   // 3 floats
+
+        #elif HAS_EXTRA_ENDSTOPS
+
+          _FIELD_TEST(x2_endstop_adj);
+
+          EEPROM_READ(TERN(X_DUAL_ENDSTOPS, endstops.x2_endstop_adj, dummyf));  // 1 float
+          EEPROM_READ(TERN(Y_DUAL_ENDSTOPS, endstops.y2_endstop_adj, dummyf));  // 1 float
+          EEPROM_READ(TERN(Z_MULTI_ENDSTOPS, endstops.z2_endstop_adj, dummyf)); // 1 float
+
+          #if ENABLED(Z_MULTI_ENDSTOPS) && NUM_Z_STEPPER_DRIVERS >= 3
+            EEPROM_READ(endstops.z3_endstop_adj); // 1 float
+          #else
+            EEPROM_READ(dummyf);
+          #endif
+          #if ENABLED(Z_MULTI_ENDSTOPS) && NUM_Z_STEPPER_DRIVERS >= 4
+            EEPROM_READ(endstops.z4_endstop_adj); // 1 float
+          #else
+            EEPROM_READ(dummyf);
+          #endif
+
+        #endif
+      }
+
+      #if ENABLED(Z_STEPPER_AUTO_ALIGN)
+        EEPROM_READ(z_stepper_align.xy);
+        #if ENABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS)
+          EEPROM_READ(z_stepper_align.stepper_xy);
+        #endif
+      #endif
+
+      //
+      // LCD Preheat settings
+      //
+      #if PREHEAT_COUNT
+        _FIELD_TEST(ui_material_preset);
+        EEPROM_READ(ui.material_preset);
+      #endif
+
+      //
+      // Hotend PID
+      //
+      {
+        HOTEND_LOOP() {
+          PIDCF_t pidcf;
+          EEPROM_READ(pidcf);
+          #if ENABLED(PIDTEMP)
+            if (!validating && !isnan(pidcf.Kp)) {
+              // Scale PID values since EEPROM values are unscaled
+              PID_PARAM(Kp, e) = pidcf.Kp;
+              PID_PARAM(Ki, e) = scalePID_i(pidcf.Ki);
+              PID_PARAM(Kd, e) = scalePID_d(pidcf.Kd);
+              TERN_(PID_EXTRUSION_SCALING, PID_PARAM(Kc, e) = pidcf.Kc);
+              TERN_(PID_FAN_SCALING, PID_PARAM(Kf, e) = pidcf.Kf);
+            }
+          #endif
+        }
+      }
+
+      //
+      // PID Extrusion Scaling
+      //
+      {
+        _FIELD_TEST(lpq_len);
+        #if ENABLED(PID_EXTRUSION_SCALING)
+          const int16_t &lpq_len = thermalManager.lpq_len;
+        #else
+          int16_t lpq_len;
+        #endif
+        EEPROM_READ(lpq_len);
+      }
+
+      //
+      // Heated Bed PID
+      //
+      {
+        PID_t pid;
+        EEPROM_READ(pid);
+        #if ENABLED(PIDTEMPBED)
+          if (!validating && !isnan(pid.Kp)) {
+            // Scale PID values since EEPROM values are unscaled
+            thermalManager.temp_bed.pid.Kp = pid.Kp;
+            thermalManager.temp_bed.pid.Ki = scalePID_i(pid.Ki);
+            thermalManager.temp_bed.pid.Kd = scalePID_d(pid.Kd);
+          }
+        #endif
+      }
+
+      //
+      // User-defined Thermistors
+      //
+      #if HAS_USER_THERMISTORS
+      {
+        _FIELD_TEST(user_thermistor);
+        EEPROM_READ(thermalManager.user_thermistor);
+      }
+      #endif
+
+      //
+      // Power monitor
+      //
+      {
+        #if HAS_POWER_MONITOR
+          uint8_t &power_monitor_flags = power_monitor.flags;
+        #else
+          uint8_t power_monitor_flags;
+        #endif
+        _FIELD_TEST(power_monitor_flags);
+        EEPROM_READ(power_monitor_flags);
+      }
+
+      //
+      // LCD Contrast
+      //
+      {
+        _FIELD_TEST(lcd_contrast);
+        int16_t lcd_contrast;
+        EEPROM_READ(lcd_contrast);
+        if (!validating) {
+          TERN_(HAS_LCD_CONTRAST, ui.set_contrast(lcd_contrast));
+        }
+      }
+
+      //
+      // Controller Fan
+      //
+      {
+        _FIELD_TEST(controllerFan_settings);
+        #if ENABLED(CONTROLLER_FAN_EDITABLE)
+          const controllerFan_settings_t &cfs = controllerFan.settings;
+        #else
+          controllerFan_settings_t cfs = { 0 };
+        #endif
+        EEPROM_READ(cfs);
+      }
+
+      //
+      // Power-Loss Recovery
+      //
+      {
+        _FIELD_TEST(recovery_enabled);
+        #if ENABLED(POWER_LOSS_RECOVERY)
+          const bool &recovery_enabled = recovery.enabled;
+        #else
+          bool recovery_enabled;
+        #endif
+        EEPROM_READ(recovery_enabled);
+      }
+
+      //
+      // Firmware Retraction
+      //
+      {
+        _FIELD_TEST(fwretract_settings);
+
+        #if ENABLED(FWRETRACT)
+          EEPROM_READ(fwretract.settings);
+        #else
+          fwretract_settings_t fwretract_settings;
+          EEPROM_READ(fwretract_settings);
+        #endif
+        #if BOTH(FWRETRACT, FWRETRACT_AUTORETRACT)
+          EEPROM_READ(fwretract.autoretract_enabled);
+        #else
+          bool autoretract_enabled;
+          EEPROM_READ(autoretract_enabled);
+        #endif
+      }
+
+      //
+      // Volumetric & Filament Size
+      //
+      {
+        struct {
+          bool volumetric_enabled;
+          float filament_size[EXTRUDERS];
+          float volumetric_extruder_limit[EXTRUDERS];
+        } storage;
+
+        _FIELD_TEST(parser_volumetric_enabled);
+        EEPROM_READ(storage);
+
+        #if DISABLED(NO_VOLUMETRICS)
+          if (!validating) {
+            parser.volumetric_enabled = storage.volumetric_enabled;
+            COPY(planner.filament_size, storage.filament_size);
+            #if ENABLED(VOLUMETRIC_EXTRUDER_LIMIT)
+              COPY(planner.volumetric_extruder_limit, storage.volumetric_extruder_limit);
+            #endif
+          }
+        #endif
+      }
+
+      //
+      // TMC Stepper Settings
+      //
+
+      if (!validating) reset_stepper_drivers();
+
+      // TMC Stepper Current
+      {
+        _FIELD_TEST(tmc_stepper_current);
+
+        tmc_stepper_current_t currents;
+        EEPROM_READ(currents);
+
+        #if HAS_TRINAMIC_CONFIG
+
+          #define SET_CURR(Q) stepper##Q.rms_current(currents.Q ? currents.Q : Q##_CURRENT)
+          if (!validating) {
+            #if AXIS_IS_TMC(X)
+              SET_CURR(X);
+            #endif
+            #if AXIS_IS_TMC(Y)
+              SET_CURR(Y);
+            #endif
+            #if AXIS_IS_TMC(Z)
+              SET_CURR(Z);
+            #endif
+            #if AXIS_IS_TMC(X2)
+              SET_CURR(X2);
+            #endif
+            #if AXIS_IS_TMC(Y2)
+              SET_CURR(Y2);
+            #endif
+            #if AXIS_IS_TMC(Z2)
+              SET_CURR(Z2);
+            #endif
+            #if AXIS_IS_TMC(Z3)
+              SET_CURR(Z3);
+            #endif
+            #if AXIS_IS_TMC(Z4)
+              SET_CURR(Z4);
+            #endif
+            #if AXIS_IS_TMC(E0)
+              SET_CURR(E0);
+            #endif
+            #if AXIS_IS_TMC(E1)
+              SET_CURR(E1);
+            #endif
+            #if AXIS_IS_TMC(E2)
+              SET_CURR(E2);
+            #endif
+            #if AXIS_IS_TMC(E3)
+              SET_CURR(E3);
+            #endif
+            #if AXIS_IS_TMC(E4)
+              SET_CURR(E4);
+            #endif
+            #if AXIS_IS_TMC(E5)
+              SET_CURR(E5);
+            #endif
+            #if AXIS_IS_TMC(E6)
+              SET_CURR(E6);
+            #endif
+            #if AXIS_IS_TMC(E7)
+              SET_CURR(E7);
+            #endif
+          }
+        #endif
+      }
+
+      // TMC Hybrid Threshold
+      {
+        tmc_hybrid_threshold_t tmc_hybrid_threshold;
+        _FIELD_TEST(tmc_hybrid_threshold);
+        EEPROM_READ(tmc_hybrid_threshold);
+
+        #if ENABLED(HYBRID_THRESHOLD)
+          if (!validating) {
+            #if AXIS_HAS_STEALTHCHOP(X)
+              stepperX.set_pwm_thrs(tmc_hybrid_threshold.X);
+            #endif
+            #if AXIS_HAS_STEALTHCHOP(Y)
+              stepperY.set_pwm_thrs(tmc_hybrid_threshold.Y);
+            #endif
+            #if AXIS_HAS_STEALTHCHOP(Z)
+              stepperZ.set_pwm_thrs(tmc_hybrid_threshold.Z);
+            #endif
+            #if AXIS_HAS_STEALTHCHOP(X2)
+              stepperX2.set_pwm_thrs(tmc_hybrid_threshold.X2);
+            #endif
+            #if AXIS_HAS_STEALTHCHOP(Y2)
+              stepperY2.set_pwm_thrs(tmc_hybrid_threshold.Y2);
+            #endif
+            #if AXIS_HAS_STEALTHCHOP(Z2)
+              stepperZ2.set_pwm_thrs(tmc_hybrid_threshold.Z2);
+            #endif
+            #if AXIS_HAS_STEALTHCHOP(Z3)
+              stepperZ3.set_pwm_thrs(tmc_hybrid_threshold.Z3);
+            #endif
+            #if AXIS_HAS_STEALTHCHOP(Z4)
+              stepperZ4.set_pwm_thrs(tmc_hybrid_threshold.Z4);
+            #endif
+            #if AXIS_HAS_STEALTHCHOP(E0)
+              stepperE0.set_pwm_thrs(tmc_hybrid_threshold.E0);
+            #endif
+            #if AXIS_HAS_STEALTHCHOP(E1)
+              stepperE1.set_pwm_thrs(tmc_hybrid_threshold.E1);
+            #endif
+            #if AXIS_HAS_STEALTHCHOP(E2)
+              stepperE2.set_pwm_thrs(tmc_hybrid_threshold.E2);
+            #endif
+            #if AXIS_HAS_STEALTHCHOP(E3)
+              stepperE3.set_pwm_thrs(tmc_hybrid_threshold.E3);
+            #endif
+            #if AXIS_HAS_STEALTHCHOP(E4)
+              stepperE4.set_pwm_thrs(tmc_hybrid_threshold.E4);
+            #endif
+            #if AXIS_HAS_STEALTHCHOP(E5)
+              stepperE5.set_pwm_thrs(tmc_hybrid_threshold.E5);
+            #endif
+            #if AXIS_HAS_STEALTHCHOP(E6)
+              stepperE6.set_pwm_thrs(tmc_hybrid_threshold.E6);
+            #endif
+            #if AXIS_HAS_STEALTHCHOP(E7)
+              stepperE7.set_pwm_thrs(tmc_hybrid_threshold.E7);
+            #endif
+          }
+        #endif
+      }
+
+      //
+      // TMC StallGuard threshold.
+      //
+      {
+        tmc_sgt_t tmc_sgt;
+        _FIELD_TEST(tmc_sgt);
+        EEPROM_READ(tmc_sgt);
+        #if USE_SENSORLESS
+          if (!validating) {
+            TERN_(X_SENSORLESS,  stepperX.homing_threshold(tmc_sgt.X));
+            TERN_(X2_SENSORLESS, stepperX2.homing_threshold(tmc_sgt.X2));
+            TERN_(Y_SENSORLESS,  stepperY.homing_threshold(tmc_sgt.Y));
+            TERN_(Y2_SENSORLESS, stepperY2.homing_threshold(tmc_sgt.Y2));
+            TERN_(Z_SENSORLESS,  stepperZ.homing_threshold(tmc_sgt.Z));
+            TERN_(Z2_SENSORLESS, stepperZ2.homing_threshold(tmc_sgt.Z2));
+            TERN_(Z3_SENSORLESS, stepperZ3.homing_threshold(tmc_sgt.Z3));
+            TERN_(Z4_SENSORLESS, stepperZ4.homing_threshold(tmc_sgt.Z4));
+          }
+        #endif
+      }
+
+      // TMC stepping mode
+      {
+        _FIELD_TEST(tmc_stealth_enabled);
+
+        tmc_stealth_enabled_t tmc_stealth_enabled;
+        EEPROM_READ(tmc_stealth_enabled);
+
+        #if HAS_TRINAMIC_CONFIG
+
+          #define SET_STEPPING_MODE(ST) stepper##ST.stored.stealthChop_enabled = tmc_stealth_enabled.ST; stepper##ST.refresh_stepping_mode();
+          if (!validating) {
+            #if AXIS_HAS_STEALTHCHOP(X)
+              SET_STEPPING_MODE(X);
+            #endif
+            #if AXIS_HAS_STEALTHCHOP(Y)
+              SET_STEPPING_MODE(Y);
+            #endif
+            #if AXIS_HAS_STEALTHCHOP(Z)
+              SET_STEPPING_MODE(Z);
+            #endif
+            #if AXIS_HAS_STEALTHCHOP(X2)
+              SET_STEPPING_MODE(X2);
+            #endif
+            #if AXIS_HAS_STEALTHCHOP(Y2)
+              SET_STEPPING_MODE(Y2);
+            #endif
+            #if AXIS_HAS_STEALTHCHOP(Z2)
+              SET_STEPPING_MODE(Z2);
+            #endif
+            #if AXIS_HAS_STEALTHCHOP(Z3)
+              SET_STEPPING_MODE(Z3);
+            #endif
+            #if AXIS_HAS_STEALTHCHOP(Z4)
+              SET_STEPPING_MODE(Z4);
+            #endif
+            #if AXIS_HAS_STEALTHCHOP(E0)
+              SET_STEPPING_MODE(E0);
+            #endif
+            #if AXIS_HAS_STEALTHCHOP(E1)
+              SET_STEPPING_MODE(E1);
+            #endif
+            #if AXIS_HAS_STEALTHCHOP(E2)
+              SET_STEPPING_MODE(E2);
+            #endif
+            #if AXIS_HAS_STEALTHCHOP(E3)
+              SET_STEPPING_MODE(E3);
+            #endif
+            #if AXIS_HAS_STEALTHCHOP(E4)
+              SET_STEPPING_MODE(E4);
+            #endif
+            #if AXIS_HAS_STEALTHCHOP(E5)
+              SET_STEPPING_MODE(E5);
+            #endif
+            #if AXIS_HAS_STEALTHCHOP(E6)
+              SET_STEPPING_MODE(E6);
+            #endif
+            #if AXIS_HAS_STEALTHCHOP(E7)
+              SET_STEPPING_MODE(E7);
+            #endif
+          }
+        #endif
+      }
+
+      //
+      // Linear Advance
+      //
+      {
+        float extruder_advance_K[_MAX(EXTRUDERS, 1)];
+        _FIELD_TEST(planner_extruder_advance_K);
+        EEPROM_READ(extruder_advance_K);
+        #if ENABLED(LIN_ADVANCE)
+          if (!validating)
+            COPY(planner.extruder_advance_K, extruder_advance_K);
+        #endif
+      }
+
+      //
+      // Motor Current PWM
+      //
+      {
+        _FIELD_TEST(motor_current_setting);
+        uint32_t motor_current_setting[MOTOR_CURRENT_COUNT]
+          #if HAS_MOTOR_CURRENT_SPI
+             = DIGIPOT_MOTOR_CURRENT
+          #endif
+        ;
+        DEBUG_ECHOLNPGM("DIGIPOTS Loading");
+        EEPROM_READ(motor_current_setting);
+        DEBUG_ECHOLNPGM("DIGIPOTS Loaded");
+        #if HAS_MOTOR_CURRENT_SPI || HAS_MOTOR_CURRENT_PWM
+          if (!validating)
+            COPY(stepper.motor_current_setting, motor_current_setting);
+        #endif
+      }
+
+      //
+      // CNC Coordinate System
+      //
+      {
+        _FIELD_TEST(coordinate_system);
+        #if ENABLED(CNC_COORDINATE_SYSTEMS)
+          if (!validating) (void)gcode.select_coordinate_system(-1); // Go back to machine space
+          EEPROM_READ(gcode.coordinate_system);
+        #else
+          xyz_pos_t coordinate_system[MAX_COORDINATE_SYSTEMS];
+          EEPROM_READ(coordinate_system);
+        #endif
+      }
+
+      //
+      // Skew correction factors
+      //
+      {
+        skew_factor_t skew_factor;
+        _FIELD_TEST(planner_skew_factor);
+        EEPROM_READ(skew_factor);
+        #if ENABLED(SKEW_CORRECTION_GCODE)
+          if (!validating) {
+            planner.skew_factor.xy = skew_factor.xy;
+            #if ENABLED(SKEW_CORRECTION_FOR_Z)
+              planner.skew_factor.xz = skew_factor.xz;
+              planner.skew_factor.yz = skew_factor.yz;
+            #endif
+          }
+        #endif
+      }
+
+      //
+      // Advanced Pause filament load & unload lengths
+      //
+      #if EXTRUDERS
+      {
+        #if DISABLED(ADVANCED_PAUSE_FEATURE)
+          fil_change_settings_t fc_settings[EXTRUDERS];
+        #endif
+        _FIELD_TEST(fc_settings);
+        EEPROM_READ(fc_settings);
+      }
+      #endif
+
+      //
+      // Tool-change settings
+      //
+      #if HAS_MULTI_EXTRUDER
+        _FIELD_TEST(toolchange_settings);
+        EEPROM_READ(toolchange_settings);
+      #endif
+
+      //
+      // Backlash Compensation
+      //
+      {
+        #if ENABLED(BACKLASH_GCODE)
+          const xyz_float_t &backlash_distance_mm = backlash.distance_mm;
+          const uint8_t &backlash_correction = backlash.correction;
+        #else
+          float backlash_distance_mm[XYZ];
+          uint8_t backlash_correction;
+        #endif
+        #if ENABLED(BACKLASH_GCODE) && defined(BACKLASH_SMOOTHING_MM)
+          const float &backlash_smoothing_mm = backlash.smoothing_mm;
+        #else
+          float backlash_smoothing_mm;
+        #endif
+        _FIELD_TEST(backlash_distance_mm);
+        EEPROM_READ(backlash_distance_mm);
+        EEPROM_READ(backlash_correction);
+        EEPROM_READ(backlash_smoothing_mm);
+      }
+
+      //
+      // Extensible UI User Data
+      //
+      #if ENABLED(EXTENSIBLE_UI)
+        // This is a significant hardware change; don't reserve EEPROM space when not present
+        {
+          const char extui_data[ExtUI::eeprom_data_size] = { 0 };
+          _FIELD_TEST(extui_data);
+          EEPROM_READ(extui_data);
+          if (!validating) ExtUI::onLoadSettings(extui_data);
+        }
+      #endif
+
+      //
+      // Case Light Brightness
+      //
+      #if CASELIGHT_USES_BRIGHTNESS
+        _FIELD_TEST(caselight_brightness);
+        EEPROM_READ(caselight.brightness);
+      #endif
+
+      //
+      // Password feature
+      //
+      #if ENABLED(PASSWORD_FEATURE)
+        _FIELD_TEST(password_is_set);
+        EEPROM_READ(password.is_set);
+        EEPROM_READ(password.value);
+      #endif
+
+      //
+      // TOUCH_SCREEN_CALIBRATION
+      //
+      #if ENABLED(TOUCH_SCREEN_CALIBRATION)
+        _FIELD_TEST(touch_calibration_data);
+        EEPROM_READ(touch_calibration.calibration);
+      #endif
+
+      //
+      // Ethernet network info
+      //
+      #if HAS_ETHERNET
+        _FIELD_TEST(ethernet_hardware_enabled);
+        uint32_t ethernet_ip, ethernet_dns, ethernet_gateway, ethernet_subnet;
+        EEPROM_READ(ethernet.hardware_enabled);
+        EEPROM_READ(ethernet_ip);      ethernet.ip      = ethernet_ip;
+        EEPROM_READ(ethernet_dns);     ethernet.myDns   = ethernet_dns;
+        EEPROM_READ(ethernet_gateway); ethernet.gateway = ethernet_gateway;
+        EEPROM_READ(ethernet_subnet);  ethernet.subnet  = ethernet_subnet;
+      #endif
+
+      //
+      // Buzzer enable/disable
+      //
+      #if ENABLED(SOUND_MENU_ITEM)
+        _FIELD_TEST(buzzer_enabled);
+        EEPROM_READ(ui.buzzer_enabled);
+      #endif
+
+      //
+      // Selected LCD language
+      //
+      #if HAS_MULTI_LANGUAGE
+      {
+        uint8_t ui_language;
+        EEPROM_READ(ui_language);
+        if (ui_language >= NUM_LANGUAGES) ui_language = 0;
+        ui.set_language(ui_language);
+      }
+      #endif
+
+      //
+      // Validate Final Size and CRC
+      //
+      eeprom_error = size_error(eeprom_index - (EEPROM_OFFSET));
+      if (eeprom_error) {
+        DEBUG_ECHO_START();
+        DEBUG_ECHOLNPAIR("Index: ", int(eeprom_index - (EEPROM_OFFSET)), " Size: ", datasize());
+        IF_DISABLED(EEPROM_AUTO_INIT, ui.eeprom_alert_index());
+      }
+      else if (working_crc != stored_crc) {
+        eeprom_error = true;
+        DEBUG_ERROR_START();
+        DEBUG_ECHOLNPAIR("EEPROM CRC mismatch - (stored) ", stored_crc, " != ", working_crc, " (calculated)!");
+        IF_DISABLED(EEPROM_AUTO_INIT, ui.eeprom_alert_crc());
+      }
+      else if (!validating) {
+        DEBUG_ECHO_START();
+        DEBUG_ECHO(version);
+        DEBUG_ECHOLNPAIR(" stored settings retrieved (", eeprom_index - (EEPROM_OFFSET), " bytes; crc ", (uint32_t)working_crc, ")");
+      }
+
+      if (!validating && !eeprom_error) postprocess();
+
+      #if ENABLED(AUTO_BED_LEVELING_UBL)
+        if (!validating) {
+          ubl.report_state();
+
+          if (!ubl.sanity_check()) {
+            SERIAL_EOL();
+            #if BOTH(EEPROM_CHITCHAT, DEBUG_LEVELING_FEATURE)
+              ubl.echo_name();
+              DEBUG_ECHOLNPGM(" initialized.\n");
+            #endif
+          }
+          else {
+            eeprom_error = true;
+            #if BOTH(EEPROM_CHITCHAT, DEBUG_LEVELING_FEATURE)
+              DEBUG_ECHOPGM("?Can't enable ");
+              ubl.echo_name();
+              DEBUG_ECHOLNPGM(".");
+            #endif
+            ubl.reset();
+          }
+
+          if (ubl.storage_slot >= 0) {
+            load_mesh(ubl.storage_slot);
+            DEBUG_ECHOLNPAIR("Mesh ", ubl.storage_slot, " loaded from storage.");
+          }
+          else {
+            ubl.reset();
+            DEBUG_ECHOLNPGM("UBL reset");
+          }
+        }
+      #endif
+    }
+
+    #if ENABLED(EEPROM_CHITCHAT) && DISABLED(DISABLE_M503)
+      // Report the EEPROM settings
+      if (!validating && TERN1(EEPROM_BOOT_SILENT, IsRunning())) report();
+    #endif
+
+    EEPROM_FINISH();
+
+    return !eeprom_error;
+  }
+
+  #ifdef ARCHIM2_SPI_FLASH_EEPROM_BACKUP_SIZE
+    extern bool restoreEEPROM();
+  #endif
+
+  bool MarlinSettings::validate() {
+    validating = true;
+    #ifdef ARCHIM2_SPI_FLASH_EEPROM_BACKUP_SIZE
+      bool success = _load();
+      if (!success && restoreEEPROM()) {
+        SERIAL_ECHOLNPGM("Recovered backup EEPROM settings from SPI Flash");
+        success = _load();
+      }
+    #else
+      const bool success = _load();
+    #endif
+    validating = false;
+    return success;
+  }
+
+  bool MarlinSettings::load() {
+    if (validate()) {
+      const bool success = _load();
+      TERN_(EXTENSIBLE_UI, ExtUI::onConfigurationStoreRead(success));
+      return success;
+    }
+    reset();
+    #if ENABLED(EEPROM_AUTO_INIT)
+      (void)save();
+      SERIAL_ECHO_MSG("EEPROM Initialized");
+    #endif
+    return false;
+  }
+
+  #if ENABLED(AUTO_BED_LEVELING_UBL)
+
+    inline void ubl_invalid_slot(const int s) {
+      #if BOTH(EEPROM_CHITCHAT, DEBUG_OUT)
+        DEBUG_ECHOLNPGM("?Invalid slot.");
+        DEBUG_ECHO(s);
+        DEBUG_ECHOLNPGM(" mesh slots available.");
+      #else
+        UNUSED(s);
+      #endif
+    }
+
+    const uint16_t MarlinSettings::meshes_end = persistentStore.capacity() - 129; // 128 (+1 because of the change to capacity rather than last valid address)
+                                                                                  // is a placeholder for the size of the MAT; the MAT will always
+                                                                                  // live at the very end of the eeprom
+
+    uint16_t MarlinSettings::meshes_start_index() {
+      return (datasize() + EEPROM_OFFSET + 32) & 0xFFF8;  // Pad the end of configuration data so it can float up
+                                                          // or down a little bit without disrupting the mesh data
+    }
+
+    #define MESH_STORE_SIZE sizeof(TERN(OPTIMIZED_MESH_STORAGE, mesh_store_t, ubl.z_values))
+
+    uint16_t MarlinSettings::calc_num_meshes() {
+      return (meshes_end - meshes_start_index()) / MESH_STORE_SIZE;
+    }
+
+    int MarlinSettings::mesh_slot_offset(const int8_t slot) {
+      return meshes_end - (slot + 1) * MESH_STORE_SIZE;
+    }
+
+    void MarlinSettings::store_mesh(const int8_t slot) {
+
+      #if ENABLED(AUTO_BED_LEVELING_UBL)
+        const int16_t a = calc_num_meshes();
+        if (!WITHIN(slot, 0, a - 1)) {
+          ubl_invalid_slot(a);
+          DEBUG_ECHOLNPAIR("E2END=", persistentStore.capacity() - 1, " meshes_end=", meshes_end, " slot=", slot);
+          DEBUG_EOL();
+          return;
+        }
+
+        int pos = mesh_slot_offset(slot);
+        uint16_t crc = 0;
+
+        #if ENABLED(OPTIMIZED_MESH_STORAGE)
+          int16_t z_mesh_store[GRID_MAX_POINTS_X][GRID_MAX_POINTS_Y];
+          ubl.set_store_from_mesh(ubl.z_values, z_mesh_store);
+          uint8_t * const src = (uint8_t*)&z_mesh_store;
+        #else
+          uint8_t * const src = (uint8_t*)&ubl.z_values;
+        #endif
+
+        // Write crc to MAT along with other data, or just tack on to the beginning or end
+        persistentStore.access_start();
+        const bool status = persistentStore.write_data(pos, src, MESH_STORE_SIZE, &crc);
+        persistentStore.access_finish();
+
+        if (status) SERIAL_ECHOLNPGM("?Unable to save mesh data.");
+        else        DEBUG_ECHOLNPAIR("Mesh saved in slot ", slot);
+
+      #else
+
+        // Other mesh types
+
+      #endif
+    }
+
+    void MarlinSettings::load_mesh(const int8_t slot, void * const into/*=nullptr*/) {
+
+      #if ENABLED(AUTO_BED_LEVELING_UBL)
+
+        const int16_t a = settings.calc_num_meshes();
+
+        if (!WITHIN(slot, 0, a - 1)) {
+          ubl_invalid_slot(a);
+          return;
+        }
+
+        int pos = mesh_slot_offset(slot);
+        uint16_t crc = 0;
+        #if ENABLED(OPTIMIZED_MESH_STORAGE)
+          int16_t z_mesh_store[GRID_MAX_POINTS_X][GRID_MAX_POINTS_Y];
+          uint8_t * const dest = (uint8_t*)&z_mesh_store;
+        #else
+          uint8_t * const dest = into ? (uint8_t*)into : (uint8_t*)&ubl.z_values;
+        #endif
+
+        persistentStore.access_start();
+        const uint16_t status = persistentStore.read_data(pos, dest, MESH_STORE_SIZE, &crc);
+        persistentStore.access_finish();
+
+        #if ENABLED(OPTIMIZED_MESH_STORAGE)
+          if (into) {
+            float z_values[GRID_MAX_POINTS_X][GRID_MAX_POINTS_Y];
+            ubl.set_mesh_from_store(z_mesh_store, z_values);
+            memcpy(into, z_values, sizeof(z_values));
+          }
+          else
+            ubl.set_mesh_from_store(z_mesh_store, ubl.z_values);
+        #endif
+
+        if (status) SERIAL_ECHOLNPGM("?Unable to load mesh data.");
+        else        DEBUG_ECHOLNPAIR("Mesh loaded from slot ", slot);
+
+        EEPROM_FINISH();
+
+      #else
+
+        // Other mesh types
+
+      #endif
+    }
+
+    //void MarlinSettings::delete_mesh() { return; }
+    //void MarlinSettings::defrag_meshes() { return; }
+
+  #endif // AUTO_BED_LEVELING_UBL
+
+#else // !EEPROM_SETTINGS
+
+  bool MarlinSettings::save() {
+    DEBUG_ERROR_MSG("EEPROM disabled");
+    return false;
+  }
+
+#endif // !EEPROM_SETTINGS
+
+/**
+ * M502 - Reset Configuration
+ */
+void MarlinSettings::reset() {
+  LOOP_XYZE_N(i) {
+    planner.settings.max_acceleration_mm_per_s2[i] = pgm_read_dword(&_DMA[ALIM(i, _DMA)]);
+    planner.settings.axis_steps_per_mm[i]          = pgm_read_float(&_DASU[ALIM(i, _DASU)]);
+    planner.settings.max_feedrate_mm_s[i]          = pgm_read_float(&_DMF[ALIM(i, _DMF)]);
+  }
+
+  planner.settings.min_segment_time_us = DEFAULT_MINSEGMENTTIME;
+  planner.settings.acceleration = DEFAULT_ACCELERATION;
+  planner.settings.retract_acceleration = DEFAULT_RETRACT_ACCELERATION;
+  planner.settings.travel_acceleration = DEFAULT_TRAVEL_ACCELERATION;
+  planner.settings.min_feedrate_mm_s = feedRate_t(DEFAULT_MINIMUMFEEDRATE);
+  planner.settings.min_travel_feedrate_mm_s = feedRate_t(DEFAULT_MINTRAVELFEEDRATE);
+
+  #if HAS_CLASSIC_JERK
+    #ifndef DEFAULT_XJERK
+      #define DEFAULT_XJERK 0
+    #endif
+    #ifndef DEFAULT_YJERK
+      #define DEFAULT_YJERK 0
+    #endif
+    #ifndef DEFAULT_ZJERK
+      #define DEFAULT_ZJERK 0
+    #endif
+    planner.max_jerk.set(DEFAULT_XJERK, DEFAULT_YJERK, DEFAULT_ZJERK);
+    TERN_(HAS_CLASSIC_E_JERK, planner.max_jerk.e = DEFAULT_EJERK;);
+  #endif
+
+  #if HAS_JUNCTION_DEVIATION
+    planner.junction_deviation_mm = float(JUNCTION_DEVIATION_MM);
+  #endif
+
+  #if HAS_SCARA_OFFSET
+    scara_home_offset.reset();
+  #elif HAS_HOME_OFFSET
+    home_offset.reset();
+  #endif
+
+  TERN_(HAS_HOTEND_OFFSET, reset_hotend_offsets());
+
+  //
+  // Filament Runout Sensor
+  //
+
+  #if HAS_FILAMENT_SENSOR
+    runout.enabled = FIL_RUNOUT_ENABLED_DEFAULT;
+    runout.reset();
+    TERN_(HAS_FILAMENT_RUNOUT_DISTANCE, runout.set_runout_distance(FILAMENT_RUNOUT_DISTANCE_MM));
+  #endif
+
+  //
+  // Tool-change Settings
+  //
+
+  #if HAS_MULTI_EXTRUDER
+    #if ENABLED(TOOLCHANGE_FILAMENT_SWAP)
+      toolchange_settings.swap_length     = TOOLCHANGE_FS_LENGTH;
+      toolchange_settings.extra_resume    = TOOLCHANGE_FS_EXTRA_RESUME_LENGTH;
+      toolchange_settings.retract_speed   = TOOLCHANGE_FS_RETRACT_SPEED;
+      toolchange_settings.unretract_speed = TOOLCHANGE_FS_UNRETRACT_SPEED;
+      toolchange_settings.extra_prime     = TOOLCHANGE_FS_EXTRA_PRIME;
+      toolchange_settings.prime_speed     = TOOLCHANGE_FS_PRIME_SPEED;
+      toolchange_settings.fan_speed       = TOOLCHANGE_FS_FAN_SPEED;
+      toolchange_settings.fan_time        = TOOLCHANGE_FS_FAN_TIME;
+    #endif
+
+    #if ENABLED(TOOLCHANGE_FS_PRIME_FIRST_USED)
+      enable_first_prime = false;
+    #endif
+
+    #if ENABLED(TOOLCHANGE_PARK)
+      constexpr xyz_pos_t tpxy = TOOLCHANGE_PARK_XY;
+      toolchange_settings.enable_park = true;
+      toolchange_settings.change_point = tpxy;
+    #endif
+
+    toolchange_settings.z_raise = TOOLCHANGE_ZRAISE;
+
+    #if ENABLED(TOOLCHANGE_MIGRATION_FEATURE)
+      migration = migration_defaults;
+    #endif
+
+  #endif
+
+  #if ENABLED(BACKLASH_GCODE)
+    backlash.correction = (BACKLASH_CORRECTION) * 255;
+    constexpr xyz_float_t tmp = BACKLASH_DISTANCE_MM;
+    backlash.distance_mm = tmp;
+    #ifdef BACKLASH_SMOOTHING_MM
+      backlash.smoothing_mm = BACKLASH_SMOOTHING_MM;
+    #endif
+  #endif
+
+  TERN_(EXTENSIBLE_UI, ExtUI::onFactoryReset());
+
+  //
+  // Case Light Brightness
+  //
+  TERN_(CASELIGHT_USES_BRIGHTNESS, caselight.brightness = CASE_LIGHT_DEFAULT_BRIGHTNESS);
+
+  //
+  // TOUCH_SCREEN_CALIBRATION
+  //
+  TERN_(TOUCH_SCREEN_CALIBRATION, touch_calibration.calibration_reset());
+
+  //
+  // Buzzer enable/disable
+  //
+  TERN_(SOUND_MENU_ITEM, ui.buzzer_enabled = true);
+
+  //
+  // Magnetic Parking Extruder
+  //
+  TERN_(MAGNETIC_PARKING_EXTRUDER, mpe_settings_init());
+
+  //
+  // Global Leveling
+  //
+  TERN_(ENABLE_LEVELING_FADE_HEIGHT, new_z_fade_height = (DEFAULT_LEVELING_FADE_HEIGHT));
+  TERN_(HAS_LEVELING, reset_bed_level());
+
+  #if HAS_BED_PROBE
+    constexpr float dpo[] = NOZZLE_TO_PROBE_OFFSET;
+    static_assert(COUNT(dpo) == 3, "NOZZLE_TO_PROBE_OFFSET must contain offsets for X, Y, and Z.");
+    #if HAS_PROBE_XY_OFFSET
+      LOOP_XYZ(a) probe.offset[a] = dpo[a];
+    #else
+      probe.offset.set(0, 0, dpo[Z_AXIS]);
+    #endif
+  #endif
+
+  //
+  // Z Stepper Auto-alignment points
+  //
+  TERN_(Z_STEPPER_AUTO_ALIGN, z_stepper_align.reset_to_default());
+
+  //
+  // Servo Angles
+  //
+  TERN_(EDITABLE_SERVO_ANGLES, COPY(servo_angles, base_servo_angles)); // When not editable only one copy of servo angles exists
+
+  //
+  // BLTOUCH
+  //
+  //#if ENABLED(BLTOUCH)
+  //  bltouch.last_written_mode;
+  //#endif
+
+  //
+  // Endstop Adjustments
+  //
+
+  #if ENABLED(DELTA)
+    const abc_float_t adj = DELTA_ENDSTOP_ADJ, dta = DELTA_TOWER_ANGLE_TRIM, ddr = DELTA_DIAGONAL_ROD_TRIM_TOWER;
+    delta_height = DELTA_HEIGHT;
+    delta_endstop_adj = adj;
+    delta_radius = DELTA_RADIUS;
+    delta_diagonal_rod = DELTA_DIAGONAL_ROD;
+    delta_segments_per_second = DELTA_SEGMENTS_PER_SECOND;
+    delta_tower_angle_trim = dta;
+    delta_diagonal_rod_trim = ddr;
+  #endif
+
+  #if ENABLED(X_DUAL_ENDSTOPS)
+    #ifndef X2_ENDSTOP_ADJUSTMENT
+      #define X2_ENDSTOP_ADJUSTMENT 0
+    #endif
+    endstops.x2_endstop_adj = X2_ENDSTOP_ADJUSTMENT;
+  #endif
+
+  #if ENABLED(Y_DUAL_ENDSTOPS)
+    #ifndef Y2_ENDSTOP_ADJUSTMENT
+      #define Y2_ENDSTOP_ADJUSTMENT 0
+    #endif
+    endstops.y2_endstop_adj = Y2_ENDSTOP_ADJUSTMENT;
+  #endif
+
+  #if ENABLED(Z_MULTI_ENDSTOPS)
+    #ifndef Z2_ENDSTOP_ADJUSTMENT
+      #define Z2_ENDSTOP_ADJUSTMENT 0
+    #endif
+    endstops.z2_endstop_adj = Z2_ENDSTOP_ADJUSTMENT;
+    #if NUM_Z_STEPPER_DRIVERS >= 3
+      #ifndef Z3_ENDSTOP_ADJUSTMENT
+        #define Z3_ENDSTOP_ADJUSTMENT 0
+      #endif
+      endstops.z3_endstop_adj = Z3_ENDSTOP_ADJUSTMENT;
+    #endif
+    #if NUM_Z_STEPPER_DRIVERS >= 4
+      #ifndef Z4_ENDSTOP_ADJUSTMENT
+        #define Z4_ENDSTOP_ADJUSTMENT 0
+      #endif
+      endstops.z4_endstop_adj = Z4_ENDSTOP_ADJUSTMENT;
+    #endif
+  #endif
+
+  //
+  // Preheat parameters
+  //
+  #if PREHEAT_COUNT
+    #if HAS_HOTEND
+      constexpr uint16_t hpre[] = ARRAY_N(PREHEAT_COUNT, PREHEAT_1_TEMP_HOTEND, PREHEAT_2_TEMP_HOTEND, PREHEAT_3_TEMP_HOTEND, PREHEAT_4_TEMP_HOTEND, PREHEAT_5_TEMP_HOTEND);
+    #endif
+    #if HAS_HEATED_BED
+      constexpr uint16_t bpre[] = ARRAY_N(PREHEAT_COUNT, PREHEAT_1_TEMP_BED, PREHEAT_2_TEMP_BED, PREHEAT_3_TEMP_BED, PREHEAT_4_TEMP_BED, PREHEAT_5_TEMP_BED);
+    #endif
+    #if HAS_FAN
+      constexpr uint8_t fpre[] = ARRAY_N(PREHEAT_COUNT, PREHEAT_1_FAN_SPEED, PREHEAT_2_FAN_SPEED, PREHEAT_3_FAN_SPEED, PREHEAT_4_FAN_SPEED, PREHEAT_5_FAN_SPEED);
+    #endif
+    LOOP_L_N(i, PREHEAT_COUNT) {
+      #if HAS_HOTEND
+        ui.material_preset[i].hotend_temp = hpre[i];
+      #endif
+      #if HAS_HEATED_BED
+        ui.material_preset[i].bed_temp = bpre[i];
+      #endif
+      #if HAS_FAN
+        ui.material_preset[i].fan_speed = fpre[i];
+      #endif
+    }
+  #endif
+
+  //
+  // Hotend PID
+  //
+
+  #if ENABLED(PIDTEMP)
+    #if ENABLED(PID_PARAMS_PER_HOTEND)
+      constexpr float defKp[] =
+        #ifdef DEFAULT_Kp_LIST
+          DEFAULT_Kp_LIST
+        #else
+          ARRAY_BY_HOTENDS1(DEFAULT_Kp)
+        #endif
+      , defKi[] =
+        #ifdef DEFAULT_Ki_LIST
+          DEFAULT_Ki_LIST
+        #else
+          ARRAY_BY_HOTENDS1(DEFAULT_Ki)
+        #endif
+      , defKd[] =
+        #ifdef DEFAULT_Kd_LIST
+          DEFAULT_Kd_LIST
+        #else
+          ARRAY_BY_HOTENDS1(DEFAULT_Kd)
+        #endif
+      ;
+      static_assert(WITHIN(COUNT(defKp), 1, HOTENDS), "DEFAULT_Kp_LIST must have between 1 and HOTENDS items.");
+      static_assert(WITHIN(COUNT(defKi), 1, HOTENDS), "DEFAULT_Ki_LIST must have between 1 and HOTENDS items.");
+      static_assert(WITHIN(COUNT(defKd), 1, HOTENDS), "DEFAULT_Kd_LIST must have between 1 and HOTENDS items.");
+      #if ENABLED(PID_EXTRUSION_SCALING)
+        constexpr float defKc[] =
+          #ifdef DEFAULT_Kc_LIST
+            DEFAULT_Kc_LIST
+          #else
+            ARRAY_BY_HOTENDS1(DEFAULT_Kc)
+          #endif
+        ;
+        static_assert(WITHIN(COUNT(defKc), 1, HOTENDS), "DEFAULT_Kc_LIST must have between 1 and HOTENDS items.");
+      #endif
+      #if ENABLED(PID_FAN_SCALING)
+        constexpr float defKf[] =
+          #ifdef DEFAULT_Kf_LIST
+            DEFAULT_Kf_LIST
+          #else
+            ARRAY_BY_HOTENDS1(DEFAULT_Kf)
+          #endif
+        ;
+        static_assert(WITHIN(COUNT(defKf), 1, HOTENDS), "DEFAULT_Kf_LIST must have between 1 and HOTENDS items.");
+      #endif
+      #define PID_DEFAULT(N,E) def##N[E]
+    #else
+      #define PID_DEFAULT(N,E) DEFAULT_##N
+    #endif
+    HOTEND_LOOP() {
+      PID_PARAM(Kp, e) =      float(PID_DEFAULT(Kp, ALIM(e, defKp)));
+      PID_PARAM(Ki, e) = scalePID_i(PID_DEFAULT(Ki, ALIM(e, defKi)));
+      PID_PARAM(Kd, e) = scalePID_d(PID_DEFAULT(Kd, ALIM(e, defKd)));
+      TERN_(PID_EXTRUSION_SCALING, PID_PARAM(Kc, e) = float(PID_DEFAULT(Kc, ALIM(e, defKc))));
+      TERN_(PID_FAN_SCALING, PID_PARAM(Kf, e) = float(PID_DEFAULT(Kf, ALIM(e, defKf))));
+    }
+  #endif
+
+  //
+  // PID Extrusion Scaling
+  //
+  TERN_(PID_EXTRUSION_SCALING, thermalManager.lpq_len = 20); // Default last-position-queue size
+
+  //
+  // Heated Bed PID
+  //
+
+  #if ENABLED(PIDTEMPBED)
+    thermalManager.temp_bed.pid.Kp = DEFAULT_bedKp;
+    thermalManager.temp_bed.pid.Ki = scalePID_i(DEFAULT_bedKi);
+    thermalManager.temp_bed.pid.Kd = scalePID_d(DEFAULT_bedKd);
+  #endif
+
+  //
+  // User-Defined Thermistors
+  //
+  TERN_(HAS_USER_THERMISTORS, thermalManager.reset_user_thermistors());
+
+  //
+  // Power Monitor
+  //
+  TERN_(POWER_MONITOR, power_monitor.reset());
+
+  //
+  // LCD Contrast
+  //
+  TERN_(HAS_LCD_CONTRAST, ui.set_contrast(DEFAULT_LCD_CONTRAST));
+
+  //
+  // Controller Fan
+  //
+  TERN_(USE_CONTROLLER_FAN, controllerFan.reset());
+
+  //
+  // Power-Loss Recovery
+  //
+  TERN_(POWER_LOSS_RECOVERY, recovery.enable(ENABLED(PLR_ENABLED_DEFAULT)));
+
+  //
+  // Firmware Retraction
+  //
+  TERN_(FWRETRACT, fwretract.reset());
+
+  //
+  // Volumetric & Filament Size
+  //
+
+  #if DISABLED(NO_VOLUMETRICS)
+    parser.volumetric_enabled = ENABLED(VOLUMETRIC_DEFAULT_ON);
+    LOOP_L_N(q, COUNT(planner.filament_size))
+      planner.filament_size[q] = DEFAULT_NOMINAL_FILAMENT_DIA;
+    #if ENABLED(VOLUMETRIC_EXTRUDER_LIMIT)
+      LOOP_L_N(q, COUNT(planner.volumetric_extruder_limit))
+        planner.volumetric_extruder_limit[q] = DEFAULT_VOLUMETRIC_EXTRUDER_LIMIT;
+    #endif
+  #endif
+
+  endstops.enable_globally(ENABLED(ENDSTOPS_ALWAYS_ON_DEFAULT));
+
+  reset_stepper_drivers();
+
+  //
+  // Linear Advance
+  //
+
+  #if ENABLED(LIN_ADVANCE)
+    LOOP_L_N(i, EXTRUDERS) {
+      planner.extruder_advance_K[i] = LIN_ADVANCE_K;
+      TERN_(EXTRA_LIN_ADVANCE_K, other_extruder_advance_K[i] = LIN_ADVANCE_K);
+    }
+  #endif
+
+  //
+  // Motor Current PWM
+  //
+
+  #if HAS_MOTOR_CURRENT_PWM
+    constexpr uint32_t tmp_motor_current_setting[MOTOR_CURRENT_COUNT] = PWM_MOTOR_CURRENT;
+    LOOP_L_N(q, MOTOR_CURRENT_COUNT)
+      stepper.set_digipot_current(q, (stepper.motor_current_setting[q] = tmp_motor_current_setting[q]));
+  #endif
+
+  //
+  // DIGIPOTS
+  //
+  #if HAS_MOTOR_CURRENT_SPI
+    static constexpr uint32_t tmp_motor_current_setting[] = DIGIPOT_MOTOR_CURRENT;
+    DEBUG_ECHOLNPGM("Writing Digipot");
+    LOOP_L_N(q, COUNT(tmp_motor_current_setting))
+      stepper.set_digipot_current(q, tmp_motor_current_setting[q]);
+    DEBUG_ECHOLNPGM("Digipot Written");
+  #endif
+
+  //
+  // CNC Coordinate System
+  //
+  TERN_(CNC_COORDINATE_SYSTEMS, (void)gcode.select_coordinate_system(-1)); // Go back to machine space
+
+  //
+  // Skew Correction
+  //
+  #if ENABLED(SKEW_CORRECTION_GCODE)
+    planner.skew_factor.xy = XY_SKEW_FACTOR;
+    #if ENABLED(SKEW_CORRECTION_FOR_Z)
+      planner.skew_factor.xz = XZ_SKEW_FACTOR;
+      planner.skew_factor.yz = YZ_SKEW_FACTOR;
+    #endif
+  #endif
+
+  //
+  // Advanced Pause filament load & unload lengths
+  //
+  #if ENABLED(ADVANCED_PAUSE_FEATURE)
+    LOOP_L_N(e, EXTRUDERS) {
+      fc_settings[e].unload_length = FILAMENT_CHANGE_UNLOAD_LENGTH;
+      fc_settings[e].load_length = FILAMENT_CHANGE_FAST_LOAD_LENGTH;
+    }
+  #endif
+
+  #if ENABLED(PASSWORD_FEATURE)
+    #ifdef PASSWORD_DEFAULT_VALUE
+      password.is_set = true;
+      password.value = PASSWORD_DEFAULT_VALUE;
+    #else
+      password.is_set = false;
+    #endif
+  #endif
+
+  postprocess();
+
+  DEBUG_ECHO_START();
+  DEBUG_ECHOLNPGM("Hardcoded Default Settings Loaded");
+
+  TERN_(EXTENSIBLE_UI, ExtUI::onFactoryReset());
+}
+
+#if DISABLED(DISABLE_M503)
+
+  static void config_heading(const bool repl, PGM_P const pstr, const bool eol=true) {
+    if (!repl) {
+      SERIAL_ECHO_START();
+      SERIAL_ECHOPGM("; ");
+      serialprintPGM(pstr);
+      if (eol) SERIAL_EOL();
+    }
+  }
+
+  #define CONFIG_ECHO_START()       do{ if (!forReplay) SERIAL_ECHO_START(); }while(0)
+  #define CONFIG_ECHO_MSG(STR)      do{ CONFIG_ECHO_START(); SERIAL_ECHOLNPGM(STR); }while(0)
+  #define CONFIG_ECHO_HEADING(STR)  config_heading(forReplay, PSTR(STR))
+
+  #if HAS_TRINAMIC_CONFIG
+    inline void say_M906(const bool forReplay) { CONFIG_ECHO_START(); SERIAL_ECHOPGM("  M906"); }
+    #if HAS_STEALTHCHOP
+      void say_M569(const bool forReplay, const char * const etc=nullptr, const bool newLine = false) {
+        CONFIG_ECHO_START();
+        SERIAL_ECHOPGM("  M569 S1");
+        if (etc) {
+          SERIAL_CHAR(' ');
+          serialprintPGM(etc);
+        }
+        if (newLine) SERIAL_EOL();
+      }
+    #endif
+    #if ENABLED(HYBRID_THRESHOLD)
+      inline void say_M913(const bool forReplay) { CONFIG_ECHO_START(); SERIAL_ECHOPGM("  M913"); }
+    #endif
+    #if USE_SENSORLESS
+      inline void say_M914() { SERIAL_ECHOPGM("  M914"); }
+    #endif
+  #endif
+
+  #if ENABLED(ADVANCED_PAUSE_FEATURE)
+    inline void say_M603(const bool forReplay) { CONFIG_ECHO_START(); SERIAL_ECHOPGM("  M603 "); }
+  #endif
+
+  inline void say_units(const bool colon) {
+    serialprintPGM(
+      #if ENABLED(INCH_MODE_SUPPORT)
+        parser.linear_unit_factor != 1.0 ? PSTR(" (in)") :
+      #endif
+      PSTR(" (mm)")
+    );
+    if (colon) SERIAL_ECHOLNPGM(":");
+  }
+
+  void report_M92(const bool echo=true, const int8_t e=-1);
+
+  /**
+   * M503 - Report current settings in RAM
+   *
+   * Unless specifically disabled, M503 is available even without EEPROM
+   */
+  void MarlinSettings::report(const bool forReplay) {
+    /**
+     * Announce current units, in case inches are being displayed
+     */
+    CONFIG_ECHO_START();
+    #if ENABLED(INCH_MODE_SUPPORT)
+      SERIAL_ECHOPGM("  G2");
+      SERIAL_CHAR(parser.linear_unit_factor == 1.0 ? '1' : '0');
+      SERIAL_ECHOPGM(" ;");
+      say_units(false);
+    #else
+      SERIAL_ECHOPGM("  G21    ; Units in mm");
+      say_units(false);
+    #endif
+    SERIAL_EOL();
+
+    #if HAS_LCD_MENU
+
+      // Temperature units - for Ultipanel temperature options
+
+      CONFIG_ECHO_START();
+      #if ENABLED(TEMPERATURE_UNITS_SUPPORT)
+        SERIAL_ECHOPGM("  M149 ");
+        SERIAL_CHAR(parser.temp_units_code());
+        SERIAL_ECHOPGM(" ; Units in ");
+        serialprintPGM(parser.temp_units_name());
+      #else
+        SERIAL_ECHOLNPGM("  M149 C ; Units in Celsius");
+      #endif
+
+    #endif
+
+    SERIAL_EOL();
+
+    #if EXTRUDERS && DISABLED(NO_VOLUMETRICS)
+
+      /**
+       * Volumetric extrusion M200
+       */
+      if (!forReplay) {
+        config_heading(forReplay, PSTR("Filament settings:"), false);
+        if (parser.volumetric_enabled)
+          SERIAL_EOL();
+        else
+          SERIAL_ECHOLNPGM(" Disabled");
+      }
+
+      #if EXTRUDERS == 1
+        CONFIG_ECHO_START();
+        SERIAL_ECHOLNPAIR("  M200 S", int(parser.volumetric_enabled)
+                              , " D", LINEAR_UNIT(planner.filament_size[0])
+                              #if ENABLED(VOLUMETRIC_EXTRUDER_LIMIT)
+                                , " L", LINEAR_UNIT(planner.volumetric_extruder_limit[0])
+                              #endif
+                         );
+      #else
+        LOOP_L_N(i, EXTRUDERS) {
+          CONFIG_ECHO_START();
+          SERIAL_ECHOLNPAIR("  M200 T", int(i)
+                                , " D", LINEAR_UNIT(planner.filament_size[i])
+                                #if ENABLED(VOLUMETRIC_EXTRUDER_LIMIT)
+                                  , " L", LINEAR_UNIT(planner.volumetric_extruder_limit[i])
+                                #endif
+                           );
+        }
+        CONFIG_ECHO_START();
+        SERIAL_ECHOLNPAIR("  M200 S", int(parser.volumetric_enabled));
+      #endif
+    #endif // EXTRUDERS && !NO_VOLUMETRICS
+
+    CONFIG_ECHO_HEADING("Steps per unit:");
+    report_M92(!forReplay);
+
+    CONFIG_ECHO_HEADING("Maximum feedrates (units/s):");
+    CONFIG_ECHO_START();
+    SERIAL_ECHOLNPAIR_P(
+        PSTR("  M203 X"), LINEAR_UNIT(planner.settings.max_feedrate_mm_s[X_AXIS])
+      , SP_Y_STR, LINEAR_UNIT(planner.settings.max_feedrate_mm_s[Y_AXIS])
+      , SP_Z_STR, LINEAR_UNIT(planner.settings.max_feedrate_mm_s[Z_AXIS])
+      #if DISABLED(DISTINCT_E_FACTORS)
+        , SP_E_STR, VOLUMETRIC_UNIT(planner.settings.max_feedrate_mm_s[E_AXIS])
+      #endif
+    );
+    #if ENABLED(DISTINCT_E_FACTORS)
+      LOOP_L_N(i, E_STEPPERS) {
+        CONFIG_ECHO_START();
+        SERIAL_ECHOLNPAIR_P(
+            PSTR("  M203 T"), (int)i
+          , SP_E_STR, VOLUMETRIC_UNIT(planner.settings.max_feedrate_mm_s[E_AXIS_N(i)])
+        );
+      }
+    #endif
+
+    CONFIG_ECHO_HEADING("Maximum Acceleration (units/s2):");
+    CONFIG_ECHO_START();
+    SERIAL_ECHOLNPAIR_P(
+        PSTR("  M201 X"), LINEAR_UNIT(planner.settings.max_acceleration_mm_per_s2[X_AXIS])
+      , SP_Y_STR, LINEAR_UNIT(planner.settings.max_acceleration_mm_per_s2[Y_AXIS])
+      , SP_Z_STR, LINEAR_UNIT(planner.settings.max_acceleration_mm_per_s2[Z_AXIS])
+      #if DISABLED(DISTINCT_E_FACTORS)
+        , SP_E_STR, VOLUMETRIC_UNIT(planner.settings.max_acceleration_mm_per_s2[E_AXIS])
+      #endif
+    );
+    #if ENABLED(DISTINCT_E_FACTORS)
+      LOOP_L_N(i, E_STEPPERS) {
+        CONFIG_ECHO_START();
+        SERIAL_ECHOLNPAIR_P(
+            PSTR("  M201 T"), (int)i
+          , SP_E_STR, VOLUMETRIC_UNIT(planner.settings.max_acceleration_mm_per_s2[E_AXIS_N(i)])
+        );
+      }
+    #endif
+
+    CONFIG_ECHO_HEADING("Acceleration (units/s2): P<print_accel> R<retract_accel> T<travel_accel>");
+    CONFIG_ECHO_START();
+    SERIAL_ECHOLNPAIR_P(
+        PSTR("  M204 P"), LINEAR_UNIT(planner.settings.acceleration)
+      , PSTR(" R"), LINEAR_UNIT(planner.settings.retract_acceleration)
+      , SP_T_STR, LINEAR_UNIT(planner.settings.travel_acceleration)
+    );
+
+    CONFIG_ECHO_HEADING(
+      "Advanced: B<min_segment_time_us> S<min_feedrate> T<min_travel_feedrate>"
+      #if HAS_JUNCTION_DEVIATION
+        " J<junc_dev>"
+      #endif
+      #if HAS_CLASSIC_JERK
+        " X<max_x_jerk> Y<max_y_jerk> Z<max_z_jerk>"
+        TERN_(HAS_CLASSIC_E_JERK, " E<max_e_jerk>")
+      #endif
+    );
+    CONFIG_ECHO_START();
+    SERIAL_ECHOLNPAIR_P(
+        PSTR("  M205 B"), LINEAR_UNIT(planner.settings.min_segment_time_us)
+      , PSTR(" S"), LINEAR_UNIT(planner.settings.min_feedrate_mm_s)
+      , SP_T_STR, LINEAR_UNIT(planner.settings.min_travel_feedrate_mm_s)
+      #if HAS_JUNCTION_DEVIATION
+        , PSTR(" J"), LINEAR_UNIT(planner.junction_deviation_mm)
+      #endif
+      #if HAS_CLASSIC_JERK
+        , SP_X_STR, LINEAR_UNIT(planner.max_jerk.x)
+        , SP_Y_STR, LINEAR_UNIT(planner.max_jerk.y)
+        , SP_Z_STR, LINEAR_UNIT(planner.max_jerk.z)
+        #if HAS_CLASSIC_E_JERK
+          , SP_E_STR, LINEAR_UNIT(planner.max_jerk.e)
+        #endif
+      #endif
+    );
+
+    #if HAS_M206_COMMAND
+      CONFIG_ECHO_HEADING("Home offset:");
+      CONFIG_ECHO_START();
+      SERIAL_ECHOLNPAIR_P(
+        #if IS_CARTESIAN
+            PSTR("  M206 X"), LINEAR_UNIT(home_offset.x)
+          , SP_Y_STR, LINEAR_UNIT(home_offset.y)
+          , SP_Z_STR
+        #else
+          PSTR("  M206 Z")
+        #endif
+        , LINEAR_UNIT(home_offset.z)
+      );
+    #endif
+
+    #if HAS_HOTEND_OFFSET
+      CONFIG_ECHO_HEADING("Hotend offsets:");
+      CONFIG_ECHO_START();
+      LOOP_S_L_N(e, 1, HOTENDS) {
+        SERIAL_ECHOPAIR_P(
+          PSTR("  M218 T"), (int)e,
+          SP_X_STR, LINEAR_UNIT(hotend_offset[e].x),
+          SP_Y_STR, LINEAR_UNIT(hotend_offset[e].y)
+        );
+        SERIAL_ECHOLNPAIR_F_P(SP_Z_STR, LINEAR_UNIT(hotend_offset[e].z), 3);
+      }
+    #endif
+
+    /**
+     * Bed Leveling
+     */
+    #if HAS_LEVELING
+
+      #if ENABLED(MESH_BED_LEVELING)
+
+        CONFIG_ECHO_HEADING("Mesh Bed Leveling:");
+
+      #elif ENABLED(AUTO_BED_LEVELING_UBL)
+
+        config_heading(forReplay, NUL_STR, false);
+        if (!forReplay) {
+          ubl.echo_name();
+          SERIAL_CHAR(':');
+          SERIAL_EOL();
+        }
+
+      #elif HAS_ABL_OR_UBL
+
+        CONFIG_ECHO_HEADING("Auto Bed Leveling:");
+
+      #endif
+
+      CONFIG_ECHO_START();
+      SERIAL_ECHOLNPAIR_P(
+        PSTR("  M420 S"), planner.leveling_active ? 1 : 0
+        #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
+          , SP_Z_STR, LINEAR_UNIT(planner.z_fade_height)
+        #endif
+      );
+
+      #if ENABLED(MESH_BED_LEVELING)
+
+        if (leveling_is_valid()) {
+          LOOP_L_N(py, GRID_MAX_POINTS_Y) {
+            LOOP_L_N(px, GRID_MAX_POINTS_X) {
+              CONFIG_ECHO_START();
+              SERIAL_ECHOPAIR_P(PSTR("  G29 S3 I"), (int)px, PSTR(" J"), (int)py);
+              SERIAL_ECHOLNPAIR_F_P(SP_Z_STR, LINEAR_UNIT(mbl.z_values[px][py]), 5);
+            }
+          }
+          CONFIG_ECHO_START();
+          SERIAL_ECHOLNPAIR_F_P(PSTR("  G29 S4 Z"), LINEAR_UNIT(mbl.z_offset), 5);
+        }
+
+      #elif ENABLED(AUTO_BED_LEVELING_UBL)
+
+        if (!forReplay) {
+          SERIAL_EOL();
+          ubl.report_state();
+          SERIAL_EOL();
+          config_heading(false, PSTR("Active Mesh Slot: "), false);
+          SERIAL_ECHOLN(ubl.storage_slot);
+          config_heading(false, PSTR("EEPROM can hold "), false);
+          SERIAL_ECHO(calc_num_meshes());
+          SERIAL_ECHOLNPGM(" meshes.\n");
+        }
+
+       //ubl.report_current_mesh();   // This is too verbose for large meshes. A better (more terse)
+                                                  // solution needs to be found.
+      #elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
+
+        if (leveling_is_valid()) {
+          LOOP_L_N(py, GRID_MAX_POINTS_Y) {
+            LOOP_L_N(px, GRID_MAX_POINTS_X) {
+              CONFIG_ECHO_START();
+              SERIAL_ECHOPAIR("  G29 W I", (int)px, " J", (int)py);
+              SERIAL_ECHOLNPAIR_F_P(SP_Z_STR, LINEAR_UNIT(z_values[px][py]), 5);
+            }
+          }
+        }
+
+      #endif
+
+    #endif // HAS_LEVELING
+
+    #if ENABLED(EDITABLE_SERVO_ANGLES)
+
+      CONFIG_ECHO_HEADING("Servo Angles:");
+      LOOP_L_N(i, NUM_SERVOS) {
+        switch (i) {
+          #if ENABLED(SWITCHING_EXTRUDER)
+            case SWITCHING_EXTRUDER_SERVO_NR:
+            #if EXTRUDERS > 3
+              case SWITCHING_EXTRUDER_E23_SERVO_NR:
+            #endif
+          #elif ENABLED(SWITCHING_NOZZLE)
+            case SWITCHING_NOZZLE_SERVO_NR:
+          #elif ENABLED(BLTOUCH) || (HAS_Z_SERVO_PROBE && defined(Z_SERVO_ANGLES))
+            case Z_PROBE_SERVO_NR:
+          #endif
+            CONFIG_ECHO_START();
+            SERIAL_ECHOLNPAIR("  M281 P", int(i), " L", servo_angles[i][0], " U", servo_angles[i][1]);
+          default: break;
+        }
+      }
+
+    #endif // EDITABLE_SERVO_ANGLES
+
+    #if HAS_SCARA_OFFSET
+
+      CONFIG_ECHO_HEADING("SCARA settings: S<seg-per-sec> P<theta-psi-offset> T<theta-offset>");
+      CONFIG_ECHO_START();
+      SERIAL_ECHOLNPAIR_P(
+          PSTR("  M665 S"), delta_segments_per_second
+        , SP_P_STR, scara_home_offset.a
+        , SP_T_STR, scara_home_offset.b
+        , SP_Z_STR, LINEAR_UNIT(scara_home_offset.z)
+      );
+
+    #elif ENABLED(DELTA)
+
+      CONFIG_ECHO_HEADING("Endstop adjustment:");
+      CONFIG_ECHO_START();
+      SERIAL_ECHOLNPAIR_P(
+          PSTR("  M666 X"), LINEAR_UNIT(delta_endstop_adj.a)
+        , SP_Y_STR, LINEAR_UNIT(delta_endstop_adj.b)
+        , SP_Z_STR, LINEAR_UNIT(delta_endstop_adj.c)
+      );
+
+      CONFIG_ECHO_HEADING("Delta settings: L<diagonal rod> R<radius> H<height> S<segments per sec> XYZ<tower angle trim> ABC<rod trim>");
+      CONFIG_ECHO_START();
+      SERIAL_ECHOLNPAIR_P(
+          PSTR("  M665 L"), LINEAR_UNIT(delta_diagonal_rod)
+        , PSTR(" R"), LINEAR_UNIT(delta_radius)
+        , PSTR(" H"), LINEAR_UNIT(delta_height)
+        , PSTR(" S"), delta_segments_per_second
+        , SP_X_STR, LINEAR_UNIT(delta_tower_angle_trim.a)
+        , SP_Y_STR, LINEAR_UNIT(delta_tower_angle_trim.b)
+        , SP_Z_STR, LINEAR_UNIT(delta_tower_angle_trim.c)
+        , PSTR(" A"), LINEAR_UNIT(delta_diagonal_rod_trim.a)
+        , PSTR(" B"), LINEAR_UNIT(delta_diagonal_rod_trim.b)
+        , PSTR(" C"), LINEAR_UNIT(delta_diagonal_rod_trim.c)
+      );
+
+    #elif HAS_EXTRA_ENDSTOPS
+
+      CONFIG_ECHO_HEADING("Endstop adjustment:");
+      CONFIG_ECHO_START();
+      SERIAL_ECHOPGM("  M666");
+      #if ENABLED(X_DUAL_ENDSTOPS)
+        SERIAL_ECHOLNPAIR_P(SP_X_STR, LINEAR_UNIT(endstops.x2_endstop_adj));
+      #endif
+      #if ENABLED(Y_DUAL_ENDSTOPS)
+        SERIAL_ECHOLNPAIR_P(SP_Y_STR, LINEAR_UNIT(endstops.y2_endstop_adj));
+      #endif
+      #if ENABLED(Z_MULTI_ENDSTOPS)
+        #if NUM_Z_STEPPER_DRIVERS >= 3
+          SERIAL_ECHOPAIR(" S2 Z", LINEAR_UNIT(endstops.z3_endstop_adj));
+          CONFIG_ECHO_START();
+          SERIAL_ECHOPAIR("  M666 S3 Z", LINEAR_UNIT(endstops.z3_endstop_adj));
+          #if NUM_Z_STEPPER_DRIVERS >= 4
+            CONFIG_ECHO_START();
+            SERIAL_ECHOPAIR("  M666 S4 Z", LINEAR_UNIT(endstops.z4_endstop_adj));
+          #endif
+        #else
+          SERIAL_ECHOLNPAIR_P(SP_Z_STR, LINEAR_UNIT(endstops.z2_endstop_adj));
+        #endif
+      #endif
+
+    #endif // [XYZ]_DUAL_ENDSTOPS
+
+    #if PREHEAT_COUNT
+
+      CONFIG_ECHO_HEADING("Material heatup parameters:");
+      LOOP_L_N(i, PREHEAT_COUNT) {
+        CONFIG_ECHO_START();
+        SERIAL_ECHOLNPAIR_P(
+          PSTR("  M145 S"), (int)i
+          #if HAS_HOTEND
+            , PSTR(" H"), TEMP_UNIT(ui.material_preset[i].hotend_temp)
+          #endif
+          #if HAS_HEATED_BED
+            , SP_B_STR, TEMP_UNIT(ui.material_preset[i].bed_temp)
+          #endif
+          #if HAS_FAN
+            , PSTR(" F"), ui.material_preset[i].fan_speed
+          #endif
+        );
+      }
+
+    #endif
+
+    #if HAS_PID_HEATING
+
+      CONFIG_ECHO_HEADING("PID settings:");
+
+      #if ENABLED(PIDTEMP)
+        HOTEND_LOOP() {
+          CONFIG_ECHO_START();
+          SERIAL_ECHOPAIR_P(
+            #if ENABLED(PID_PARAMS_PER_HOTEND)
+              PSTR("  M301 E"), e,
+              SP_P_STR
+            #else
+              PSTR("  M301 P")
+            #endif
+                        , PID_PARAM(Kp, e)
+            , PSTR(" I"), unscalePID_i(PID_PARAM(Ki, e))
+            , PSTR(" D"), unscalePID_d(PID_PARAM(Kd, e))
+          );
+          #if ENABLED(PID_EXTRUSION_SCALING)
+            SERIAL_ECHOPAIR_P(SP_C_STR, PID_PARAM(Kc, e));
+            if (e == 0) SERIAL_ECHOPAIR(" L", thermalManager.lpq_len);
+          #endif
+          #if ENABLED(PID_FAN_SCALING)
+            SERIAL_ECHOPAIR(" F", PID_PARAM(Kf, e));
+          #endif
+          SERIAL_EOL();
+        }
+      #endif // PIDTEMP
+
+      #if ENABLED(PIDTEMPBED)
+        CONFIG_ECHO_START();
+        SERIAL_ECHOLNPAIR(
+            "  M304 P", thermalManager.temp_bed.pid.Kp
+          , " I", unscalePID_i(thermalManager.temp_bed.pid.Ki)
+          , " D", unscalePID_d(thermalManager.temp_bed.pid.Kd)
+        );
+      #endif
+
+    #endif // PIDTEMP || PIDTEMPBED
+
+    #if HAS_USER_THERMISTORS
+      CONFIG_ECHO_HEADING("User thermistors:");
+      LOOP_L_N(i, USER_THERMISTORS)
+        thermalManager.log_user_thermistor(i, true);
+    #endif
+
+    #if HAS_LCD_CONTRAST
+      CONFIG_ECHO_HEADING("LCD Contrast:");
+      CONFIG_ECHO_START();
+      SERIAL_ECHOLNPAIR("  M250 C", ui.contrast);
+    #endif
+
+    TERN_(CONTROLLER_FAN_EDITABLE, M710_report(forReplay));
+
+    #if ENABLED(POWER_LOSS_RECOVERY)
+      CONFIG_ECHO_HEADING("Power-Loss Recovery:");
+      CONFIG_ECHO_START();
+      SERIAL_ECHOLNPAIR("  M413 S", int(recovery.enabled));
+    #endif
+
+    #if ENABLED(FWRETRACT)
+
+      CONFIG_ECHO_HEADING("Retract: S<length> F<units/m> Z<lift>");
+      CONFIG_ECHO_START();
+      SERIAL_ECHOLNPAIR_P(
+          PSTR("  M207 S"), LINEAR_UNIT(fwretract.settings.retract_length)
+        , PSTR(" W"), LINEAR_UNIT(fwretract.settings.swap_retract_length)
+        , PSTR(" F"), LINEAR_UNIT(MMS_TO_MMM(fwretract.settings.retract_feedrate_mm_s))
+        , SP_Z_STR, LINEAR_UNIT(fwretract.settings.retract_zraise)
+      );
+
+      CONFIG_ECHO_HEADING("Recover: S<length> F<units/m>");
+      CONFIG_ECHO_START();
+      SERIAL_ECHOLNPAIR(
+          "  M208 S", LINEAR_UNIT(fwretract.settings.retract_recover_extra)
+        , " W", LINEAR_UNIT(fwretract.settings.swap_retract_recover_extra)
+        , " F", LINEAR_UNIT(MMS_TO_MMM(fwretract.settings.retract_recover_feedrate_mm_s))
+      );
+
+      #if ENABLED(FWRETRACT_AUTORETRACT)
+
+        CONFIG_ECHO_HEADING("Auto-Retract: S=0 to disable, 1 to interpret E-only moves as retract/recover");
+        CONFIG_ECHO_START();
+        SERIAL_ECHOLNPAIR("  M209 S", fwretract.autoretract_enabled ? 1 : 0);
+
+      #endif // FWRETRACT_AUTORETRACT
+
+    #endif // FWRETRACT
+
+    /**
+     * Probe Offset
+     */
+    #if HAS_BED_PROBE
+      config_heading(forReplay, PSTR("Z-Probe Offset"), false);
+      if (!forReplay) say_units(true);
+      CONFIG_ECHO_START();
+      SERIAL_ECHOLNPAIR_P(
+        #if HAS_PROBE_XY_OFFSET
+          PSTR("  M851 X"), LINEAR_UNIT(probe.offset_xy.x),
+                  SP_Y_STR, LINEAR_UNIT(probe.offset_xy.y),
+                  SP_Z_STR
+        #else
+          PSTR("  M851 X0 Y0 Z")
+        #endif
+        , LINEAR_UNIT(probe.offset.z)
+      );
+    #endif
+
+    /**
+     * Bed Skew Correction
+     */
+    #if ENABLED(SKEW_CORRECTION_GCODE)
+      CONFIG_ECHO_HEADING("Skew Factor: ");
+      CONFIG_ECHO_START();
+      #if ENABLED(SKEW_CORRECTION_FOR_Z)
+        SERIAL_ECHOPAIR_F("  M852 I", LINEAR_UNIT(planner.skew_factor.xy), 6);
+        SERIAL_ECHOPAIR_F(" J", LINEAR_UNIT(planner.skew_factor.xz), 6);
+        SERIAL_ECHOLNPAIR_F(" K", LINEAR_UNIT(planner.skew_factor.yz), 6);
+      #else
+        SERIAL_ECHOLNPAIR_F("  M852 S", LINEAR_UNIT(planner.skew_factor.xy), 6);
+      #endif
+    #endif
+
+    #if HAS_TRINAMIC_CONFIG
+
+      /**
+       * TMC stepper driver current
+       */
+      CONFIG_ECHO_HEADING("Stepper driver current:");
+
+      #if AXIS_IS_TMC(X) || AXIS_IS_TMC(Y) || AXIS_IS_TMC(Z)
+        say_M906(forReplay);
+        #if AXIS_IS_TMC(X)
+          SERIAL_ECHOPAIR_P(SP_X_STR, stepperX.getMilliamps());
+        #endif
+        #if AXIS_IS_TMC(Y)
+          SERIAL_ECHOPAIR_P(SP_Y_STR, stepperY.getMilliamps());
+        #endif
+        #if AXIS_IS_TMC(Z)
+          SERIAL_ECHOPAIR_P(SP_Z_STR, stepperZ.getMilliamps());
+        #endif
+        SERIAL_EOL();
+      #endif
+
+      #if AXIS_IS_TMC(X2) || AXIS_IS_TMC(Y2) || AXIS_IS_TMC(Z2)
+        say_M906(forReplay);
+        SERIAL_ECHOPGM(" I1");
+        #if AXIS_IS_TMC(X2)
+          SERIAL_ECHOPAIR_P(SP_X_STR, stepperX2.getMilliamps());
+        #endif
+        #if AXIS_IS_TMC(Y2)
+          SERIAL_ECHOPAIR_P(SP_Y_STR, stepperY2.getMilliamps());
+        #endif
+        #if AXIS_IS_TMC(Z2)
+          SERIAL_ECHOPAIR_P(SP_Z_STR, stepperZ2.getMilliamps());
+        #endif
+        SERIAL_EOL();
+      #endif
+
+      #if AXIS_IS_TMC(Z3)
+        say_M906(forReplay);
+        SERIAL_ECHOLNPAIR(" I2 Z", stepperZ3.getMilliamps());
+      #endif
+
+      #if AXIS_IS_TMC(Z4)
+        say_M906(forReplay);
+        SERIAL_ECHOLNPAIR(" I3 Z", stepperZ4.getMilliamps());
+      #endif
+
+      #if AXIS_IS_TMC(E0)
+        say_M906(forReplay);
+        SERIAL_ECHOLNPAIR(" T0 E", stepperE0.getMilliamps());
+      #endif
+      #if AXIS_IS_TMC(E1)
+        say_M906(forReplay);
+        SERIAL_ECHOLNPAIR(" T1 E", stepperE1.getMilliamps());
+      #endif
+      #if AXIS_IS_TMC(E2)
+        say_M906(forReplay);
+        SERIAL_ECHOLNPAIR(" T2 E", stepperE2.getMilliamps());
+      #endif
+      #if AXIS_IS_TMC(E3)
+        say_M906(forReplay);
+        SERIAL_ECHOLNPAIR(" T3 E", stepperE3.getMilliamps());
+      #endif
+      #if AXIS_IS_TMC(E4)
+        say_M906(forReplay);
+        SERIAL_ECHOLNPAIR(" T4 E", stepperE4.getMilliamps());
+      #endif
+      #if AXIS_IS_TMC(E5)
+        say_M906(forReplay);
+        SERIAL_ECHOLNPAIR(" T5 E", stepperE5.getMilliamps());
+      #endif
+      #if AXIS_IS_TMC(E6)
+        say_M906(forReplay);
+        SERIAL_ECHOLNPAIR(" T6 E", stepperE6.getMilliamps());
+      #endif
+      #if AXIS_IS_TMC(E7)
+        say_M906(forReplay);
+        SERIAL_ECHOLNPAIR(" T7 E", stepperE7.getMilliamps());
+      #endif
+      SERIAL_EOL();
+
+      /**
+       * TMC Hybrid Threshold
+       */
+      #if ENABLED(HYBRID_THRESHOLD)
+        CONFIG_ECHO_HEADING("Hybrid Threshold:");
+        #if AXIS_HAS_STEALTHCHOP(X) || AXIS_HAS_STEALTHCHOP(Y) || AXIS_HAS_STEALTHCHOP(Z)
+          say_M913(forReplay);
+          #if AXIS_HAS_STEALTHCHOP(X)
+            SERIAL_ECHOPAIR_P(SP_X_STR, stepperX.get_pwm_thrs());
+          #endif
+          #if AXIS_HAS_STEALTHCHOP(Y)
+            SERIAL_ECHOPAIR_P(SP_Y_STR, stepperY.get_pwm_thrs());
+          #endif
+          #if AXIS_HAS_STEALTHCHOP(Z)
+            SERIAL_ECHOPAIR_P(SP_Z_STR, stepperZ.get_pwm_thrs());
+          #endif
+          SERIAL_EOL();
+        #endif
+
+        #if AXIS_HAS_STEALTHCHOP(X2) || AXIS_HAS_STEALTHCHOP(Y2) || AXIS_HAS_STEALTHCHOP(Z2)
+          say_M913(forReplay);
+          SERIAL_ECHOPGM(" I1");
+          #if AXIS_HAS_STEALTHCHOP(X2)
+            SERIAL_ECHOPAIR_P(SP_X_STR, stepperX2.get_pwm_thrs());
+          #endif
+          #if AXIS_HAS_STEALTHCHOP(Y2)
+            SERIAL_ECHOPAIR_P(SP_Y_STR, stepperY2.get_pwm_thrs());
+          #endif
+          #if AXIS_HAS_STEALTHCHOP(Z2)
+            SERIAL_ECHOPAIR_P(SP_Z_STR, stepperZ2.get_pwm_thrs());
+          #endif
+          SERIAL_EOL();
+        #endif
+
+        #if AXIS_HAS_STEALTHCHOP(Z3)
+          say_M913(forReplay);
+          SERIAL_ECHOLNPAIR(" I2 Z", stepperZ3.get_pwm_thrs());
+        #endif
+
+        #if AXIS_HAS_STEALTHCHOP(Z4)
+          say_M913(forReplay);
+          SERIAL_ECHOLNPAIR(" I3 Z", stepperZ4.get_pwm_thrs());
+        #endif
+
+        #if AXIS_HAS_STEALTHCHOP(E0)
+          say_M913(forReplay);
+          SERIAL_ECHOLNPAIR(" T0 E", stepperE0.get_pwm_thrs());
+        #endif
+        #if AXIS_HAS_STEALTHCHOP(E1)
+          say_M913(forReplay);
+          SERIAL_ECHOLNPAIR(" T1 E", stepperE1.get_pwm_thrs());
+        #endif
+        #if AXIS_HAS_STEALTHCHOP(E2)
+          say_M913(forReplay);
+          SERIAL_ECHOLNPAIR(" T2 E", stepperE2.get_pwm_thrs());
+        #endif
+        #if AXIS_HAS_STEALTHCHOP(E3)
+          say_M913(forReplay);
+          SERIAL_ECHOLNPAIR(" T3 E", stepperE3.get_pwm_thrs());
+        #endif
+        #if AXIS_HAS_STEALTHCHOP(E4)
+          say_M913(forReplay);
+          SERIAL_ECHOLNPAIR(" T4 E", stepperE4.get_pwm_thrs());
+        #endif
+        #if AXIS_HAS_STEALTHCHOP(E5)
+          say_M913(forReplay);
+          SERIAL_ECHOLNPAIR(" T5 E", stepperE5.get_pwm_thrs());
+        #endif
+        #if AXIS_HAS_STEALTHCHOP(E6)
+          say_M913(forReplay);
+          SERIAL_ECHOLNPAIR(" T6 E", stepperE6.get_pwm_thrs());
+        #endif
+        #if AXIS_HAS_STEALTHCHOP(E7)
+          say_M913(forReplay);
+          SERIAL_ECHOLNPAIR(" T7 E", stepperE7.get_pwm_thrs());
+        #endif
+        SERIAL_EOL();
+      #endif // HYBRID_THRESHOLD
+
+      /**
+       * TMC Sensorless homing thresholds
+       */
+      #if USE_SENSORLESS
+        CONFIG_ECHO_HEADING("StallGuard threshold:");
+        #if X_SENSORLESS || Y_SENSORLESS || Z_SENSORLESS
+          CONFIG_ECHO_START();
+          say_M914();
+          #if X_SENSORLESS
+            SERIAL_ECHOPAIR_P(SP_X_STR, stepperX.homing_threshold());
+          #endif
+          #if Y_SENSORLESS
+            SERIAL_ECHOPAIR_P(SP_Y_STR, stepperY.homing_threshold());
+          #endif
+          #if Z_SENSORLESS
+            SERIAL_ECHOPAIR_P(SP_Z_STR, stepperZ.homing_threshold());
+          #endif
+          SERIAL_EOL();
+        #endif
+
+        #if X2_SENSORLESS || Y2_SENSORLESS || Z2_SENSORLESS
+          CONFIG_ECHO_START();
+          say_M914();
+          SERIAL_ECHOPGM(" I1");
+          #if X2_SENSORLESS
+            SERIAL_ECHOPAIR_P(SP_X_STR, stepperX2.homing_threshold());
+          #endif
+          #if Y2_SENSORLESS
+            SERIAL_ECHOPAIR_P(SP_Y_STR, stepperY2.homing_threshold());
+          #endif
+          #if Z2_SENSORLESS
+            SERIAL_ECHOPAIR_P(SP_Z_STR, stepperZ2.homing_threshold());
+          #endif
+          SERIAL_EOL();
+        #endif
+
+        #if Z3_SENSORLESS
+          CONFIG_ECHO_START();
+          say_M914();
+          SERIAL_ECHOLNPAIR(" I2 Z", stepperZ3.homing_threshold());
+        #endif
+
+        #if Z4_SENSORLESS
+          CONFIG_ECHO_START();
+          say_M914();
+          SERIAL_ECHOLNPAIR(" I3 Z", stepperZ4.homing_threshold());
+        #endif
+
+      #endif // USE_SENSORLESS
+
+      /**
+       * TMC stepping mode
+       */
+      #if HAS_STEALTHCHOP
+        CONFIG_ECHO_HEADING("Driver stepping mode:");
+        #if AXIS_HAS_STEALTHCHOP(X)
+          const bool chop_x = stepperX.get_stored_stealthChop();
+        #else
+          constexpr bool chop_x = false;
+        #endif
+        #if AXIS_HAS_STEALTHCHOP(Y)
+          const bool chop_y = stepperY.get_stored_stealthChop();
+        #else
+          constexpr bool chop_y = false;
+        #endif
+        #if AXIS_HAS_STEALTHCHOP(Z)
+          const bool chop_z = stepperZ.get_stored_stealthChop();
+        #else
+          constexpr bool chop_z = false;
+        #endif
+
+        if (chop_x || chop_y || chop_z) {
+          say_M569(forReplay);
+          if (chop_x) SERIAL_ECHOPGM_P(SP_X_STR);
+          if (chop_y) SERIAL_ECHOPGM_P(SP_Y_STR);
+          if (chop_z) SERIAL_ECHOPGM_P(SP_Z_STR);
+          SERIAL_EOL();
+        }
+
+        #if AXIS_HAS_STEALTHCHOP(X2)
+          const bool chop_x2 = stepperX2.get_stored_stealthChop();
+        #else
+          constexpr bool chop_x2 = false;
+        #endif
+        #if AXIS_HAS_STEALTHCHOP(Y2)
+          const bool chop_y2 = stepperY2.get_stored_stealthChop();
+        #else
+          constexpr bool chop_y2 = false;
+        #endif
+        #if AXIS_HAS_STEALTHCHOP(Z2)
+          const bool chop_z2 = stepperZ2.get_stored_stealthChop();
+        #else
+          constexpr bool chop_z2 = false;
+        #endif
+
+        if (chop_x2 || chop_y2 || chop_z2) {
+          say_M569(forReplay, PSTR("I1"));
+          if (chop_x2) SERIAL_ECHOPGM_P(SP_X_STR);
+          if (chop_y2) SERIAL_ECHOPGM_P(SP_Y_STR);
+          if (chop_z2) SERIAL_ECHOPGM_P(SP_Z_STR);
+          SERIAL_EOL();
+        }
+
+        #if AXIS_HAS_STEALTHCHOP(Z3)
+          if (stepperZ3.get_stored_stealthChop()) { say_M569(forReplay, PSTR("I2 Z"), true); }
+        #endif
+
+        #if AXIS_HAS_STEALTHCHOP(Z4)
+          if (stepperZ4.get_stored_stealthChop()) { say_M569(forReplay, PSTR("I3 Z"), true); }
+        #endif
+
+        #if AXIS_HAS_STEALTHCHOP(E0)
+          if (stepperE0.get_stored_stealthChop()) { say_M569(forReplay, PSTR("T0 E"), true); }
+        #endif
+        #if AXIS_HAS_STEALTHCHOP(E1)
+          if (stepperE1.get_stored_stealthChop()) { say_M569(forReplay, PSTR("T1 E"), true); }
+        #endif
+        #if AXIS_HAS_STEALTHCHOP(E2)
+          if (stepperE2.get_stored_stealthChop()) { say_M569(forReplay, PSTR("T2 E"), true); }
+        #endif
+        #if AXIS_HAS_STEALTHCHOP(E3)
+          if (stepperE3.get_stored_stealthChop()) { say_M569(forReplay, PSTR("T3 E"), true); }
+        #endif
+        #if AXIS_HAS_STEALTHCHOP(E4)
+          if (stepperE4.get_stored_stealthChop()) { say_M569(forReplay, PSTR("T4 E"), true); }
+        #endif
+        #if AXIS_HAS_STEALTHCHOP(E5)
+          if (stepperE5.get_stored_stealthChop()) { say_M569(forReplay, PSTR("T5 E"), true); }
+        #endif
+        #if AXIS_HAS_STEALTHCHOP(E6)
+          if (stepperE6.get_stored_stealthChop()) { say_M569(forReplay, PSTR("T6 E"), true); }
+        #endif
+        #if AXIS_HAS_STEALTHCHOP(E7)
+          if (stepperE7.get_stored_stealthChop()) { say_M569(forReplay, PSTR("T7 E"), true); }
+        #endif
+
+      #endif // HAS_STEALTHCHOP
+
+    #endif // HAS_TRINAMIC_CONFIG
+
+    /**
+     * Linear Advance
+     */
+    #if ENABLED(LIN_ADVANCE)
+      CONFIG_ECHO_HEADING("Linear Advance:");
+      #if EXTRUDERS < 2
+        CONFIG_ECHO_START();
+        SERIAL_ECHOLNPAIR("  M900 K", planner.extruder_advance_K[0]);
+      #else
+        LOOP_L_N(i, EXTRUDERS) {
+          CONFIG_ECHO_START();
+          SERIAL_ECHOLNPAIR("  M900 T", int(i), " K", planner.extruder_advance_K[i]);
+        }
+      #endif
+    #endif
+
+    #if EITHER(HAS_MOTOR_CURRENT_SPI, HAS_MOTOR_CURRENT_PWM)
+      CONFIG_ECHO_HEADING("Stepper motor currents:");
+      CONFIG_ECHO_START();
+      #if HAS_MOTOR_CURRENT_PWM
+        SERIAL_ECHOLNPAIR_P(                                   // PWM-based has 3 values:
+            PSTR("  M907 X"), stepper.motor_current_setting[0] // X and Y
+                  , SP_Z_STR, stepper.motor_current_setting[1] // Z
+                  , SP_E_STR, stepper.motor_current_setting[2] // E
+        );
+      #elif HAS_MOTOR_CURRENT_SPI
+        SERIAL_ECHOPGM("  M907");                              // SPI-based has 5 values:
+        LOOP_XYZE(q) {                                         // X Y Z E (map to X Y Z E0 by default)
+          SERIAL_CHAR(' ', axis_codes[q]);
+          SERIAL_ECHO(stepper.motor_current_setting[q]);
+        }
+        SERIAL_CHAR(' ', 'B');                                 // B (maps to E1 by default)
+        SERIAL_ECHOLN(stepper.motor_current_setting[4]);
+      #endif
+    #elif ENABLED(HAS_MOTOR_CURRENT_I2C)                       // i2c-based has any number of values
+      // Values sent over i2c are not stored.
+      // Indexes map directly to drivers, not axes.
+    #elif ENABLED(HAS_MOTOR_CURRENT_DAC)                       // DAC-based has 4 values, for X Y Z E
+      // Values sent over i2c are not stored. Uses indirect mapping.
+    #endif
+
+    /**
+     * Advanced Pause filament load & unload lengths
+     */
+    #if ENABLED(ADVANCED_PAUSE_FEATURE)
+      CONFIG_ECHO_HEADING("Filament load/unload lengths:");
+      #if EXTRUDERS == 1
+        say_M603(forReplay);
+        SERIAL_ECHOLNPAIR("L", LINEAR_UNIT(fc_settings[0].load_length), " U", LINEAR_UNIT(fc_settings[0].unload_length));
+      #else
+        #define _ECHO_603(N) do{ say_M603(forReplay); SERIAL_ECHOLNPAIR("T" STRINGIFY(N) " L", LINEAR_UNIT(fc_settings[N].load_length), " U", LINEAR_UNIT(fc_settings[N].unload_length)); }while(0);
+        REPEAT(EXTRUDERS, _ECHO_603)
+      #endif
+    #endif
+
+    #if HAS_MULTI_EXTRUDER
+      CONFIG_ECHO_HEADING("Tool-changing:");
+      CONFIG_ECHO_START();
+      M217_report(true);
+    #endif
+
+    #if ENABLED(BACKLASH_GCODE)
+      CONFIG_ECHO_HEADING("Backlash compensation:");
+      CONFIG_ECHO_START();
+      SERIAL_ECHOLNPAIR_P(
+          PSTR("  M425 F"), backlash.get_correction()
+        , SP_X_STR, LINEAR_UNIT(backlash.distance_mm.x)
+        , SP_Y_STR, LINEAR_UNIT(backlash.distance_mm.y)
+        , SP_Z_STR, LINEAR_UNIT(backlash.distance_mm.z)
+        #ifdef BACKLASH_SMOOTHING_MM
+          , PSTR(" S"), LINEAR_UNIT(backlash.smoothing_mm)
+        #endif
+      );
+    #endif
+
+    #if HAS_FILAMENT_SENSOR
+      CONFIG_ECHO_HEADING("Filament runout sensor:");
+      CONFIG_ECHO_START();
+      SERIAL_ECHOLNPAIR(
+        "  M412 S", int(runout.enabled)
+        #if HAS_FILAMENT_RUNOUT_DISTANCE
+          , " D", LINEAR_UNIT(runout.runout_distance())
+        #endif
+      );
+    #endif
+
+    #if HAS_ETHERNET
+      CONFIG_ECHO_HEADING("Ethernet:");
+      if (!forReplay) { CONFIG_ECHO_START(); ETH0_report(); }
+      CONFIG_ECHO_START(); SERIAL_ECHO_SP(2); MAC_report();
+      CONFIG_ECHO_START(); SERIAL_ECHO_SP(2); M552_report();
+      CONFIG_ECHO_START(); SERIAL_ECHO_SP(2); M553_report();
+      CONFIG_ECHO_START(); SERIAL_ECHO_SP(2); M554_report();
+    #endif
+
+    #if HAS_MULTI_LANGUAGE
+      CONFIG_ECHO_HEADING("UI Language:");
+      SERIAL_ECHO_MSG("  M414 S", int(ui.language));
+    #endif
+  }
+
+#endif // !DISABLE_M503
+
+#pragma pack(pop)
diff --git a/Marlin/src/module/settings.h b/Marlin/src/module/settings.h
new file mode 100644
index 0000000..b213de9
--- /dev/null
+++ b/Marlin/src/module/settings.h
@@ -0,0 +1,111 @@
+/**
+ * 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
+
+//
+// settings.cpp - Settings and EEPROM storage
+//
+
+#include "../inc/MarlinConfig.h"
+
+#if ENABLED(EEPROM_SETTINGS)
+  #include "../HAL/shared/eeprom_api.h"
+#endif
+
+class MarlinSettings {
+  public:
+    static uint16_t datasize();
+
+    static void reset();
+    static bool save();    // Return 'true' if data was saved
+
+    FORCE_INLINE static bool init_eeprom() {
+      reset();
+      #if ENABLED(EEPROM_SETTINGS)
+        const bool success = save();
+        if (TERN0(EEPROM_CHITCHAT, success)) report();
+        return success;
+      #else
+        return true;
+      #endif
+    }
+
+    #if ENABLED(SD_FIRMWARE_UPDATE)
+      static bool sd_update_status();                       // True if the SD-Firmware-Update EEPROM flag is set
+      static bool set_sd_update_status(const bool enable);  // Return 'true' after EEPROM is set (-> always true)
+    #endif
+
+    #if ENABLED(EEPROM_SETTINGS)
+
+      static bool load();      // Return 'true' if data was loaded ok
+      static bool validate();  // Return 'true' if EEPROM data is ok
+
+      static inline void first_load() {
+        static bool loaded = false;
+        if (!loaded && load()) loaded = true;
+      }
+
+      #if ENABLED(AUTO_BED_LEVELING_UBL) // Eventually make these available if any leveling system
+                                         // That can store is enabled
+        static uint16_t meshes_start_index();
+        FORCE_INLINE static uint16_t meshes_end_index() { return meshes_end; }
+        static uint16_t calc_num_meshes();
+        static int mesh_slot_offset(const int8_t slot);
+        static void store_mesh(const int8_t slot);
+        static void load_mesh(const int8_t slot, void * const into=nullptr);
+
+        //static void delete_mesh();    // necessary if we have a MAT
+        //static void defrag_meshes();  // "
+      #endif
+    #else
+      FORCE_INLINE
+      static bool load() { reset(); report(); return true; }
+      FORCE_INLINE
+      static void first_load() { (void)load(); }
+    #endif
+
+    #if DISABLED(DISABLE_M503)
+      static void report(const bool forReplay=false);
+    #else
+      FORCE_INLINE
+      static void report(const bool=false) {}
+    #endif
+
+  private:
+    static void postprocess();
+
+    #if ENABLED(EEPROM_SETTINGS)
+
+      static bool eeprom_error, validating;
+
+      #if ENABLED(AUTO_BED_LEVELING_UBL)  // Eventually make these available if any leveling system
+                                          // That can store is enabled
+        static const uint16_t meshes_end; // 128 is a placeholder for the size of the MAT; the MAT will always
+                                          // live at the very end of the eeprom
+      #endif
+
+      static bool _load();
+      static bool size_error(const uint16_t size);
+    #endif
+};
+
+extern MarlinSettings settings;
diff --git a/Marlin/src/module/speed_lookuptable.h b/Marlin/src/module/speed_lookuptable.h
new file mode 100644
index 0000000..b173ebe
--- /dev/null
+++ b/Marlin/src/module/speed_lookuptable.h
@@ -0,0 +1,168 @@
+/**
+ * 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
+
+#if F_CPU == 16000000
+
+  const uint16_t speed_lookuptable_fast[256][2] PROGMEM = {
+    { 62500, 55556}, { 6944, 3268}, { 3676, 1176}, { 2500, 607}, { 1893, 369}, { 1524, 249}, { 1275, 179}, { 1096, 135},
+    { 961, 105}, { 856, 85}, { 771, 69}, { 702, 58}, { 644, 49}, { 595, 42}, { 553, 37}, { 516, 32},
+    { 484, 28}, { 456, 25}, { 431, 23}, { 408, 20}, { 388, 19}, { 369, 16}, { 353, 16}, { 337, 14},
+    { 323, 13}, { 310, 11}, { 299, 11}, { 288, 11}, { 277, 9}, { 268, 9}, { 259, 8}, { 251, 8},
+    { 243, 8}, { 235, 7}, { 228, 6}, { 222, 6}, { 216, 6}, { 210, 6}, { 204, 5}, { 199, 5},
+    { 194, 5}, { 189, 4}, { 185, 4}, { 181, 4}, { 177, 4}, { 173, 4}, { 169, 4}, { 165, 3},
+    { 162, 3}, { 159, 4}, { 155, 3}, { 152, 3}, { 149, 2}, { 147, 3}, { 144, 3}, { 141, 2},
+    { 139, 3}, { 136, 2}, { 134, 2}, { 132, 3}, { 129, 2}, { 127, 2}, { 125, 2}, { 123, 2},
+    { 121, 2}, { 119, 1}, { 118, 2}, { 116, 2}, { 114, 1}, { 113, 2}, { 111, 2}, { 109, 1},
+    { 108, 2}, { 106, 1}, { 105, 2}, { 103, 1}, { 102, 1}, { 101, 1}, { 100, 2}, { 98, 1},
+    { 97, 1}, { 96, 1}, { 95, 2}, { 93, 1}, { 92, 1}, { 91, 1}, { 90, 1}, { 89, 1},
+    { 88, 1}, { 87, 1}, { 86, 1}, { 85, 1}, { 84, 1}, { 83, 0}, { 83, 1}, { 82, 1},
+    { 81, 1}, { 80, 1}, { 79, 1}, { 78, 0}, { 78, 1}, { 77, 1}, { 76, 1}, { 75, 0},
+    { 75, 1}, { 74, 1}, { 73, 1}, { 72, 0}, { 72, 1}, { 71, 1}, { 70, 0}, { 70, 1},
+    { 69, 0}, { 69, 1}, { 68, 1}, { 67, 0}, { 67, 1}, { 66, 0}, { 66, 1}, { 65, 0},
+    { 65, 1}, { 64, 1}, { 63, 0}, { 63, 1}, { 62, 0}, { 62, 1}, { 61, 0}, { 61, 1},
+    { 60, 0}, { 60, 0}, { 60, 1}, { 59, 0}, { 59, 1}, { 58, 0}, { 58, 1}, { 57, 0},
+    { 57, 1}, { 56, 0}, { 56, 0}, { 56, 1}, { 55, 0}, { 55, 1}, { 54, 0}, { 54, 0},
+    { 54, 1}, { 53, 0}, { 53, 0}, { 53, 1}, { 52, 0}, { 52, 0}, { 52, 1}, { 51, 0},
+    { 51, 0}, { 51, 1}, { 50, 0}, { 50, 0}, { 50, 1}, { 49, 0}, { 49, 0}, { 49, 1},
+    { 48, 0}, { 48, 0}, { 48, 1}, { 47, 0}, { 47, 0}, { 47, 0}, { 47, 1}, { 46, 0},
+    { 46, 0}, { 46, 1}, { 45, 0}, { 45, 0}, { 45, 0}, { 45, 1}, { 44, 0}, { 44, 0},
+    { 44, 0}, { 44, 1}, { 43, 0}, { 43, 0}, { 43, 0}, { 43, 1}, { 42, 0}, { 42, 0},
+    { 42, 0}, { 42, 1}, { 41, 0}, { 41, 0}, { 41, 0}, { 41, 0}, { 41, 1}, { 40, 0},
+    { 40, 0}, { 40, 0}, { 40, 0}, { 40, 1}, { 39, 0}, { 39, 0}, { 39, 0}, { 39, 0},
+    { 39, 1}, { 38, 0}, { 38, 0}, { 38, 0}, { 38, 0}, { 38, 1}, { 37, 0}, { 37, 0},
+    { 37, 0}, { 37, 0}, { 37, 0}, { 37, 1}, { 36, 0}, { 36, 0}, { 36, 0}, { 36, 0},
+    { 36, 1}, { 35, 0}, { 35, 0}, { 35, 0}, { 35, 0}, { 35, 0}, { 35, 0}, { 35, 1},
+    { 34, 0}, { 34, 0}, { 34, 0}, { 34, 0}, { 34, 0}, { 34, 1}, { 33, 0}, { 33, 0},
+    { 33, 0}, { 33, 0}, { 33, 0}, { 33, 0}, { 33, 1}, { 32, 0}, { 32, 0}, { 32, 0},
+    { 32, 0}, { 32, 0}, { 32, 0}, { 32, 0}, { 32, 1}, { 31, 0}, { 31, 0}, { 31, 0},
+    { 31, 0}, { 31, 0}, { 31, 0}, { 31, 1}, { 30, 0}, { 30, 0}, { 30, 0}, { 30, 0}
+  };
+
+  const uint16_t speed_lookuptable_slow[256][2] PROGMEM = {
+    { 62500, 12500}, { 50000, 8334}, { 41666, 5952}, { 35714, 4464}, { 31250, 3473}, { 27777, 2777}, { 25000, 2273}, { 22727, 1894},
+    { 20833, 1603}, { 19230, 1373}, { 17857, 1191}, { 16666, 1041}, { 15625, 920}, { 14705, 817}, { 13888, 731}, { 13157, 657},
+    { 12500, 596}, { 11904, 541}, { 11363, 494}, { 10869, 453}, { 10416, 416}, { 10000, 385}, { 9615, 356}, { 9259, 331},
+    { 8928, 308}, { 8620, 287}, { 8333, 269}, { 8064, 252}, { 7812, 237}, { 7575, 223}, { 7352, 210}, { 7142, 198},
+    { 6944, 188}, { 6756, 178}, { 6578, 168}, { 6410, 160}, { 6250, 153}, { 6097, 145}, { 5952, 139}, { 5813, 132},
+    { 5681, 126}, { 5555, 121}, { 5434, 115}, { 5319, 111}, { 5208, 106}, { 5102, 102}, { 5000, 99}, { 4901, 94},
+    { 4807, 91}, { 4716, 87}, { 4629, 84}, { 4545, 81}, { 4464, 79}, { 4385, 75}, { 4310, 73}, { 4237, 71},
+    { 4166, 68}, { 4098, 66}, { 4032, 64}, { 3968, 62}, { 3906, 60}, { 3846, 59}, { 3787, 56}, { 3731, 55},
+    { 3676, 53}, { 3623, 52}, { 3571, 50}, { 3521, 49}, { 3472, 48}, { 3424, 46}, { 3378, 45}, { 3333, 44},
+    { 3289, 43}, { 3246, 41}, { 3205, 41}, { 3164, 39}, { 3125, 39}, { 3086, 38}, { 3048, 36}, { 3012, 36},
+    { 2976, 35}, { 2941, 35}, { 2906, 33}, { 2873, 33}, { 2840, 32}, { 2808, 31}, { 2777, 30}, { 2747, 30},
+    { 2717, 29}, { 2688, 29}, { 2659, 28}, { 2631, 27}, { 2604, 27}, { 2577, 26}, { 2551, 26}, { 2525, 25},
+    { 2500, 25}, { 2475, 25}, { 2450, 23}, { 2427, 24}, { 2403, 23}, { 2380, 22}, { 2358, 22}, { 2336, 22},
+    { 2314, 21}, { 2293, 21}, { 2272, 20}, { 2252, 20}, { 2232, 20}, { 2212, 20}, { 2192, 19}, { 2173, 18},
+    { 2155, 19}, { 2136, 18}, { 2118, 18}, { 2100, 17}, { 2083, 17}, { 2066, 17}, { 2049, 17}, { 2032, 16},
+    { 2016, 16}, { 2000, 16}, { 1984, 16}, { 1968, 15}, { 1953, 16}, { 1937, 14}, { 1923, 15}, { 1908, 15},
+    { 1893, 14}, { 1879, 14}, { 1865, 14}, { 1851, 13}, { 1838, 14}, { 1824, 13}, { 1811, 13}, { 1798, 13},
+    { 1785, 12}, { 1773, 13}, { 1760, 12}, { 1748, 12}, { 1736, 12}, { 1724, 12}, { 1712, 12}, { 1700, 11},
+    { 1689, 12}, { 1677, 11}, { 1666, 11}, { 1655, 11}, { 1644, 11}, { 1633, 10}, { 1623, 11}, { 1612, 10},
+    { 1602, 10}, { 1592, 10}, { 1582, 10}, { 1572, 10}, { 1562, 10}, { 1552, 9}, { 1543, 10}, { 1533, 9},
+    { 1524, 9}, { 1515, 9}, { 1506, 9}, { 1497, 9}, { 1488, 9}, { 1479, 9}, { 1470, 9}, { 1461, 8},
+    { 1453, 8}, { 1445, 9}, { 1436, 8}, { 1428, 8}, { 1420, 8}, { 1412, 8}, { 1404, 8}, { 1396, 8},
+    { 1388, 7}, { 1381, 8}, { 1373, 7}, { 1366, 8}, { 1358, 7}, { 1351, 7}, { 1344, 8}, { 1336, 7},
+    { 1329, 7}, { 1322, 7}, { 1315, 7}, { 1308, 6}, { 1302, 7}, { 1295, 7}, { 1288, 6}, { 1282, 7},
+    { 1275, 6}, { 1269, 7}, { 1262, 6}, { 1256, 6}, { 1250, 7}, { 1243, 6}, { 1237, 6}, { 1231, 6},
+    { 1225, 6}, { 1219, 6}, { 1213, 6}, { 1207, 6}, { 1201, 5}, { 1196, 6}, { 1190, 6}, { 1184, 5},
+    { 1179, 6}, { 1173, 5}, { 1168, 6}, { 1162, 5}, { 1157, 5}, { 1152, 6}, { 1146, 5}, { 1141, 5},
+    { 1136, 5}, { 1131, 5}, { 1126, 5}, { 1121, 5}, { 1116, 5}, { 1111, 5}, { 1106, 5}, { 1101, 5},
+    { 1096, 5}, { 1091, 5}, { 1086, 4}, { 1082, 5}, { 1077, 5}, { 1072, 4}, { 1068, 5}, { 1063, 4},
+    { 1059, 5}, { 1054, 4}, { 1050, 4}, { 1046, 5}, { 1041, 4}, { 1037, 4}, { 1033, 5}, { 1028, 4},
+    { 1024, 4}, { 1020, 4}, { 1016, 4}, { 1012, 4}, { 1008, 4}, { 1004, 4}, { 1000, 4}, { 996, 4},
+    { 992, 4}, { 988, 4}, { 984, 4}, { 980, 4}, { 976, 4}, { 972, 4}, { 968, 3}, { 965, 3}
+  };
+
+#elif F_CPU == 20000000
+
+  const uint16_t speed_lookuptable_fast[256][2] PROGMEM = {
+    {62500, 54055}, {8445, 3917}, {4528, 1434}, {3094, 745}, {2349, 456}, {1893, 307}, {1586, 222}, {1364, 167},
+    {1197, 131}, {1066, 105}, {961, 86}, {875, 72}, {803, 61}, {742, 53}, {689, 45}, {644, 40},
+    {604, 35}, {569, 32}, {537, 28}, {509, 25}, {484, 23}, {461, 21}, {440, 19}, {421, 17},
+    {404, 16}, {388, 15}, {373, 14}, {359, 13}, {346, 12}, {334, 11}, {323, 10}, {313, 10},
+    {303, 9}, {294, 9}, {285, 8}, {277, 7}, {270, 8}, {262, 7}, {255, 6}, {249, 6},
+    {243, 6}, {237, 6}, {231, 5}, {226, 5}, {221, 5}, {216, 5}, {211, 4}, {207, 5},
+    {202, 4}, {198, 4}, {194, 4}, {190, 3}, {187, 4}, {183, 3}, {180, 3}, {177, 4},
+    {173, 3}, {170, 3}, {167, 2}, {165, 3}, {162, 3}, {159, 2}, {157, 3}, {154, 2},
+    {152, 3}, {149, 2}, {147, 2}, {145, 2}, {143, 2}, {141, 2}, {139, 2}, {137, 2},
+    {135, 2}, {133, 2}, {131, 2}, {129, 1}, {128, 2}, {126, 2}, {124, 1}, {123, 2},
+    {121, 1}, {120, 2}, {118, 1}, {117, 1}, {116, 2}, {114, 1}, {113, 1}, {112, 2},
+    {110, 1}, {109, 1}, {108, 1}, {107, 2}, {105, 1}, {104, 1}, {103, 1}, {102, 1},
+    {101, 1}, {100, 1}, {99, 1}, {98, 1}, {97, 1}, {96, 1}, {95, 1}, {94, 1},
+    {93, 1}, {92, 1}, {91, 0}, {91, 1}, {90, 1}, {89, 1}, {88, 1}, {87, 0},
+    {87, 1}, {86, 1}, {85, 1}, {84, 0}, {84, 1}, {83, 1}, {82, 1}, {81, 0},
+    {81, 1}, {80, 1}, {79, 0}, {79, 1}, {78, 0}, {78, 1}, {77, 1}, {76, 0},
+    {76, 1}, {75, 0}, {75, 1}, {74, 1}, {73, 0}, {73, 1}, {72, 0}, {72, 1},
+    {71, 0}, {71, 1}, {70, 0}, {70, 1}, {69, 0}, {69, 1}, {68, 0}, {68, 1},
+    {67, 0}, {67, 1}, {66, 0}, {66, 1}, {65, 0}, {65, 0}, {65, 1}, {64, 0},
+    {64, 1}, {63, 0}, {63, 1}, {62, 0}, {62, 0}, {62, 1}, {61, 0}, {61, 1},
+    {60, 0}, {60, 0}, {60, 1}, {59, 0}, {59, 0}, {59, 1}, {58, 0}, {58, 0},
+    {58, 1}, {57, 0}, {57, 0}, {57, 1}, {56, 0}, {56, 0}, {56, 1}, {55, 0},
+    {55, 0}, {55, 1}, {54, 0}, {54, 0}, {54, 1}, {53, 0}, {53, 0}, {53, 0},
+    {53, 1}, {52, 0}, {52, 0}, {52, 1}, {51, 0}, {51, 0}, {51, 0}, {51, 1},
+    {50, 0}, {50, 0}, {50, 0}, {50, 1}, {49, 0}, {49, 0}, {49, 0}, {49, 1},
+    {48, 0}, {48, 0}, {48, 0}, {48, 1}, {47, 0}, {47, 0}, {47, 0}, {47, 1},
+    {46, 0}, {46, 0}, {46, 0}, {46, 0}, {46, 1}, {45, 0}, {45, 0}, {45, 0},
+    {45, 1}, {44, 0}, {44, 0}, {44, 0}, {44, 0}, {44, 1}, {43, 0}, {43, 0},
+    {43, 0}, {43, 0}, {43, 1}, {42, 0}, {42, 0}, {42, 0}, {42, 0}, {42, 0},
+    {42, 1}, {41, 0}, {41, 0}, {41, 0}, {41, 0}, {41, 0}, {41, 1}, {40, 0},
+    {40, 0}, {40, 0}, {40, 0}, {40, 1}, {39, 0}, {39, 0}, {39, 0}, {39, 0},
+    {39, 0}, {39, 0}, {39, 1}, {38, 0}, {38, 0}, {38, 0}, {38, 0}, {38, 0},
+  };
+
+  const uint16_t speed_lookuptable_slow[256][2] PROGMEM = {
+    {62500, 10417}, {52083, 7441}, {44642, 5580}, {39062, 4340}, {34722, 3472}, {31250, 2841}, {28409, 2368}, {26041, 2003},
+    {24038, 1717}, {22321, 1488}, {20833, 1302}, {19531, 1149}, {18382, 1021}, {17361, 914}, {16447, 822}, {15625, 745},
+    {14880, 676}, {14204, 618}, {13586, 566}, {13020, 520}, {12500, 481}, {12019, 445}, {11574, 414}, {11160, 385},
+    {10775, 359}, {10416, 336}, {10080, 315}, {9765, 296}, {9469, 278}, {9191, 263}, {8928, 248}, {8680, 235},
+    {8445, 222}, {8223, 211}, {8012, 200}, {7812, 191}, {7621, 181}, {7440, 173}, {7267, 165}, {7102, 158},
+    {6944, 151}, {6793, 145}, {6648, 138}, {6510, 133}, {6377, 127}, {6250, 123}, {6127, 118}, {6009, 113},
+    {5896, 109}, {5787, 106}, {5681, 101}, {5580, 98}, {5482, 95}, {5387, 91}, {5296, 88}, {5208, 86},
+    {5122, 82}, {5040, 80}, {4960, 78}, {4882, 75}, {4807, 73}, {4734, 70}, {4664, 69}, {4595, 67},
+    {4528, 64}, {4464, 63}, {4401, 61}, {4340, 60}, {4280, 58}, {4222, 56}, {4166, 55}, {4111, 53},
+    {4058, 52}, {4006, 51}, {3955, 49}, {3906, 48}, {3858, 48}, {3810, 45}, {3765, 45}, {3720, 44},
+    {3676, 43}, {3633, 42}, {3591, 40}, {3551, 40}, {3511, 39}, {3472, 38}, {3434, 38}, {3396, 36},
+    {3360, 36}, {3324, 35}, {3289, 34}, {3255, 34}, {3221, 33}, {3188, 32}, {3156, 31}, {3125, 31},
+    {3094, 31}, {3063, 30}, {3033, 29}, {3004, 28}, {2976, 28}, {2948, 28}, {2920, 27}, {2893, 27},
+    {2866, 26}, {2840, 25}, {2815, 25}, {2790, 25}, {2765, 24}, {2741, 24}, {2717, 24}, {2693, 23},
+    {2670, 22}, {2648, 22}, {2626, 22}, {2604, 22}, {2582, 21}, {2561, 21}, {2540, 20}, {2520, 20},
+    {2500, 20}, {2480, 20}, {2460, 19}, {2441, 19}, {2422, 19}, {2403, 18}, {2385, 18}, {2367, 18},
+    {2349, 17}, {2332, 18}, {2314, 17}, {2297, 16}, {2281, 17}, {2264, 16}, {2248, 16}, {2232, 16},
+    {2216, 16}, {2200, 15}, {2185, 15}, {2170, 15}, {2155, 15}, {2140, 15}, {2125, 14}, {2111, 14},
+    {2097, 14}, {2083, 14}, {2069, 14}, {2055, 13}, {2042, 13}, {2029, 13}, {2016, 13}, {2003, 13},
+    {1990, 13}, {1977, 12}, {1965, 12}, {1953, 13}, {1940, 11}, {1929, 12}, {1917, 12}, {1905, 12},
+    {1893, 11}, {1882, 11}, {1871, 11}, {1860, 11}, {1849, 11}, {1838, 11}, {1827, 11}, {1816, 10},
+    {1806, 11}, {1795, 10}, {1785, 10}, {1775, 10}, {1765, 10}, {1755, 10}, {1745, 9}, {1736, 10},
+    {1726, 9}, {1717, 10}, {1707, 9}, {1698, 9}, {1689, 9}, {1680, 9}, {1671, 9}, {1662, 9},
+    {1653, 9}, {1644, 8}, {1636, 9}, {1627, 8}, {1619, 9}, {1610, 8}, {1602, 8}, {1594, 8},
+    {1586, 8}, {1578, 8}, {1570, 8}, {1562, 8}, {1554, 7}, {1547, 8}, {1539, 8}, {1531, 7},
+    {1524, 8}, {1516, 7}, {1509, 7}, {1502, 7}, {1495, 7}, {1488, 7}, {1481, 7}, {1474, 7},
+    {1467, 7}, {1460, 7}, {1453, 7}, {1446, 6}, {1440, 7}, {1433, 7}, {1426, 6}, {1420, 6},
+    {1414, 7}, {1407, 6}, {1401, 6}, {1395, 7}, {1388, 6}, {1382, 6}, {1376, 6}, {1370, 6},
+    {1364, 6}, {1358, 6}, {1352, 6}, {1346, 5}, {1341, 6}, {1335, 6}, {1329, 5}, {1324, 6},
+    {1318, 5}, {1313, 6}, {1307, 5}, {1302, 6}, {1296, 5}, {1291, 5}, {1286, 6}, {1280, 5},
+    {1275, 5}, {1270, 5}, {1265, 5}, {1260, 5}, {1255, 5}, {1250, 5}, {1245, 5}, {1240, 5},
+    {1235, 5}, {1230, 5}, {1225, 5}, {1220, 5}, {1215, 4}, {1211, 5}, {1206, 5}, {1201, 5},
+  };
+
+#endif
diff --git a/Marlin/src/module/stepper.cpp b/Marlin/src/module/stepper.cpp
new file mode 100644
index 0000000..1033774
--- /dev/null
+++ b/Marlin/src/module/stepper.cpp
@@ -0,0 +1,3514 @@
+/**
+ * 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/>.
+ *
+ */
+
+/**
+ * stepper.cpp - A singleton object to execute motion plans using stepper motors
+ * Marlin Firmware
+ *
+ * Derived from Grbl
+ * Copyright (c) 2009-2011 Simen Svale Skogsrud
+ *
+ * Grbl is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Grbl is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Grbl.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+/**
+ * Timer calculations informed by the 'RepRap cartesian firmware' by Zack Smith
+ * and Philipp Tiefenbacher.
+ */
+
+/**
+ *         __________________________
+ *        /|                        |\     _________________         ^
+ *       / |                        | \   /|               |\        |
+ *      /  |                        |  \ / |               | \       s
+ *     /   |                        |   |  |               |  \      p
+ *    /    |                        |   |  |               |   \     e
+ *   +-----+------------------------+---+--+---------------+----+    e
+ *   |               BLOCK 1            |      BLOCK 2          |    d
+ *
+ *                           time ----->
+ *
+ *  The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates
+ *  first block->accelerate_until step_events_completed, then keeps going at constant speed until
+ *  step_events_completed reaches block->decelerate_after after which it decelerates until the trapezoid generator is reset.
+ *  The slope of acceleration is calculated using v = u + at where t is the accumulated timer values of the steps so far.
+ */
+
+/**
+ * Marlin uses the Bresenham algorithm. For a detailed explanation of theory and
+ * method see https://www.cs.helsinki.fi/group/goa/mallinnus/lines/bresenh.html
+ */
+
+/**
+ * Jerk controlled movements planner added Apr 2018 by Eduardo José Tagle.
+ * Equations based on Synthethos TinyG2 sources, but the fixed-point
+ * implementation is new, as we are running the ISR with a variable period.
+ * Also implemented the Bézier velocity curve evaluation in ARM assembler,
+ * to avoid impacting ISR speed.
+ */
+
+#include "stepper.h"
+
+Stepper stepper; // Singleton
+
+#define BABYSTEPPING_EXTRA_DIR_WAIT
+
+#ifdef __AVR__
+  #include "speed_lookuptable.h"
+#endif
+
+#include "endstops.h"
+#include "planner.h"
+#include "motion.h"
+
+#include "../lcd/marlinui.h"
+#include "../gcode/queue.h"
+#include "../sd/cardreader.h"
+#include "../MarlinCore.h"
+#include "../HAL/shared/Delay.h"
+
+#if ENABLED(INTEGRATED_BABYSTEPPING)
+  #include "../feature/babystep.h"
+#endif
+
+#if MB(ALLIGATOR)
+  #include "../feature/dac/dac_dac084s085.h"
+#endif
+
+#if HAS_MOTOR_CURRENT_SPI
+  #include <SPI.h>
+#endif
+
+#if ENABLED(MIXING_EXTRUDER)
+  #include "../feature/mixing.h"
+#endif
+
+#if HAS_FILAMENT_RUNOUT_DISTANCE
+  #include "../feature/runout.h"
+#endif
+
+#if HAS_L64XX
+  #include "../libs/L64XX/L64XX_Marlin.h"
+  uint8_t L6470_buf[MAX_L64XX + 1];   // chip command sequence - element 0 not used
+  bool L64XX_OK_to_power_up = false;  // flag to keep L64xx steppers powered down after a reset or power up
+#endif
+
+#if ENABLED(POWER_LOSS_RECOVERY)
+  #include "../feature/powerloss.h"
+#endif
+
+#if HAS_CUTTER
+  #include "../feature/spindle_laser.h"
+#endif
+
+// public:
+
+#if EITHER(HAS_EXTRA_ENDSTOPS, Z_STEPPER_AUTO_ALIGN)
+  bool Stepper::separate_multi_axis = false;
+#endif
+
+#if HAS_MOTOR_CURRENT_SPI || HAS_MOTOR_CURRENT_PWM
+  bool Stepper::initialized; // = false
+  uint32_t Stepper::motor_current_setting[MOTOR_CURRENT_COUNT]; // Initialized by settings.load()
+  #if HAS_MOTOR_CURRENT_SPI
+    constexpr uint32_t Stepper::digipot_count[];
+  #endif
+#endif
+
+// private:
+
+block_t* Stepper::current_block; // (= nullptr) A pointer to the block currently being traced
+
+uint8_t Stepper::last_direction_bits, // = 0
+        Stepper::axis_did_move; // = 0
+
+bool Stepper::abort_current_block;
+
+#if DISABLED(MIXING_EXTRUDER) && HAS_MULTI_EXTRUDER
+  uint8_t Stepper::last_moved_extruder = 0xFF;
+#endif
+
+#if ENABLED(X_DUAL_ENDSTOPS)
+  bool Stepper::locked_X_motor = false, Stepper::locked_X2_motor = false;
+#endif
+#if ENABLED(Y_DUAL_ENDSTOPS)
+  bool Stepper::locked_Y_motor = false, Stepper::locked_Y2_motor = false;
+#endif
+
+#if EITHER(Z_MULTI_ENDSTOPS, Z_STEPPER_AUTO_ALIGN)
+  bool Stepper::locked_Z_motor = false, Stepper::locked_Z2_motor = false
+    #if NUM_Z_STEPPER_DRIVERS >= 3
+      , Stepper::locked_Z3_motor = false
+      #if NUM_Z_STEPPER_DRIVERS >= 4
+        , Stepper::locked_Z4_motor = false
+      #endif
+    #endif
+  ;
+#endif
+
+uint32_t Stepper::acceleration_time, Stepper::deceleration_time;
+uint8_t Stepper::steps_per_isr;
+
+IF_DISABLED(ADAPTIVE_STEP_SMOOTHING, constexpr) uint8_t Stepper::oversampling_factor;
+
+xyze_long_t Stepper::delta_error{0};
+
+xyze_ulong_t Stepper::advance_dividend{0};
+uint32_t Stepper::advance_divisor = 0,
+         Stepper::step_events_completed = 0, // The number of step events executed in the current block
+         Stepper::accelerate_until,          // The count at which to stop accelerating
+         Stepper::decelerate_after,          // The count at which to start decelerating
+         Stepper::step_event_count;          // The total event count for the current block
+
+#if EITHER(HAS_MULTI_EXTRUDER, MIXING_EXTRUDER)
+  uint8_t Stepper::stepper_extruder;
+#else
+  constexpr uint8_t Stepper::stepper_extruder;
+#endif
+
+#if ENABLED(S_CURVE_ACCELERATION)
+  int32_t __attribute__((used)) Stepper::bezier_A __asm__("bezier_A");    // A coefficient in Bézier speed curve with alias for assembler
+  int32_t __attribute__((used)) Stepper::bezier_B __asm__("bezier_B");    // B coefficient in Bézier speed curve with alias for assembler
+  int32_t __attribute__((used)) Stepper::bezier_C __asm__("bezier_C");    // C coefficient in Bézier speed curve with alias for assembler
+  uint32_t __attribute__((used)) Stepper::bezier_F __asm__("bezier_F");   // F coefficient in Bézier speed curve with alias for assembler
+  uint32_t __attribute__((used)) Stepper::bezier_AV __asm__("bezier_AV"); // AV coefficient in Bézier speed curve with alias for assembler
+  #ifdef __AVR__
+    bool __attribute__((used)) Stepper::A_negative __asm__("A_negative"); // If A coefficient was negative
+  #endif
+  bool Stepper::bezier_2nd_half;    // =false If Bézier curve has been initialized or not
+#endif
+
+#if ENABLED(LIN_ADVANCE)
+
+  uint32_t Stepper::nextAdvanceISR = LA_ADV_NEVER,
+           Stepper::LA_isr_rate = LA_ADV_NEVER;
+  uint16_t Stepper::LA_current_adv_steps = 0,
+           Stepper::LA_final_adv_steps,
+           Stepper::LA_max_adv_steps;
+
+  int8_t   Stepper::LA_steps = 0;
+
+  bool Stepper::LA_use_advance_lead;
+
+#endif // LIN_ADVANCE
+
+#if ENABLED(INTEGRATED_BABYSTEPPING)
+  uint32_t Stepper::nextBabystepISR = BABYSTEP_NEVER;
+#endif
+
+#if ENABLED(DIRECT_STEPPING)
+  page_step_state_t Stepper::page_step_state;
+#endif
+
+int32_t Stepper::ticks_nominal = -1;
+#if DISABLED(S_CURVE_ACCELERATION)
+  uint32_t Stepper::acc_step_rate; // needed for deceleration start point
+#endif
+
+xyz_long_t Stepper::endstops_trigsteps;
+xyze_long_t Stepper::count_position{0};
+xyze_int8_t Stepper::count_direction{0};
+
+#if ENABLED(LASER_POWER_INLINE_TRAPEZOID)
+  Stepper::stepper_laser_t Stepper::laser_trap = {
+    .enabled = false,
+    .cur_power = 0,
+    .cruise_set = false,
+    #if DISABLED(LASER_POWER_INLINE_TRAPEZOID_CONT)
+      .last_step_count = 0,
+      .acc_step_count = 0
+    #else
+      .till_update = 0
+    #endif
+  };
+#endif
+
+#define DUAL_ENDSTOP_APPLY_STEP(A,V)                                                                                        \
+  if (separate_multi_axis) {                                                                                                \
+    if (A##_HOME_DIR < 0) {                                                                                                 \
+      if (!(TEST(endstops.state(), A##_MIN) && count_direction[_AXIS(A)] < 0) && !locked_##A##_motor) A##_STEP_WRITE(V);    \
+      if (!(TEST(endstops.state(), A##2_MIN) && count_direction[_AXIS(A)] < 0) && !locked_##A##2_motor) A##2_STEP_WRITE(V); \
+    }                                                                                                                       \
+    else {                                                                                                                  \
+      if (!(TEST(endstops.state(), A##_MAX) && count_direction[_AXIS(A)] > 0) && !locked_##A##_motor) A##_STEP_WRITE(V);    \
+      if (!(TEST(endstops.state(), A##2_MAX) && count_direction[_AXIS(A)] > 0) && !locked_##A##2_motor) A##2_STEP_WRITE(V); \
+    }                                                                                                                       \
+  }                                                                                                                         \
+  else {                                                                                                                    \
+    A##_STEP_WRITE(V);                                                                                                      \
+    A##2_STEP_WRITE(V);                                                                                                     \
+  }
+
+#define DUAL_SEPARATE_APPLY_STEP(A,V)             \
+  if (separate_multi_axis) {                      \
+    if (!locked_##A##_motor) A##_STEP_WRITE(V);   \
+    if (!locked_##A##2_motor) A##2_STEP_WRITE(V); \
+  }                                               \
+  else {                                          \
+    A##_STEP_WRITE(V);                            \
+    A##2_STEP_WRITE(V);                           \
+  }
+
+#define TRIPLE_ENDSTOP_APPLY_STEP(A,V)                                                                                      \
+  if (separate_multi_axis) {                                                                                                \
+    if (A##_HOME_DIR < 0) {                                                                                                 \
+      if (!(TEST(endstops.state(), A##_MIN) && count_direction[_AXIS(A)] < 0) && !locked_##A##_motor) A##_STEP_WRITE(V);    \
+      if (!(TEST(endstops.state(), A##2_MIN) && count_direction[_AXIS(A)] < 0) && !locked_##A##2_motor) A##2_STEP_WRITE(V); \
+      if (!(TEST(endstops.state(), A##3_MIN) && count_direction[_AXIS(A)] < 0) && !locked_##A##3_motor) A##3_STEP_WRITE(V); \
+    }                                                                                                                       \
+    else {                                                                                                                  \
+      if (!(TEST(endstops.state(), A##_MAX) && count_direction[_AXIS(A)] > 0) && !locked_##A##_motor) A##_STEP_WRITE(V);    \
+      if (!(TEST(endstops.state(), A##2_MAX) && count_direction[_AXIS(A)] > 0) && !locked_##A##2_motor) A##2_STEP_WRITE(V); \
+      if (!(TEST(endstops.state(), A##3_MAX) && count_direction[_AXIS(A)] > 0) && !locked_##A##3_motor) A##3_STEP_WRITE(V); \
+    }                                                                                                                       \
+  }                                                                                                                         \
+  else {                                                                                                                    \
+    A##_STEP_WRITE(V);                                                                                                      \
+    A##2_STEP_WRITE(V);                                                                                                     \
+    A##3_STEP_WRITE(V);                                                                                                     \
+  }
+
+#define TRIPLE_SEPARATE_APPLY_STEP(A,V)           \
+  if (separate_multi_axis) {                      \
+    if (!locked_##A##_motor) A##_STEP_WRITE(V);   \
+    if (!locked_##A##2_motor) A##2_STEP_WRITE(V); \
+    if (!locked_##A##3_motor) A##3_STEP_WRITE(V); \
+  }                                               \
+  else {                                          \
+    A##_STEP_WRITE(V);                            \
+    A##2_STEP_WRITE(V);                           \
+    A##3_STEP_WRITE(V);                           \
+  }
+
+#define QUAD_ENDSTOP_APPLY_STEP(A,V)                                                                                        \
+  if (separate_multi_axis) {                                                                                                \
+    if (A##_HOME_DIR < 0) {                                                                                                 \
+      if (!(TEST(endstops.state(), A##_MIN) && count_direction[_AXIS(A)] < 0) && !locked_##A##_motor) A##_STEP_WRITE(V);    \
+      if (!(TEST(endstops.state(), A##2_MIN) && count_direction[_AXIS(A)] < 0) && !locked_##A##2_motor) A##2_STEP_WRITE(V); \
+      if (!(TEST(endstops.state(), A##3_MIN) && count_direction[_AXIS(A)] < 0) && !locked_##A##3_motor) A##3_STEP_WRITE(V); \
+      if (!(TEST(endstops.state(), A##4_MIN) && count_direction[_AXIS(A)] < 0) && !locked_##A##4_motor) A##4_STEP_WRITE(V); \
+    }                                                                                                                       \
+    else {                                                                                                                  \
+      if (!(TEST(endstops.state(), A##_MAX) && count_direction[_AXIS(A)] > 0) && !locked_##A##_motor) A##_STEP_WRITE(V);    \
+      if (!(TEST(endstops.state(), A##2_MAX) && count_direction[_AXIS(A)] > 0) && !locked_##A##2_motor) A##2_STEP_WRITE(V); \
+      if (!(TEST(endstops.state(), A##3_MAX) && count_direction[_AXIS(A)] > 0) && !locked_##A##3_motor) A##3_STEP_WRITE(V); \
+      if (!(TEST(endstops.state(), A##4_MAX) && count_direction[_AXIS(A)] > 0) && !locked_##A##4_motor) A##4_STEP_WRITE(V); \
+    }                                                                                                                       \
+  }                                                                                                                         \
+  else {                                                                                                                    \
+    A##_STEP_WRITE(V);                                                                                                      \
+    A##2_STEP_WRITE(V);                                                                                                     \
+    A##3_STEP_WRITE(V);                                                                                                     \
+    A##4_STEP_WRITE(V);                                                                                                     \
+  }
+
+#define QUAD_SEPARATE_APPLY_STEP(A,V)             \
+  if (separate_multi_axis) {                      \
+    if (!locked_##A##_motor) A##_STEP_WRITE(V);   \
+    if (!locked_##A##2_motor) A##2_STEP_WRITE(V); \
+    if (!locked_##A##3_motor) A##3_STEP_WRITE(V); \
+    if (!locked_##A##4_motor) A##4_STEP_WRITE(V); \
+  }                                               \
+  else {                                          \
+    A##_STEP_WRITE(V);                            \
+    A##2_STEP_WRITE(V);                           \
+    A##3_STEP_WRITE(V);                           \
+    A##4_STEP_WRITE(V);                           \
+  }
+
+#if ENABLED(X_DUAL_STEPPER_DRIVERS)
+  #define X_APPLY_DIR(v,Q) do{ X_DIR_WRITE(v); X2_DIR_WRITE((v) ^ ENABLED(INVERT_X2_VS_X_DIR)); }while(0)
+  #if ENABLED(X_DUAL_ENDSTOPS)
+    #define X_APPLY_STEP(v,Q) DUAL_ENDSTOP_APPLY_STEP(X,v)
+  #else
+    #define X_APPLY_STEP(v,Q) do{ X_STEP_WRITE(v); X2_STEP_WRITE(v); }while(0)
+  #endif
+#elif ENABLED(DUAL_X_CARRIAGE)
+  #define X_APPLY_DIR(v,ALWAYS) do{ \
+    if (extruder_duplication_enabled || ALWAYS) { X_DIR_WRITE(v); X2_DIR_WRITE((v) ^ idex_mirrored_mode); } \
+    else if (last_moved_extruder) X2_DIR_WRITE(v); else X_DIR_WRITE(v); \
+  }while(0)
+  #define X_APPLY_STEP(v,ALWAYS) do{ \
+    if (extruder_duplication_enabled || ALWAYS) { X_STEP_WRITE(v); X2_STEP_WRITE(v); } \
+    else if (last_moved_extruder) X2_STEP_WRITE(v); else X_STEP_WRITE(v); \
+  }while(0)
+#else
+  #define X_APPLY_DIR(v,Q) X_DIR_WRITE(v)
+  #define X_APPLY_STEP(v,Q) X_STEP_WRITE(v)
+#endif
+
+#if ENABLED(Y_DUAL_STEPPER_DRIVERS)
+  #define Y_APPLY_DIR(v,Q) do{ Y_DIR_WRITE(v); Y2_DIR_WRITE((v) ^ ENABLED(INVERT_Y2_VS_Y_DIR)); }while(0)
+  #if ENABLED(Y_DUAL_ENDSTOPS)
+    #define Y_APPLY_STEP(v,Q) DUAL_ENDSTOP_APPLY_STEP(Y,v)
+  #else
+    #define Y_APPLY_STEP(v,Q) do{ Y_STEP_WRITE(v); Y2_STEP_WRITE(v); }while(0)
+  #endif
+#else
+  #define Y_APPLY_DIR(v,Q) Y_DIR_WRITE(v)
+  #define Y_APPLY_STEP(v,Q) Y_STEP_WRITE(v)
+#endif
+
+#if NUM_Z_STEPPER_DRIVERS == 4
+  #define Z_APPLY_DIR(v,Q) do{ \
+    Z_DIR_WRITE(v); Z2_DIR_WRITE((v) ^ ENABLED(INVERT_Z2_VS_Z_DIR)); \
+    Z3_DIR_WRITE((v) ^ ENABLED(INVERT_Z3_VS_Z_DIR)); Z4_DIR_WRITE((v) ^ ENABLED(INVERT_Z4_VS_Z_DIR)); \
+  }while(0)
+  #if ENABLED(Z_MULTI_ENDSTOPS)
+    #define Z_APPLY_STEP(v,Q) QUAD_ENDSTOP_APPLY_STEP(Z,v)
+  #elif ENABLED(Z_STEPPER_AUTO_ALIGN)
+    #define Z_APPLY_STEP(v,Q) QUAD_SEPARATE_APPLY_STEP(Z,v)
+  #else
+    #define Z_APPLY_STEP(v,Q) do{ Z_STEP_WRITE(v); Z2_STEP_WRITE(v); Z3_STEP_WRITE(v); Z4_STEP_WRITE(v); }while(0)
+  #endif
+#elif NUM_Z_STEPPER_DRIVERS == 3
+  #define Z_APPLY_DIR(v,Q) do{ \
+    Z_DIR_WRITE(v); Z2_DIR_WRITE((v) ^ ENABLED(INVERT_Z2_VS_Z_DIR)); Z3_DIR_WRITE((v) ^ ENABLED(INVERT_Z3_VS_Z_DIR)); \
+  }while(0)
+  #if ENABLED(Z_MULTI_ENDSTOPS)
+    #define Z_APPLY_STEP(v,Q) TRIPLE_ENDSTOP_APPLY_STEP(Z,v)
+  #elif ENABLED(Z_STEPPER_AUTO_ALIGN)
+    #define Z_APPLY_STEP(v,Q) TRIPLE_SEPARATE_APPLY_STEP(Z,v)
+  #else
+    #define Z_APPLY_STEP(v,Q) do{ Z_STEP_WRITE(v); Z2_STEP_WRITE(v); Z3_STEP_WRITE(v); }while(0)
+  #endif
+#elif NUM_Z_STEPPER_DRIVERS == 2
+  #define Z_APPLY_DIR(v,Q) do{ Z_DIR_WRITE(v); Z2_DIR_WRITE((v) ^ ENABLED(INVERT_Z2_VS_Z_DIR)); }while(0)
+  #if ENABLED(Z_MULTI_ENDSTOPS)
+    #define Z_APPLY_STEP(v,Q) DUAL_ENDSTOP_APPLY_STEP(Z,v)
+  #elif ENABLED(Z_STEPPER_AUTO_ALIGN)
+    #define Z_APPLY_STEP(v,Q) DUAL_SEPARATE_APPLY_STEP(Z,v)
+  #else
+    #define Z_APPLY_STEP(v,Q) do{ Z_STEP_WRITE(v); Z2_STEP_WRITE(v); }while(0)
+  #endif
+#else
+  #define Z_APPLY_DIR(v,Q) Z_DIR_WRITE(v)
+  #define Z_APPLY_STEP(v,Q) Z_STEP_WRITE(v)
+#endif
+
+#if DISABLED(MIXING_EXTRUDER)
+  #define E_APPLY_STEP(v,Q) E_STEP_WRITE(stepper_extruder, v)
+#endif
+
+#define CYCLES_TO_NS(CYC) (1000UL * (CYC) / ((F_CPU) / 1000000))
+#define NS_PER_PULSE_TIMER_TICK (1000000000UL / (STEPPER_TIMER_RATE))
+
+// Round up when converting from ns to timer ticks
+#define NS_TO_PULSE_TIMER_TICKS(NS) (((NS) + (NS_PER_PULSE_TIMER_TICK) / 2) / (NS_PER_PULSE_TIMER_TICK))
+
+#define TIMER_SETUP_NS (CYCLES_TO_NS(TIMER_READ_ADD_AND_STORE_CYCLES))
+
+#define PULSE_HIGH_TICK_COUNT hal_timer_t(NS_TO_PULSE_TIMER_TICKS(_MIN_PULSE_HIGH_NS - _MIN(_MIN_PULSE_HIGH_NS, TIMER_SETUP_NS)))
+#define PULSE_LOW_TICK_COUNT hal_timer_t(NS_TO_PULSE_TIMER_TICKS(_MIN_PULSE_LOW_NS - _MIN(_MIN_PULSE_LOW_NS, TIMER_SETUP_NS)))
+
+#define USING_TIMED_PULSE() hal_timer_t start_pulse_count = 0
+#define START_TIMED_PULSE(DIR) (start_pulse_count = HAL_timer_get_count(PULSE_TIMER_NUM))
+#define AWAIT_TIMED_PULSE(DIR) while (PULSE_##DIR##_TICK_COUNT > HAL_timer_get_count(PULSE_TIMER_NUM) - start_pulse_count) { }
+#define START_HIGH_PULSE()  START_TIMED_PULSE(HIGH)
+#define AWAIT_HIGH_PULSE()  AWAIT_TIMED_PULSE(HIGH)
+#define START_LOW_PULSE()   START_TIMED_PULSE(LOW)
+#define AWAIT_LOW_PULSE()   AWAIT_TIMED_PULSE(LOW)
+
+#if MINIMUM_STEPPER_PRE_DIR_DELAY > 0
+  #define DIR_WAIT_BEFORE() DELAY_NS(MINIMUM_STEPPER_PRE_DIR_DELAY)
+#else
+  #define DIR_WAIT_BEFORE()
+#endif
+
+#if MINIMUM_STEPPER_POST_DIR_DELAY > 0
+  #define DIR_WAIT_AFTER() DELAY_NS(MINIMUM_STEPPER_POST_DIR_DELAY)
+#else
+  #define DIR_WAIT_AFTER()
+#endif
+
+/**
+ * Set the stepper direction of each axis
+ *
+ *   COREXY: X_AXIS=A_AXIS and Y_AXIS=B_AXIS
+ *   COREXZ: X_AXIS=A_AXIS and Z_AXIS=C_AXIS
+ *   COREYZ: Y_AXIS=B_AXIS and Z_AXIS=C_AXIS
+ */
+void Stepper::set_directions() {
+
+  DIR_WAIT_BEFORE();
+
+  #define SET_STEP_DIR(A)                       \
+    if (motor_direction(_AXIS(A))) {            \
+      A##_APPLY_DIR(INVERT_##A##_DIR, false);   \
+      count_direction[_AXIS(A)] = -1;           \
+    }                                           \
+    else {                                      \
+      A##_APPLY_DIR(!INVERT_##A##_DIR, false);  \
+      count_direction[_AXIS(A)] = 1;            \
+    }
+
+  #if HAS_X_DIR
+    SET_STEP_DIR(X); // A
+  #endif
+  #if HAS_Y_DIR
+    SET_STEP_DIR(Y); // B
+  #endif
+  #if HAS_Z_DIR
+    SET_STEP_DIR(Z); // C
+  #endif
+
+  #if DISABLED(LIN_ADVANCE)
+    #if ENABLED(MIXING_EXTRUDER)
+       // Because this is valid for the whole block we don't know
+       // what e-steppers will step. Likely all. Set all.
+      if (motor_direction(E_AXIS)) {
+        MIXER_STEPPER_LOOP(j) REV_E_DIR(j);
+        count_direction.e = -1;
+      }
+      else {
+        MIXER_STEPPER_LOOP(j) NORM_E_DIR(j);
+        count_direction.e = 1;
+      }
+    #else
+      if (motor_direction(E_AXIS)) {
+        REV_E_DIR(stepper_extruder);
+        count_direction.e = -1;
+      }
+      else {
+        NORM_E_DIR(stepper_extruder);
+        count_direction.e = 1;
+      }
+    #endif
+  #endif // !LIN_ADVANCE
+
+  #if HAS_L64XX
+    if (L64XX_OK_to_power_up) { // OK to send the direction commands (which powers up the L64XX steppers)
+      if (L64xxManager.spi_active) {
+        L64xxManager.spi_abort = true;                    // Interrupted SPI transfer needs to shut down gracefully
+        for (uint8_t j = 1; j <= L64XX::chain[0]; j++)
+          L6470_buf[j] = dSPIN_NOP;                         // Fill buffer with NOOPs
+        L64xxManager.transfer(L6470_buf, L64XX::chain[0]);  // Send enough NOOPs to complete any command
+        L64xxManager.transfer(L6470_buf, L64XX::chain[0]);
+        L64xxManager.transfer(L6470_buf, L64XX::chain[0]);
+      }
+
+      // L64xxManager.dir_commands[] is an array that holds direction command for each stepper
+
+      // Scan command array, copy matches into L64xxManager.transfer
+      for (uint8_t j = 1; j <= L64XX::chain[0]; j++)
+        L6470_buf[j] = L64xxManager.dir_commands[L64XX::chain[j]];
+
+      L64xxManager.transfer(L6470_buf, L64XX::chain[0]);  // send the command stream to the drivers
+    }
+  #endif
+
+  DIR_WAIT_AFTER();
+}
+
+#if ENABLED(S_CURVE_ACCELERATION)
+  /**
+   *  This uses a quintic (fifth-degree) Bézier polynomial for the velocity curve, giving
+   *  a "linear pop" velocity curve; with pop being the sixth derivative of position:
+   *  velocity - 1st, acceleration - 2nd, jerk - 3rd, snap - 4th, crackle - 5th, pop - 6th
+   *
+   *  The Bézier curve takes the form:
+   *
+   *  V(t) = P_0 * B_0(t) + P_1 * B_1(t) + P_2 * B_2(t) + P_3 * B_3(t) + P_4 * B_4(t) + P_5 * B_5(t)
+   *
+   *  Where 0 <= t <= 1, and V(t) is the velocity. P_0 through P_5 are the control points, and B_0(t)
+   *  through B_5(t) are the Bernstein basis as follows:
+   *
+   *        B_0(t) =   (1-t)^5        =   -t^5 +  5t^4 - 10t^3 + 10t^2 -  5t   +   1
+   *        B_1(t) =  5(1-t)^4 * t    =   5t^5 - 20t^4 + 30t^3 - 20t^2 +  5t
+   *        B_2(t) = 10(1-t)^3 * t^2  = -10t^5 + 30t^4 - 30t^3 + 10t^2
+   *        B_3(t) = 10(1-t)^2 * t^3  =  10t^5 - 20t^4 + 10t^3
+   *        B_4(t) =  5(1-t)   * t^4  =  -5t^5 +  5t^4
+   *        B_5(t) =             t^5  =    t^5
+   *                                      ^       ^       ^       ^       ^       ^
+   *                                      |       |       |       |       |       |
+   *                                      A       B       C       D       E       F
+   *
+   *  Unfortunately, we cannot use forward-differencing to calculate each position through
+   *  the curve, as Marlin uses variable timer periods. So, we require a formula of the form:
+   *
+   *        V_f(t) = A*t^5 + B*t^4 + C*t^3 + D*t^2 + E*t + F
+   *
+   *  Looking at the above B_0(t) through B_5(t) expanded forms, if we take the coefficients of t^5
+   *  through t of the Bézier form of V(t), we can determine that:
+   *
+   *        A =    -P_0 +  5*P_1 - 10*P_2 + 10*P_3 -  5*P_4 +  P_5
+   *        B =   5*P_0 - 20*P_1 + 30*P_2 - 20*P_3 +  5*P_4
+   *        C = -10*P_0 + 30*P_1 - 30*P_2 + 10*P_3
+   *        D =  10*P_0 - 20*P_1 + 10*P_2
+   *        E = - 5*P_0 +  5*P_1
+   *        F =     P_0
+   *
+   *  Now, since we will (currently) *always* want the initial acceleration and jerk values to be 0,
+   *  We set P_i = P_0 = P_1 = P_2 (initial velocity), and P_t = P_3 = P_4 = P_5 (target velocity),
+   *  which, after simplification, resolves to:
+   *
+   *        A = - 6*P_i +  6*P_t =  6*(P_t - P_i)
+   *        B =  15*P_i - 15*P_t = 15*(P_i - P_t)
+   *        C = -10*P_i + 10*P_t = 10*(P_t - P_i)
+   *        D = 0
+   *        E = 0
+   *        F = P_i
+   *
+   *  As the t is evaluated in non uniform steps here, there is no other way rather than evaluating
+   *  the Bézier curve at each point:
+   *
+   *        V_f(t) = A*t^5 + B*t^4 + C*t^3 + F          [0 <= t <= 1]
+   *
+   * Floating point arithmetic execution time cost is prohibitive, so we will transform the math to
+   * use fixed point values to be able to evaluate it in realtime. Assuming a maximum of 250000 steps
+   * per second (driver pulses should at least be 2µS hi/2µS lo), and allocating 2 bits to avoid
+   * overflows on the evaluation of the Bézier curve, means we can use
+   *
+   *   t: unsigned Q0.32 (0 <= t < 1) |range 0 to 0xFFFFFFFF unsigned
+   *   A:   signed Q24.7 ,            |range = +/- 250000 * 6 * 128 = +/- 192000000 = 0x0B71B000 | 28 bits + sign
+   *   B:   signed Q24.7 ,            |range = +/- 250000 *15 * 128 = +/- 480000000 = 0x1C9C3800 | 29 bits + sign
+   *   C:   signed Q24.7 ,            |range = +/- 250000 *10 * 128 = +/- 320000000 = 0x1312D000 | 29 bits + sign
+   *   F:   signed Q24.7 ,            |range = +/- 250000     * 128 =      32000000 = 0x01E84800 | 25 bits + sign
+   *
+   * The trapezoid generator state contains the following information, that we will use to create and evaluate
+   * the Bézier curve:
+   *
+   *  blk->step_event_count [TS] = The total count of steps for this movement. (=distance)
+   *  blk->initial_rate     [VI] = The initial steps per second (=velocity)
+   *  blk->final_rate       [VF] = The ending steps per second  (=velocity)
+   *  and the count of events completed (step_events_completed) [CS] (=distance until now)
+   *
+   *  Note the abbreviations we use in the following formulae are between []s
+   *
+   *  For Any 32bit CPU:
+   *
+   *    At the start of each trapezoid, calculate the coefficients A,B,C,F and Advance [AV], as follows:
+   *
+   *      A =  6*128*(VF - VI) =  768*(VF - VI)
+   *      B = 15*128*(VI - VF) = 1920*(VI - VF)
+   *      C = 10*128*(VF - VI) = 1280*(VF - VI)
+   *      F =    128*VI        =  128*VI
+   *     AV = (1<<32)/TS      ~= 0xFFFFFFFF / TS (To use ARM UDIV, that is 32 bits) (this is computed at the planner, to offload expensive calculations from the ISR)
+   *
+   *    And for each point, evaluate the curve with the following sequence:
+   *
+   *      void lsrs(uint32_t& d, uint32_t s, int cnt) {
+   *        d = s >> cnt;
+   *      }
+   *      void lsls(uint32_t& d, uint32_t s, int cnt) {
+   *        d = s << cnt;
+   *      }
+   *      void lsrs(int32_t& d, uint32_t s, int cnt) {
+   *        d = uint32_t(s) >> cnt;
+   *      }
+   *      void lsls(int32_t& d, uint32_t s, int cnt) {
+   *        d = uint32_t(s) << cnt;
+   *      }
+   *      void umull(uint32_t& rlo, uint32_t& rhi, uint32_t op1, uint32_t op2) {
+   *        uint64_t res = uint64_t(op1) * op2;
+   *        rlo = uint32_t(res & 0xFFFFFFFF);
+   *        rhi = uint32_t((res >> 32) & 0xFFFFFFFF);
+   *      }
+   *      void smlal(int32_t& rlo, int32_t& rhi, int32_t op1, int32_t op2) {
+   *        int64_t mul = int64_t(op1) * op2;
+   *        int64_t s = int64_t(uint32_t(rlo) | ((uint64_t(uint32_t(rhi)) << 32U)));
+   *        mul += s;
+   *        rlo = int32_t(mul & 0xFFFFFFFF);
+   *        rhi = int32_t((mul >> 32) & 0xFFFFFFFF);
+   *      }
+   *      int32_t _eval_bezier_curve_arm(uint32_t curr_step) {
+   *        uint32_t flo = 0;
+   *        uint32_t fhi = bezier_AV * curr_step;
+   *        uint32_t t = fhi;
+   *        int32_t alo = bezier_F;
+   *        int32_t ahi = 0;
+   *        int32_t A = bezier_A;
+   *        int32_t B = bezier_B;
+   *        int32_t C = bezier_C;
+   *
+   *        lsrs(ahi, alo, 1);          // a  = F << 31
+   *        lsls(alo, alo, 31);         //
+   *        umull(flo, fhi, fhi, t);    // f *= t
+   *        umull(flo, fhi, fhi, t);    // f>>=32; f*=t
+   *        lsrs(flo, fhi, 1);          //
+   *        smlal(alo, ahi, flo, C);    // a+=(f>>33)*C
+   *        umull(flo, fhi, fhi, t);    // f>>=32; f*=t
+   *        lsrs(flo, fhi, 1);          //
+   *        smlal(alo, ahi, flo, B);    // a+=(f>>33)*B
+   *        umull(flo, fhi, fhi, t);    // f>>=32; f*=t
+   *        lsrs(flo, fhi, 1);          // f>>=33;
+   *        smlal(alo, ahi, flo, A);    // a+=(f>>33)*A;
+   *        lsrs(alo, ahi, 6);          // a>>=38
+   *
+   *        return alo;
+   *      }
+   *
+   *  This is rewritten in ARM assembly for optimal performance (43 cycles to execute).
+   *
+   *  For AVR, the precision of coefficients is scaled so the Bézier curve can be evaluated in real-time:
+   *  Let's reduce precision as much as possible. After some experimentation we found that:
+   *
+   *    Assume t and AV with 24 bits is enough
+   *       A =  6*(VF - VI)
+   *       B = 15*(VI - VF)
+   *       C = 10*(VF - VI)
+   *       F =     VI
+   *      AV = (1<<24)/TS   (this is computed at the planner, to offload expensive calculations from the ISR)
+   *
+   *    Instead of storing sign for each coefficient, we will store its absolute value,
+   *    and flag the sign of the A coefficient, so we can save to store the sign bit.
+   *    It always holds that sign(A) = - sign(B) = sign(C)
+   *
+   *     So, the resulting range of the coefficients are:
+   *
+   *       t: unsigned (0 <= t < 1) |range 0 to 0xFFFFFF unsigned
+   *       A:   signed Q24 , range = 250000 * 6 = 1500000 = 0x16E360 | 21 bits
+   *       B:   signed Q24 , range = 250000 *15 = 3750000 = 0x393870 | 22 bits
+   *       C:   signed Q24 , range = 250000 *10 = 2500000 = 0x1312D0 | 21 bits
+   *       F:   signed Q24 , range = 250000     =  250000 = 0x0ED090 | 20 bits
+   *
+   *    And for each curve, estimate its coefficients with:
+   *
+   *      void _calc_bezier_curve_coeffs(int32_t v0, int32_t v1, uint32_t av) {
+   *       // Calculate the Bézier coefficients
+   *       if (v1 < v0) {
+   *         A_negative = true;
+   *         bezier_A = 6 * (v0 - v1);
+   *         bezier_B = 15 * (v0 - v1);
+   *         bezier_C = 10 * (v0 - v1);
+   *       }
+   *       else {
+   *         A_negative = false;
+   *         bezier_A = 6 * (v1 - v0);
+   *         bezier_B = 15 * (v1 - v0);
+   *         bezier_C = 10 * (v1 - v0);
+   *       }
+   *       bezier_F = v0;
+   *      }
+   *
+   *    And for each point, evaluate the curve with the following sequence:
+   *
+   *      // unsigned multiplication of 24 bits x 24bits, return upper 16 bits
+   *      void umul24x24to16hi(uint16_t& r, uint24_t op1, uint24_t op2) {
+   *        r = (uint64_t(op1) * op2) >> 8;
+   *      }
+   *      // unsigned multiplication of 16 bits x 16bits, return upper 16 bits
+   *      void umul16x16to16hi(uint16_t& r, uint16_t op1, uint16_t op2) {
+   *        r = (uint32_t(op1) * op2) >> 16;
+   *      }
+   *      // unsigned multiplication of 16 bits x 24bits, return upper 24 bits
+   *      void umul16x24to24hi(uint24_t& r, uint16_t op1, uint24_t op2) {
+   *        r = uint24_t((uint64_t(op1) * op2) >> 16);
+   *      }
+   *
+   *      int32_t _eval_bezier_curve(uint32_t curr_step) {
+   *        // To save computing, the first step is always the initial speed
+   *        if (!curr_step)
+   *          return bezier_F;
+   *
+   *        uint16_t t;
+   *        umul24x24to16hi(t, bezier_AV, curr_step);   // t: Range 0 - 1^16 = 16 bits
+   *        uint16_t f = t;
+   *        umul16x16to16hi(f, f, t);                   // Range 16 bits (unsigned)
+   *        umul16x16to16hi(f, f, t);                   // Range 16 bits : f = t^3  (unsigned)
+   *        uint24_t acc = bezier_F;                    // Range 20 bits (unsigned)
+   *        if (A_negative) {
+   *          uint24_t v;
+   *          umul16x24to24hi(v, f, bezier_C);          // Range 21bits
+   *          acc -= v;
+   *          umul16x16to16hi(f, f, t);                 // Range 16 bits : f = t^4  (unsigned)
+   *          umul16x24to24hi(v, f, bezier_B);          // Range 22bits
+   *          acc += v;
+   *          umul16x16to16hi(f, f, t);                 // Range 16 bits : f = t^5  (unsigned)
+   *          umul16x24to24hi(v, f, bezier_A);          // Range 21bits + 15 = 36bits (plus sign)
+   *          acc -= v;
+   *        }
+   *        else {
+   *          uint24_t v;
+   *          umul16x24to24hi(v, f, bezier_C);          // Range 21bits
+   *          acc += v;
+   *          umul16x16to16hi(f, f, t);                 // Range 16 bits : f = t^4  (unsigned)
+   *          umul16x24to24hi(v, f, bezier_B);          // Range 22bits
+   *          acc -= v;
+   *          umul16x16to16hi(f, f, t);                 // Range 16 bits : f = t^5  (unsigned)
+   *          umul16x24to24hi(v, f, bezier_A);          // Range 21bits + 15 = 36bits (plus sign)
+   *          acc += v;
+   *        }
+   *        return acc;
+   *      }
+   *    These functions are translated to assembler for optimal performance.
+   *    Coefficient calculation takes 70 cycles. Bezier point evaluation takes 150 cycles.
+   */
+
+  #ifdef __AVR__
+
+    // For AVR we use assembly to maximize speed
+    void Stepper::_calc_bezier_curve_coeffs(const int32_t v0, const int32_t v1, const uint32_t av) {
+
+      // Store advance
+      bezier_AV = av;
+
+      // Calculate the rest of the coefficients
+      uint8_t r2 = v0 & 0xFF;
+      uint8_t r3 = (v0 >> 8) & 0xFF;
+      uint8_t r12 = (v0 >> 16) & 0xFF;
+      uint8_t r5 = v1 & 0xFF;
+      uint8_t r6 = (v1 >> 8) & 0xFF;
+      uint8_t r7 = (v1 >> 16) & 0xFF;
+      uint8_t r4,r8,r9,r10,r11;
+
+      __asm__ __volatile__(
+        /* Calculate the Bézier coefficients */
+        /*  %10:%1:%0 = v0*/
+        /*  %5:%4:%3 = v1*/
+        /*  %7:%6:%10 = temporary*/
+        /*  %9 = val (must be high register!)*/
+        /*  %10 (must be high register!)*/
+
+        /* Store initial velocity*/
+        A("sts bezier_F, %0")
+        A("sts bezier_F+1, %1")
+        A("sts bezier_F+2, %10")    /* bezier_F = %10:%1:%0 = v0 */
+
+        /* Get delta speed */
+        A("ldi %2,-1")              /* %2 = 0xFF, means A_negative = true */
+        A("clr %8")                 /* %8 = 0 */
+        A("sub %0,%3")
+        A("sbc %1,%4")
+        A("sbc %10,%5")             /*  v0 -= v1, C=1 if result is negative */
+        A("brcc 1f")                /* branch if result is positive (C=0), that means v0 >= v1 */
+
+        /*  Result was negative, get the absolute value*/
+        A("com %10")
+        A("com %1")
+        A("neg %0")
+        A("sbc %1,%2")
+        A("sbc %10,%2")             /* %10:%1:%0 +1  -> %10:%1:%0 = -(v0 - v1) = (v1 - v0) */
+        A("clr %2")                 /* %2 = 0, means A_negative = false */
+
+        /*  Store negative flag*/
+        L("1")
+        A("sts A_negative, %2")     /* Store negative flag */
+
+        /*  Compute coefficients A,B and C   [20 cycles worst case]*/
+        A("ldi %9,6")               /* %9 = 6 */
+        A("mul %0,%9")              /* r1:r0 = 6*LO(v0-v1) */
+        A("sts bezier_A, r0")
+        A("mov %6,r1")
+        A("clr %7")                 /* %7:%6:r0 = 6*LO(v0-v1) */
+        A("mul %1,%9")              /* r1:r0 = 6*MI(v0-v1) */
+        A("add %6,r0")
+        A("adc %7,r1")              /* %7:%6:?? += 6*MI(v0-v1) << 8 */
+        A("mul %10,%9")             /* r1:r0 = 6*HI(v0-v1) */
+        A("add %7,r0")              /* %7:%6:?? += 6*HI(v0-v1) << 16 */
+        A("sts bezier_A+1, %6")
+        A("sts bezier_A+2, %7")     /* bezier_A = %7:%6:?? = 6*(v0-v1) [35 cycles worst] */
+
+        A("ldi %9,15")              /* %9 = 15 */
+        A("mul %0,%9")              /* r1:r0 = 5*LO(v0-v1) */
+        A("sts bezier_B, r0")
+        A("mov %6,r1")
+        A("clr %7")                 /* %7:%6:?? = 5*LO(v0-v1) */
+        A("mul %1,%9")              /* r1:r0 = 5*MI(v0-v1) */
+        A("add %6,r0")
+        A("adc %7,r1")              /* %7:%6:?? += 5*MI(v0-v1) << 8 */
+        A("mul %10,%9")             /* r1:r0 = 5*HI(v0-v1) */
+        A("add %7,r0")              /* %7:%6:?? += 5*HI(v0-v1) << 16 */
+        A("sts bezier_B+1, %6")
+        A("sts bezier_B+2, %7")     /* bezier_B = %7:%6:?? = 5*(v0-v1) [50 cycles worst] */
+
+        A("ldi %9,10")              /* %9 = 10 */
+        A("mul %0,%9")              /* r1:r0 = 10*LO(v0-v1) */
+        A("sts bezier_C, r0")
+        A("mov %6,r1")
+        A("clr %7")                 /* %7:%6:?? = 10*LO(v0-v1) */
+        A("mul %1,%9")              /* r1:r0 = 10*MI(v0-v1) */
+        A("add %6,r0")
+        A("adc %7,r1")              /* %7:%6:?? += 10*MI(v0-v1) << 8 */
+        A("mul %10,%9")             /* r1:r0 = 10*HI(v0-v1) */
+        A("add %7,r0")              /* %7:%6:?? += 10*HI(v0-v1) << 16 */
+        A("sts bezier_C+1, %6")
+        " sts bezier_C+2, %7"       /* bezier_C = %7:%6:?? = 10*(v0-v1) [65 cycles worst] */
+        : "+r" (r2),
+          "+d" (r3),
+          "=r" (r4),
+          "+r" (r5),
+          "+r" (r6),
+          "+r" (r7),
+          "=r" (r8),
+          "=r" (r9),
+          "=r" (r10),
+          "=d" (r11),
+          "+r" (r12)
+        :
+        : "r0", "r1", "cc", "memory"
+      );
+    }
+
+    FORCE_INLINE int32_t Stepper::_eval_bezier_curve(const uint32_t curr_step) {
+
+      // If dealing with the first step, save expensive computing and return the initial speed
+      if (!curr_step)
+        return bezier_F;
+
+      uint8_t r0 = 0; /* Zero register */
+      uint8_t r2 = (curr_step) & 0xFF;
+      uint8_t r3 = (curr_step >> 8) & 0xFF;
+      uint8_t r4 = (curr_step >> 16) & 0xFF;
+      uint8_t r1,r5,r6,r7,r8,r9,r10,r11; /* Temporary registers */
+
+      __asm__ __volatile(
+        /* umul24x24to16hi(t, bezier_AV, curr_step);  t: Range 0 - 1^16 = 16 bits*/
+        A("lds %9,bezier_AV")       /* %9 = LO(AV)*/
+        A("mul %9,%2")              /* r1:r0 = LO(bezier_AV)*LO(curr_step)*/
+        A("mov %7,r1")              /* %7 = LO(bezier_AV)*LO(curr_step) >> 8*/
+        A("clr %8")                 /* %8:%7  = LO(bezier_AV)*LO(curr_step) >> 8*/
+        A("lds %10,bezier_AV+1")    /* %10 = MI(AV)*/
+        A("mul %10,%2")             /* r1:r0  = MI(bezier_AV)*LO(curr_step)*/
+        A("add %7,r0")
+        A("adc %8,r1")              /* %8:%7 += MI(bezier_AV)*LO(curr_step)*/
+        A("lds r1,bezier_AV+2")     /* r11 = HI(AV)*/
+        A("mul r1,%2")              /* r1:r0  = HI(bezier_AV)*LO(curr_step)*/
+        A("add %8,r0")              /* %8:%7 += HI(bezier_AV)*LO(curr_step) << 8*/
+        A("mul %9,%3")              /* r1:r0 =  LO(bezier_AV)*MI(curr_step)*/
+        A("add %7,r0")
+        A("adc %8,r1")              /* %8:%7 += LO(bezier_AV)*MI(curr_step)*/
+        A("mul %10,%3")             /* r1:r0 =  MI(bezier_AV)*MI(curr_step)*/
+        A("add %8,r0")              /* %8:%7 += LO(bezier_AV)*MI(curr_step) << 8*/
+        A("mul %9,%4")              /* r1:r0 =  LO(bezier_AV)*HI(curr_step)*/
+        A("add %8,r0")              /* %8:%7 += LO(bezier_AV)*HI(curr_step) << 8*/
+        /* %8:%7 = t*/
+
+        /* uint16_t f = t;*/
+        A("mov %5,%7")              /* %6:%5 = f*/
+        A("mov %6,%8")
+        /* %6:%5 = f*/
+
+        /* umul16x16to16hi(f, f, t); / Range 16 bits (unsigned) [17] */
+        A("mul %5,%7")              /* r1:r0 = LO(f) * LO(t)*/
+        A("mov %9,r1")              /* store MIL(LO(f) * LO(t)) in %9, we need it for rounding*/
+        A("clr %10")                /* %10 = 0*/
+        A("clr %11")                /* %11 = 0*/
+        A("mul %5,%8")              /* r1:r0 = LO(f) * HI(t)*/
+        A("add %9,r0")              /* %9 += LO(LO(f) * HI(t))*/
+        A("adc %10,r1")             /* %10 = HI(LO(f) * HI(t))*/
+        A("adc %11,%0")             /* %11 += carry*/
+        A("mul %6,%7")              /* r1:r0 = HI(f) * LO(t)*/
+        A("add %9,r0")              /* %9 += LO(HI(f) * LO(t))*/
+        A("adc %10,r1")             /* %10 += HI(HI(f) * LO(t)) */
+        A("adc %11,%0")             /* %11 += carry*/
+        A("mul %6,%8")              /* r1:r0 = HI(f) * HI(t)*/
+        A("add %10,r0")             /* %10 += LO(HI(f) * HI(t))*/
+        A("adc %11,r1")             /* %11 += HI(HI(f) * HI(t))*/
+        A("mov %5,%10")             /* %6:%5 = */
+        A("mov %6,%11")             /* f = %10:%11*/
+
+        /* umul16x16to16hi(f, f, t); / Range 16 bits : f = t^3  (unsigned) [17]*/
+        A("mul %5,%7")              /* r1:r0 = LO(f) * LO(t)*/
+        A("mov %1,r1")              /* store MIL(LO(f) * LO(t)) in %1, we need it for rounding*/
+        A("clr %10")                /* %10 = 0*/
+        A("clr %11")                /* %11 = 0*/
+        A("mul %5,%8")              /* r1:r0 = LO(f) * HI(t)*/
+        A("add %1,r0")              /* %1 += LO(LO(f) * HI(t))*/
+        A("adc %10,r1")             /* %10 = HI(LO(f) * HI(t))*/
+        A("adc %11,%0")             /* %11 += carry*/
+        A("mul %6,%7")              /* r1:r0 = HI(f) * LO(t)*/
+        A("add %1,r0")              /* %1 += LO(HI(f) * LO(t))*/
+        A("adc %10,r1")             /* %10 += HI(HI(f) * LO(t))*/
+        A("adc %11,%0")             /* %11 += carry*/
+        A("mul %6,%8")              /* r1:r0 = HI(f) * HI(t)*/
+        A("add %10,r0")             /* %10 += LO(HI(f) * HI(t))*/
+        A("adc %11,r1")             /* %11 += HI(HI(f) * HI(t))*/
+        A("mov %5,%10")             /* %6:%5 =*/
+        A("mov %6,%11")             /* f = %10:%11*/
+        /* [15 +17*2] = [49]*/
+
+        /* %4:%3:%2 will be acc from now on*/
+
+        /* uint24_t acc = bezier_F; / Range 20 bits (unsigned)*/
+        A("clr %9")                 /* "decimal place we get for free"*/
+        A("lds %2,bezier_F")
+        A("lds %3,bezier_F+1")
+        A("lds %4,bezier_F+2")      /* %4:%3:%2 = acc*/
+
+        /* if (A_negative) {*/
+        A("lds r0,A_negative")
+        A("or r0,%0")               /* Is flag signalling negative? */
+        A("brne 3f")                /* If yes, Skip next instruction if A was negative*/
+        A("rjmp 1f")                /* Otherwise, jump */
+
+        /* uint24_t v; */
+        /* umul16x24to24hi(v, f, bezier_C); / Range 21bits [29] */
+        /* acc -= v; */
+        L("3")
+        A("lds %10, bezier_C")      /* %10 = LO(bezier_C)*/
+        A("mul %10,%5")             /* r1:r0 = LO(bezier_C) * LO(f)*/
+        A("sub %9,r1")
+        A("sbc %2,%0")
+        A("sbc %3,%0")
+        A("sbc %4,%0")              /* %4:%3:%2:%9 -= HI(LO(bezier_C) * LO(f))*/
+        A("lds %11, bezier_C+1")    /* %11 = MI(bezier_C)*/
+        A("mul %11,%5")             /* r1:r0 = MI(bezier_C) * LO(f)*/
+        A("sub %9,r0")
+        A("sbc %2,r1")
+        A("sbc %3,%0")
+        A("sbc %4,%0")              /* %4:%3:%2:%9 -= MI(bezier_C) * LO(f)*/
+        A("lds %1, bezier_C+2")     /* %1 = HI(bezier_C)*/
+        A("mul %1,%5")              /* r1:r0 = MI(bezier_C) * LO(f)*/
+        A("sub %2,r0")
+        A("sbc %3,r1")
+        A("sbc %4,%0")              /* %4:%3:%2:%9 -= HI(bezier_C) * LO(f) << 8*/
+        A("mul %10,%6")             /* r1:r0 = LO(bezier_C) * MI(f)*/
+        A("sub %9,r0")
+        A("sbc %2,r1")
+        A("sbc %3,%0")
+        A("sbc %4,%0")              /* %4:%3:%2:%9 -= LO(bezier_C) * MI(f)*/
+        A("mul %11,%6")             /* r1:r0 = MI(bezier_C) * MI(f)*/
+        A("sub %2,r0")
+        A("sbc %3,r1")
+        A("sbc %4,%0")              /* %4:%3:%2:%9 -= MI(bezier_C) * MI(f) << 8*/
+        A("mul %1,%6")              /* r1:r0 = HI(bezier_C) * LO(f)*/
+        A("sub %3,r0")
+        A("sbc %4,r1")              /* %4:%3:%2:%9 -= HI(bezier_C) * LO(f) << 16*/
+
+        /* umul16x16to16hi(f, f, t); / Range 16 bits : f = t^3  (unsigned) [17]*/
+        A("mul %5,%7")              /* r1:r0 = LO(f) * LO(t)*/
+        A("mov %1,r1")              /* store MIL(LO(f) * LO(t)) in %1, we need it for rounding*/
+        A("clr %10")                /* %10 = 0*/
+        A("clr %11")                /* %11 = 0*/
+        A("mul %5,%8")              /* r1:r0 = LO(f) * HI(t)*/
+        A("add %1,r0")              /* %1 += LO(LO(f) * HI(t))*/
+        A("adc %10,r1")             /* %10 = HI(LO(f) * HI(t))*/
+        A("adc %11,%0")             /* %11 += carry*/
+        A("mul %6,%7")              /* r1:r0 = HI(f) * LO(t)*/
+        A("add %1,r0")              /* %1 += LO(HI(f) * LO(t))*/
+        A("adc %10,r1")             /* %10 += HI(HI(f) * LO(t))*/
+        A("adc %11,%0")             /* %11 += carry*/
+        A("mul %6,%8")              /* r1:r0 = HI(f) * HI(t)*/
+        A("add %10,r0")             /* %10 += LO(HI(f) * HI(t))*/
+        A("adc %11,r1")             /* %11 += HI(HI(f) * HI(t))*/
+        A("mov %5,%10")             /* %6:%5 =*/
+        A("mov %6,%11")             /* f = %10:%11*/
+
+        /* umul16x24to24hi(v, f, bezier_B); / Range 22bits [29]*/
+        /* acc += v; */
+        A("lds %10, bezier_B")      /* %10 = LO(bezier_B)*/
+        A("mul %10,%5")             /* r1:r0 = LO(bezier_B) * LO(f)*/
+        A("add %9,r1")
+        A("adc %2,%0")
+        A("adc %3,%0")
+        A("adc %4,%0")              /* %4:%3:%2:%9 += HI(LO(bezier_B) * LO(f))*/
+        A("lds %11, bezier_B+1")    /* %11 = MI(bezier_B)*/
+        A("mul %11,%5")             /* r1:r0 = MI(bezier_B) * LO(f)*/
+        A("add %9,r0")
+        A("adc %2,r1")
+        A("adc %3,%0")
+        A("adc %4,%0")              /* %4:%3:%2:%9 += MI(bezier_B) * LO(f)*/
+        A("lds %1, bezier_B+2")     /* %1 = HI(bezier_B)*/
+        A("mul %1,%5")              /* r1:r0 = MI(bezier_B) * LO(f)*/
+        A("add %2,r0")
+        A("adc %3,r1")
+        A("adc %4,%0")              /* %4:%3:%2:%9 += HI(bezier_B) * LO(f) << 8*/
+        A("mul %10,%6")             /* r1:r0 = LO(bezier_B) * MI(f)*/
+        A("add %9,r0")
+        A("adc %2,r1")
+        A("adc %3,%0")
+        A("adc %4,%0")              /* %4:%3:%2:%9 += LO(bezier_B) * MI(f)*/
+        A("mul %11,%6")             /* r1:r0 = MI(bezier_B) * MI(f)*/
+        A("add %2,r0")
+        A("adc %3,r1")
+        A("adc %4,%0")              /* %4:%3:%2:%9 += MI(bezier_B) * MI(f) << 8*/
+        A("mul %1,%6")              /* r1:r0 = HI(bezier_B) * LO(f)*/
+        A("add %3,r0")
+        A("adc %4,r1")              /* %4:%3:%2:%9 += HI(bezier_B) * LO(f) << 16*/
+
+        /* umul16x16to16hi(f, f, t); / Range 16 bits : f = t^5  (unsigned) [17]*/
+        A("mul %5,%7")              /* r1:r0 = LO(f) * LO(t)*/
+        A("mov %1,r1")              /* store MIL(LO(f) * LO(t)) in %1, we need it for rounding*/
+        A("clr %10")                /* %10 = 0*/
+        A("clr %11")                /* %11 = 0*/
+        A("mul %5,%8")              /* r1:r0 = LO(f) * HI(t)*/
+        A("add %1,r0")              /* %1 += LO(LO(f) * HI(t))*/
+        A("adc %10,r1")             /* %10 = HI(LO(f) * HI(t))*/
+        A("adc %11,%0")             /* %11 += carry*/
+        A("mul %6,%7")              /* r1:r0 = HI(f) * LO(t)*/
+        A("add %1,r0")              /* %1 += LO(HI(f) * LO(t))*/
+        A("adc %10,r1")             /* %10 += HI(HI(f) * LO(t))*/
+        A("adc %11,%0")             /* %11 += carry*/
+        A("mul %6,%8")              /* r1:r0 = HI(f) * HI(t)*/
+        A("add %10,r0")             /* %10 += LO(HI(f) * HI(t))*/
+        A("adc %11,r1")             /* %11 += HI(HI(f) * HI(t))*/
+        A("mov %5,%10")             /* %6:%5 =*/
+        A("mov %6,%11")             /* f = %10:%11*/
+
+        /* umul16x24to24hi(v, f, bezier_A); / Range 21bits [29]*/
+        /* acc -= v; */
+        A("lds %10, bezier_A")      /* %10 = LO(bezier_A)*/
+        A("mul %10,%5")             /* r1:r0 = LO(bezier_A) * LO(f)*/
+        A("sub %9,r1")
+        A("sbc %2,%0")
+        A("sbc %3,%0")
+        A("sbc %4,%0")              /* %4:%3:%2:%9 -= HI(LO(bezier_A) * LO(f))*/
+        A("lds %11, bezier_A+1")    /* %11 = MI(bezier_A)*/
+        A("mul %11,%5")             /* r1:r0 = MI(bezier_A) * LO(f)*/
+        A("sub %9,r0")
+        A("sbc %2,r1")
+        A("sbc %3,%0")
+        A("sbc %4,%0")              /* %4:%3:%2:%9 -= MI(bezier_A) * LO(f)*/
+        A("lds %1, bezier_A+2")     /* %1 = HI(bezier_A)*/
+        A("mul %1,%5")              /* r1:r0 = MI(bezier_A) * LO(f)*/
+        A("sub %2,r0")
+        A("sbc %3,r1")
+        A("sbc %4,%0")              /* %4:%3:%2:%9 -= HI(bezier_A) * LO(f) << 8*/
+        A("mul %10,%6")             /* r1:r0 = LO(bezier_A) * MI(f)*/
+        A("sub %9,r0")
+        A("sbc %2,r1")
+        A("sbc %3,%0")
+        A("sbc %4,%0")              /* %4:%3:%2:%9 -= LO(bezier_A) * MI(f)*/
+        A("mul %11,%6")             /* r1:r0 = MI(bezier_A) * MI(f)*/
+        A("sub %2,r0")
+        A("sbc %3,r1")
+        A("sbc %4,%0")              /* %4:%3:%2:%9 -= MI(bezier_A) * MI(f) << 8*/
+        A("mul %1,%6")              /* r1:r0 = HI(bezier_A) * LO(f)*/
+        A("sub %3,r0")
+        A("sbc %4,r1")              /* %4:%3:%2:%9 -= HI(bezier_A) * LO(f) << 16*/
+        A("jmp 2f")                 /* Done!*/
+
+        L("1")
+
+        /* uint24_t v; */
+        /* umul16x24to24hi(v, f, bezier_C); / Range 21bits [29]*/
+        /* acc += v; */
+        A("lds %10, bezier_C")      /* %10 = LO(bezier_C)*/
+        A("mul %10,%5")             /* r1:r0 = LO(bezier_C) * LO(f)*/
+        A("add %9,r1")
+        A("adc %2,%0")
+        A("adc %3,%0")
+        A("adc %4,%0")              /* %4:%3:%2:%9 += HI(LO(bezier_C) * LO(f))*/
+        A("lds %11, bezier_C+1")    /* %11 = MI(bezier_C)*/
+        A("mul %11,%5")             /* r1:r0 = MI(bezier_C) * LO(f)*/
+        A("add %9,r0")
+        A("adc %2,r1")
+        A("adc %3,%0")
+        A("adc %4,%0")              /* %4:%3:%2:%9 += MI(bezier_C) * LO(f)*/
+        A("lds %1, bezier_C+2")     /* %1 = HI(bezier_C)*/
+        A("mul %1,%5")              /* r1:r0 = MI(bezier_C) * LO(f)*/
+        A("add %2,r0")
+        A("adc %3,r1")
+        A("adc %4,%0")              /* %4:%3:%2:%9 += HI(bezier_C) * LO(f) << 8*/
+        A("mul %10,%6")             /* r1:r0 = LO(bezier_C) * MI(f)*/
+        A("add %9,r0")
+        A("adc %2,r1")
+        A("adc %3,%0")
+        A("adc %4,%0")              /* %4:%3:%2:%9 += LO(bezier_C) * MI(f)*/
+        A("mul %11,%6")             /* r1:r0 = MI(bezier_C) * MI(f)*/
+        A("add %2,r0")
+        A("adc %3,r1")
+        A("adc %4,%0")              /* %4:%3:%2:%9 += MI(bezier_C) * MI(f) << 8*/
+        A("mul %1,%6")              /* r1:r0 = HI(bezier_C) * LO(f)*/
+        A("add %3,r0")
+        A("adc %4,r1")              /* %4:%3:%2:%9 += HI(bezier_C) * LO(f) << 16*/
+
+        /* umul16x16to16hi(f, f, t); / Range 16 bits : f = t^3  (unsigned) [17]*/
+        A("mul %5,%7")              /* r1:r0 = LO(f) * LO(t)*/
+        A("mov %1,r1")              /* store MIL(LO(f) * LO(t)) in %1, we need it for rounding*/
+        A("clr %10")                /* %10 = 0*/
+        A("clr %11")                /* %11 = 0*/
+        A("mul %5,%8")              /* r1:r0 = LO(f) * HI(t)*/
+        A("add %1,r0")              /* %1 += LO(LO(f) * HI(t))*/
+        A("adc %10,r1")             /* %10 = HI(LO(f) * HI(t))*/
+        A("adc %11,%0")             /* %11 += carry*/
+        A("mul %6,%7")              /* r1:r0 = HI(f) * LO(t)*/
+        A("add %1,r0")              /* %1 += LO(HI(f) * LO(t))*/
+        A("adc %10,r1")             /* %10 += HI(HI(f) * LO(t))*/
+        A("adc %11,%0")             /* %11 += carry*/
+        A("mul %6,%8")              /* r1:r0 = HI(f) * HI(t)*/
+        A("add %10,r0")             /* %10 += LO(HI(f) * HI(t))*/
+        A("adc %11,r1")             /* %11 += HI(HI(f) * HI(t))*/
+        A("mov %5,%10")             /* %6:%5 =*/
+        A("mov %6,%11")             /* f = %10:%11*/
+
+        /* umul16x24to24hi(v, f, bezier_B); / Range 22bits [29]*/
+        /* acc -= v;*/
+        A("lds %10, bezier_B")      /* %10 = LO(bezier_B)*/
+        A("mul %10,%5")             /* r1:r0 = LO(bezier_B) * LO(f)*/
+        A("sub %9,r1")
+        A("sbc %2,%0")
+        A("sbc %3,%0")
+        A("sbc %4,%0")              /* %4:%3:%2:%9 -= HI(LO(bezier_B) * LO(f))*/
+        A("lds %11, bezier_B+1")    /* %11 = MI(bezier_B)*/
+        A("mul %11,%5")             /* r1:r0 = MI(bezier_B) * LO(f)*/
+        A("sub %9,r0")
+        A("sbc %2,r1")
+        A("sbc %3,%0")
+        A("sbc %4,%0")              /* %4:%3:%2:%9 -= MI(bezier_B) * LO(f)*/
+        A("lds %1, bezier_B+2")     /* %1 = HI(bezier_B)*/
+        A("mul %1,%5")              /* r1:r0 = MI(bezier_B) * LO(f)*/
+        A("sub %2,r0")
+        A("sbc %3,r1")
+        A("sbc %4,%0")              /* %4:%3:%2:%9 -= HI(bezier_B) * LO(f) << 8*/
+        A("mul %10,%6")             /* r1:r0 = LO(bezier_B) * MI(f)*/
+        A("sub %9,r0")
+        A("sbc %2,r1")
+        A("sbc %3,%0")
+        A("sbc %4,%0")              /* %4:%3:%2:%9 -= LO(bezier_B) * MI(f)*/
+        A("mul %11,%6")             /* r1:r0 = MI(bezier_B) * MI(f)*/
+        A("sub %2,r0")
+        A("sbc %3,r1")
+        A("sbc %4,%0")              /* %4:%3:%2:%9 -= MI(bezier_B) * MI(f) << 8*/
+        A("mul %1,%6")              /* r1:r0 = HI(bezier_B) * LO(f)*/
+        A("sub %3,r0")
+        A("sbc %4,r1")              /* %4:%3:%2:%9 -= HI(bezier_B) * LO(f) << 16*/
+
+        /* umul16x16to16hi(f, f, t); / Range 16 bits : f = t^5  (unsigned) [17]*/
+        A("mul %5,%7")              /* r1:r0 = LO(f) * LO(t)*/
+        A("mov %1,r1")              /* store MIL(LO(f) * LO(t)) in %1, we need it for rounding*/
+        A("clr %10")                /* %10 = 0*/
+        A("clr %11")                /* %11 = 0*/
+        A("mul %5,%8")              /* r1:r0 = LO(f) * HI(t)*/
+        A("add %1,r0")              /* %1 += LO(LO(f) * HI(t))*/
+        A("adc %10,r1")             /* %10 = HI(LO(f) * HI(t))*/
+        A("adc %11,%0")             /* %11 += carry*/
+        A("mul %6,%7")              /* r1:r0 = HI(f) * LO(t)*/
+        A("add %1,r0")              /* %1 += LO(HI(f) * LO(t))*/
+        A("adc %10,r1")             /* %10 += HI(HI(f) * LO(t))*/
+        A("adc %11,%0")             /* %11 += carry*/
+        A("mul %6,%8")              /* r1:r0 = HI(f) * HI(t)*/
+        A("add %10,r0")             /* %10 += LO(HI(f) * HI(t))*/
+        A("adc %11,r1")             /* %11 += HI(HI(f) * HI(t))*/
+        A("mov %5,%10")             /* %6:%5 =*/
+        A("mov %6,%11")             /* f = %10:%11*/
+
+        /* umul16x24to24hi(v, f, bezier_A); / Range 21bits [29]*/
+        /* acc += v; */
+        A("lds %10, bezier_A")      /* %10 = LO(bezier_A)*/
+        A("mul %10,%5")             /* r1:r0 = LO(bezier_A) * LO(f)*/
+        A("add %9,r1")
+        A("adc %2,%0")
+        A("adc %3,%0")
+        A("adc %4,%0")              /* %4:%3:%2:%9 += HI(LO(bezier_A) * LO(f))*/
+        A("lds %11, bezier_A+1")    /* %11 = MI(bezier_A)*/
+        A("mul %11,%5")             /* r1:r0 = MI(bezier_A) * LO(f)*/
+        A("add %9,r0")
+        A("adc %2,r1")
+        A("adc %3,%0")
+        A("adc %4,%0")              /* %4:%3:%2:%9 += MI(bezier_A) * LO(f)*/
+        A("lds %1, bezier_A+2")     /* %1 = HI(bezier_A)*/
+        A("mul %1,%5")              /* r1:r0 = MI(bezier_A) * LO(f)*/
+        A("add %2,r0")
+        A("adc %3,r1")
+        A("adc %4,%0")              /* %4:%3:%2:%9 += HI(bezier_A) * LO(f) << 8*/
+        A("mul %10,%6")             /* r1:r0 = LO(bezier_A) * MI(f)*/
+        A("add %9,r0")
+        A("adc %2,r1")
+        A("adc %3,%0")
+        A("adc %4,%0")              /* %4:%3:%2:%9 += LO(bezier_A) * MI(f)*/
+        A("mul %11,%6")             /* r1:r0 = MI(bezier_A) * MI(f)*/
+        A("add %2,r0")
+        A("adc %3,r1")
+        A("adc %4,%0")              /* %4:%3:%2:%9 += MI(bezier_A) * MI(f) << 8*/
+        A("mul %1,%6")              /* r1:r0 = HI(bezier_A) * LO(f)*/
+        A("add %3,r0")
+        A("adc %4,r1")              /* %4:%3:%2:%9 += HI(bezier_A) * LO(f) << 16*/
+        L("2")
+        " clr __zero_reg__"         /* C runtime expects r1 = __zero_reg__ = 0 */
+        : "+r"(r0),
+          "+r"(r1),
+          "+r"(r2),
+          "+r"(r3),
+          "+r"(r4),
+          "+r"(r5),
+          "+r"(r6),
+          "+r"(r7),
+          "+r"(r8),
+          "+r"(r9),
+          "+r"(r10),
+          "+r"(r11)
+        :
+        :"cc","r0","r1"
+      );
+      return (r2 | (uint16_t(r3) << 8)) | (uint32_t(r4) << 16);
+    }
+
+  #else
+
+    // For all the other 32bit CPUs
+    FORCE_INLINE void Stepper::_calc_bezier_curve_coeffs(const int32_t v0, const int32_t v1, const uint32_t av) {
+      // Calculate the Bézier coefficients
+      bezier_A =  768 * (v1 - v0);
+      bezier_B = 1920 * (v0 - v1);
+      bezier_C = 1280 * (v1 - v0);
+      bezier_F =  128 * v0;
+      bezier_AV = av;
+    }
+
+    FORCE_INLINE int32_t Stepper::_eval_bezier_curve(const uint32_t curr_step) {
+      #if defined(__arm__) || defined(__thumb__)
+
+        // For ARM Cortex M3/M4 CPUs, we have the optimized assembler version, that takes 43 cycles to execute
+        uint32_t flo = 0;
+        uint32_t fhi = bezier_AV * curr_step;
+        uint32_t t = fhi;
+        int32_t alo = bezier_F;
+        int32_t ahi = 0;
+        int32_t A = bezier_A;
+        int32_t B = bezier_B;
+        int32_t C = bezier_C;
+
+         __asm__ __volatile__(
+          ".syntax unified" "\n\t"              // is to prevent CM0,CM1 non-unified syntax
+          A("lsrs  %[ahi],%[alo],#1")           // a  = F << 31      1 cycles
+          A("lsls  %[alo],%[alo],#31")          //                   1 cycles
+          A("umull %[flo],%[fhi],%[fhi],%[t]")  // f *= t            5 cycles [fhi:flo=64bits]
+          A("umull %[flo],%[fhi],%[fhi],%[t]")  // f>>=32; f*=t      5 cycles [fhi:flo=64bits]
+          A("lsrs  %[flo],%[fhi],#1")           //                   1 cycles [31bits]
+          A("smlal %[alo],%[ahi],%[flo],%[C]")  // a+=(f>>33)*C;     5 cycles
+          A("umull %[flo],%[fhi],%[fhi],%[t]")  // f>>=32; f*=t      5 cycles [fhi:flo=64bits]
+          A("lsrs  %[flo],%[fhi],#1")           //                   1 cycles [31bits]
+          A("smlal %[alo],%[ahi],%[flo],%[B]")  // a+=(f>>33)*B;     5 cycles
+          A("umull %[flo],%[fhi],%[fhi],%[t]")  // f>>=32; f*=t      5 cycles [fhi:flo=64bits]
+          A("lsrs  %[flo],%[fhi],#1")           // f>>=33;           1 cycles [31bits]
+          A("smlal %[alo],%[ahi],%[flo],%[A]")  // a+=(f>>33)*A;     5 cycles
+          A("lsrs  %[alo],%[ahi],#6")           // a>>=38            1 cycles
+          : [alo]"+r"( alo ) ,
+            [flo]"+r"( flo ) ,
+            [fhi]"+r"( fhi ) ,
+            [ahi]"+r"( ahi ) ,
+            [A]"+r"( A ) ,  // <== Note: Even if A, B, C, and t registers are INPUT ONLY
+            [B]"+r"( B ) ,  //  GCC does bad optimizations on the code if we list them as
+            [C]"+r"( C ) ,  //  such, breaking this function. So, to avoid that problem,
+            [t]"+r"( t )    //  we list all registers as input-outputs.
+          :
+          : "cc"
+        );
+        return alo;
+
+      #else
+
+        // For non ARM targets, we provide a fallback implementation. Really doubt it
+        // will be useful, unless the processor is fast and 32bit
+
+        uint32_t t = bezier_AV * curr_step;               // t: Range 0 - 1^32 = 32 bits
+        uint64_t f = t;
+        f *= t;                                           // Range 32*2 = 64 bits (unsigned)
+        f >>= 32;                                         // Range 32 bits  (unsigned)
+        f *= t;                                           // Range 32*2 = 64 bits  (unsigned)
+        f >>= 32;                                         // Range 32 bits : f = t^3  (unsigned)
+        int64_t acc = (int64_t) bezier_F << 31;           // Range 63 bits (signed)
+        acc += ((uint32_t) f >> 1) * (int64_t) bezier_C;  // Range 29bits + 31 = 60bits (plus sign)
+        f *= t;                                           // Range 32*2 = 64 bits
+        f >>= 32;                                         // Range 32 bits : f = t^3  (unsigned)
+        acc += ((uint32_t) f >> 1) * (int64_t) bezier_B;  // Range 29bits + 31 = 60bits (plus sign)
+        f *= t;                                           // Range 32*2 = 64 bits
+        f >>= 32;                                         // Range 32 bits : f = t^3  (unsigned)
+        acc += ((uint32_t) f >> 1) * (int64_t) bezier_A;  // Range 28bits + 31 = 59bits (plus sign)
+        acc >>= (31 + 7);                                 // Range 24bits (plus sign)
+        return (int32_t) acc;
+
+      #endif
+    }
+  #endif
+#endif // S_CURVE_ACCELERATION
+
+/**
+ * Stepper Driver Interrupt
+ *
+ * Directly pulses the stepper motors at high frequency.
+ */
+
+HAL_STEP_TIMER_ISR() {
+  HAL_timer_isr_prologue(STEP_TIMER_NUM);
+
+  Stepper::isr();
+
+  HAL_timer_isr_epilogue(STEP_TIMER_NUM);
+}
+
+#ifdef CPU_32_BIT
+  #define STEP_MULTIPLY(A,B) MultiU32X24toH32(A, B)
+#else
+  #define STEP_MULTIPLY(A,B) MultiU24X32toH16(A, B)
+#endif
+
+#if ENABLED(MKS_TEST)
+  extern uint8_t mks_test_flag;
+#endif
+
+void Stepper::isr() {
+
+  static uint32_t nextMainISR = 0;  // Interval until the next main Stepper Pulse phase (0 = Now)
+
+  #ifndef __AVR__
+    // Disable interrupts, to avoid ISR preemption while we reprogram the period
+    // (AVR enters the ISR with global interrupts disabled, so no need to do it here)
+    DISABLE_ISRS();
+  #endif
+
+  // Program timer compare for the maximum period, so it does NOT
+  // flag an interrupt while this ISR is running - So changes from small
+  // periods to big periods are respected and the timer does not reset to 0
+  HAL_timer_set_compare(STEP_TIMER_NUM, hal_timer_t(HAL_TIMER_TYPE_MAX));
+
+  // Count of ticks for the next ISR
+  hal_timer_t next_isr_ticks = 0;
+
+  // Limit the amount of iterations
+  uint8_t max_loops = 10;
+
+  // We need this variable here to be able to use it in the following loop
+  hal_timer_t min_ticks;
+  do {
+    // Enable ISRs to reduce USART processing latency
+    ENABLE_ISRS();
+    #if ENABLED(MKS_TEST)
+      if(mks_test_flag == 0x1e) {
+        WRITE(X_STEP_PIN, HIGH);
+        WRITE(Y_STEP_PIN, HIGH);
+        WRITE(Z_STEP_PIN, HIGH);
+        WRITE(E0_STEP_PIN, HIGH);
+        #if !MB(MKS_ROBIN_E3P)
+          WRITE(E1_STEP_PIN, HIGH);
+        #endif
+        //WRITE(E2_STEP_PIN, HIGH);
+      }
+    #endif
+
+    if (!nextMainISR) pulse_phase_isr();                            // 0 = Do coordinated axes Stepper pulses
+
+    #if ENABLED(LIN_ADVANCE)
+      if (!nextAdvanceISR) nextAdvanceISR = advance_isr();          // 0 = Do Linear Advance E Stepper pulses
+    #endif
+
+    #if ENABLED(INTEGRATED_BABYSTEPPING)
+      const bool is_babystep = (nextBabystepISR == 0);              // 0 = Do Babystepping (XY)Z pulses
+      if (is_babystep) nextBabystepISR = babystepping_isr();
+    #endif
+
+    // ^== Time critical. NOTHING besides pulse generation should be above here!!!
+
+    if (!nextMainISR) nextMainISR = block_phase_isr();  // Manage acc/deceleration, get next block
+
+    #if ENABLED(INTEGRATED_BABYSTEPPING)
+      if (is_babystep)                                  // Avoid ANY stepping too soon after baby-stepping
+        NOLESS(nextMainISR, (BABYSTEP_TICKS) / 8);      // FULL STOP for 125µs after a baby-step
+
+      if (nextBabystepISR != BABYSTEP_NEVER)            // Avoid baby-stepping too close to axis Stepping
+        NOLESS(nextBabystepISR, nextMainISR / 2);       // TODO: Only look at axes enabled for baby-stepping
+    #endif
+
+    // Get the interval to the next ISR call
+    const uint32_t interval = _MIN(
+      nextMainISR                                       // Time until the next Pulse / Block phase
+      #if ENABLED(LIN_ADVANCE)
+        , nextAdvanceISR                                // Come back early for Linear Advance?
+      #endif
+      #if ENABLED(INTEGRATED_BABYSTEPPING)
+        , nextBabystepISR                               // Come back early for Babystepping?
+      #endif
+      , uint32_t(HAL_TIMER_TYPE_MAX)                    // Come back in a very long time
+    );
+
+    //
+    // Compute remaining time for each ISR phase
+    //     NEVER : The phase is idle
+    //      Zero : The phase will occur on the next ISR call
+    //  Non-zero : The phase will occur on a future ISR call
+    //
+
+    nextMainISR -= interval;
+
+    #if ENABLED(LIN_ADVANCE)
+      if (nextAdvanceISR != LA_ADV_NEVER) nextAdvanceISR -= interval;
+    #endif
+
+    #if ENABLED(INTEGRATED_BABYSTEPPING)
+      if (nextBabystepISR != BABYSTEP_NEVER) nextBabystepISR -= interval;
+    #endif
+
+    /**
+     * This needs to avoid a race-condition caused by interleaving
+     * of interrupts required by both the LA and Stepper algorithms.
+     *
+     * Assume the following tick times for stepper pulses:
+     *   Stepper ISR (S):  1 1000 2000 3000 4000
+     *   Linear Adv. (E): 10 1010 2010 3010 4010
+     *
+     * The current algorithm tries to interleave them, giving:
+     *  1:S 10:E 1000:S 1010:E 2000:S 2010:E 3000:S 3010:E 4000:S 4010:E
+     *
+     * Ideal timing would yield these delta periods:
+     *  1:S  9:E  990:S   10:E  990:S   10:E  990:S   10:E  990:S   10:E
+     *
+     * But, since each event must fire an ISR with a minimum duration, the
+     * minimum delta might be 900, so deltas under 900 get rounded up:
+     *  900:S d900:E d990:S d900:E d990:S d900:E d990:S d900:E d990:S d900:E
+     *
+     * It works, but divides the speed of all motors by half, leading to a sudden
+     * reduction to 1/2 speed! Such jumps in speed lead to lost steps (not even
+     * accounting for double/quad stepping, which makes it even worse).
+     */
+
+    // Compute the tick count for the next ISR
+    next_isr_ticks += interval;
+
+    /**
+     * The following section must be done with global interrupts disabled.
+     * We want nothing to interrupt it, as that could mess the calculations
+     * we do for the next value to program in the period register of the
+     * stepper timer and lead to skipped ISRs (if the value we happen to program
+     * is less than the current count due to something preempting between the
+     * read and the write of the new period value).
+     */
+    #if ENABLED(MKS_TEST)
+      if(mks_test_flag == 0x1e) {
+	     WRITE(X_STEP_PIN, LOW);
+	     WRITE(Y_STEP_PIN, LOW);
+	     WRITE(Z_STEP_PIN, LOW);
+	     WRITE(E0_STEP_PIN, LOW);
+       #if !MB(MKS_ROBIN_E3P)
+	      WRITE(E1_STEP_PIN, LOW);
+       #endif
+	     //WRITE(E2_STEP_PIN, LOW);
+      }
+    #endif
+    DISABLE_ISRS();
+
+    /**
+     * Get the current tick value + margin
+     * Assuming at least 6µs between calls to this ISR...
+     * On AVR the ISR epilogue+prologue is estimated at 100 instructions - Give 8µs as margin
+     * On ARM the ISR epilogue+prologue is estimated at 20 instructions - Give 1µs as margin
+     */
+    min_ticks = HAL_timer_get_count(STEP_TIMER_NUM) + hal_timer_t(
+      #ifdef __AVR__
+        8
+      #else
+        1
+      #endif
+      * (STEPPER_TIMER_TICKS_PER_US)
+    );
+
+    /**
+     * NB: If for some reason the stepper monopolizes the MPU, eventually the
+     * timer will wrap around (and so will 'next_isr_ticks'). So, limit the
+     * loop to 10 iterations. Beyond that, there's no way to ensure correct pulse
+     * timing, since the MCU isn't fast enough.
+     */
+    if (!--max_loops) next_isr_ticks = min_ticks;
+
+    // Advance pulses if not enough time to wait for the next ISR
+  } while (next_isr_ticks < min_ticks);
+
+  // Now 'next_isr_ticks' contains the period to the next Stepper ISR - And we are
+  // sure that the time has not arrived yet - Warrantied by the scheduler
+
+  // Set the next ISR to fire at the proper time
+  HAL_timer_set_compare(STEP_TIMER_NUM, hal_timer_t(next_isr_ticks));
+
+  // Don't forget to finally reenable interrupts
+  ENABLE_ISRS();
+}
+
+#if MINIMUM_STEPPER_PULSE || MAXIMUM_STEPPER_RATE
+  #define ISR_PULSE_CONTROL 1
+#endif
+#if ISR_PULSE_CONTROL && DISABLED(I2S_STEPPER_STREAM)
+  #define ISR_MULTI_STEPS 1
+#endif
+
+/**
+ * This phase of the ISR should ONLY create the pulses for the steppers.
+ * This prevents jitter caused by the interval between the start of the
+ * interrupt and the start of the pulses. DON'T add any logic ahead of the
+ * call to this method that might cause variation in the timing. The aim
+ * is to keep pulse timing as regular as possible.
+ */
+void Stepper::pulse_phase_isr() {
+
+  // If we must abort the current block, do so!
+  if (abort_current_block) {
+    abort_current_block = false;
+    if (current_block) discard_current_block();
+  }
+
+  // If there is no current block, do nothing
+  if (!current_block) return;
+
+  // Count of pending loops and events for this iteration
+  const uint32_t pending_events = step_event_count - step_events_completed;
+  uint8_t events_to_do = _MIN(pending_events, steps_per_isr);
+
+  // Just update the value we will get at the end of the loop
+  step_events_completed += events_to_do;
+
+  // Take multiple steps per interrupt (For high speed moves)
+  #if ISR_MULTI_STEPS
+    bool firstStep = true;
+    USING_TIMED_PULSE();
+  #endif
+  xyze_bool_t step_needed{0};
+
+  do {
+    #define _APPLY_STEP(AXIS, INV, ALWAYS) AXIS ##_APPLY_STEP(INV, ALWAYS)
+    #define _INVERT_STEP_PIN(AXIS) INVERT_## AXIS ##_STEP_PIN
+
+    // Determine if a pulse is needed using Bresenham
+    #define PULSE_PREP(AXIS) do{ \
+      delta_error[_AXIS(AXIS)] += advance_dividend[_AXIS(AXIS)]; \
+      step_needed[_AXIS(AXIS)] = (delta_error[_AXIS(AXIS)] >= 0); \
+      if (step_needed[_AXIS(AXIS)]) { \
+        count_position[_AXIS(AXIS)] += count_direction[_AXIS(AXIS)]; \
+        delta_error[_AXIS(AXIS)] -= advance_divisor; \
+      } \
+    }while(0)
+
+    // Start an active pulse if needed
+    #define PULSE_START(AXIS) do{ \
+      if (step_needed[_AXIS(AXIS)]) { \
+        _APPLY_STEP(AXIS, !_INVERT_STEP_PIN(AXIS), 0); \
+      } \
+    }while(0)
+
+    // Stop an active pulse if needed
+    #define PULSE_STOP(AXIS) do { \
+      if (step_needed[_AXIS(AXIS)]) { \
+        _APPLY_STEP(AXIS, _INVERT_STEP_PIN(AXIS), 0); \
+      } \
+    }while(0)
+
+    // Direct Stepping page?
+    const bool is_page = IS_PAGE(current_block);
+
+    #if ENABLED(DIRECT_STEPPING)
+
+      if (is_page) {
+
+        #if STEPPER_PAGE_FORMAT == SP_4x4D_128
+
+          #define PAGE_SEGMENT_UPDATE(AXIS, VALUE) do{   \
+                 if ((VALUE) <  7) SBI(dm, _AXIS(AXIS)); \
+            else if ((VALUE) >  7) CBI(dm, _AXIS(AXIS)); \
+            page_step_state.sd[_AXIS(AXIS)] = VALUE;     \
+            page_step_state.bd[_AXIS(AXIS)] += VALUE;    \
+          }while(0)
+
+          #define PAGE_PULSE_PREP(AXIS) do{ \
+            step_needed[_AXIS(AXIS)] =      \
+              pgm_read_byte(&segment_table[page_step_state.sd[_AXIS(AXIS)]][page_step_state.segment_steps & 0x7]); \
+          }while(0)
+
+          switch (page_step_state.segment_steps) {
+            case DirectStepping::Config::SEGMENT_STEPS:
+              page_step_state.segment_idx += 2;
+              page_step_state.segment_steps = 0;
+              // fallthru
+            case 0: {
+              const uint8_t low = page_step_state.page[page_step_state.segment_idx],
+                           high = page_step_state.page[page_step_state.segment_idx + 1];
+              uint8_t dm = last_direction_bits;
+
+              PAGE_SEGMENT_UPDATE(X, low >> 4);
+              PAGE_SEGMENT_UPDATE(Y, low & 0xF);
+              PAGE_SEGMENT_UPDATE(Z, high >> 4);
+              PAGE_SEGMENT_UPDATE(E, high & 0xF);
+
+              if (dm != last_direction_bits)
+                set_directions(dm);
+
+            } break;
+
+            default: break;
+          }
+
+          PAGE_PULSE_PREP(X);
+          PAGE_PULSE_PREP(Y);
+          PAGE_PULSE_PREP(Z);
+          PAGE_PULSE_PREP(E);
+
+          page_step_state.segment_steps++;
+
+        #elif STEPPER_PAGE_FORMAT == SP_4x2_256
+
+          #define PAGE_SEGMENT_UPDATE(AXIS, VALUE) \
+            page_step_state.sd[_AXIS(AXIS)] = VALUE; \
+            page_step_state.bd[_AXIS(AXIS)] += VALUE;
+
+          #define PAGE_PULSE_PREP(AXIS) do{ \
+            step_needed[_AXIS(AXIS)] =      \
+              pgm_read_byte(&segment_table[page_step_state.sd[_AXIS(AXIS)]][page_step_state.segment_steps & 0x3]); \
+          }while(0)
+
+          switch (page_step_state.segment_steps) {
+            case DirectStepping::Config::SEGMENT_STEPS:
+              page_step_state.segment_idx++;
+              page_step_state.segment_steps = 0;
+              // fallthru
+            case 0: {
+              const uint8_t b = page_step_state.page[page_step_state.segment_idx];
+              PAGE_SEGMENT_UPDATE(X, (b >> 6) & 0x3);
+              PAGE_SEGMENT_UPDATE(Y, (b >> 4) & 0x3);
+              PAGE_SEGMENT_UPDATE(Z, (b >> 2) & 0x3);
+              PAGE_SEGMENT_UPDATE(E, (b >> 0) & 0x3);
+            } break;
+            default: break;
+          }
+
+          PAGE_PULSE_PREP(X);
+          PAGE_PULSE_PREP(Y);
+          PAGE_PULSE_PREP(Z);
+          PAGE_PULSE_PREP(E);
+
+          page_step_state.segment_steps++;
+
+        #elif STEPPER_PAGE_FORMAT == SP_4x1_512
+
+          #define PAGE_PULSE_PREP(AXIS, BITS) do{             \
+            step_needed[_AXIS(AXIS)] = (steps >> BITS) & 0x1; \
+            if (step_needed[_AXIS(AXIS)])                     \
+              page_step_state.bd[_AXIS(AXIS)]++;              \
+          }while(0)
+
+          uint8_t steps = page_step_state.page[page_step_state.segment_idx >> 1];
+          if (page_step_state.segment_idx & 0x1) steps >>= 4;
+
+          PAGE_PULSE_PREP(X, 3);
+          PAGE_PULSE_PREP(Y, 2);
+          PAGE_PULSE_PREP(Z, 1);
+          PAGE_PULSE_PREP(E, 0);
+
+          page_step_state.segment_idx++;
+
+        #else
+          #error "Unknown direct stepping page format!"
+        #endif
+      }
+
+    #endif // DIRECT_STEPPING
+
+    if (!is_page) {
+      // Determine if pulses are needed
+      #if HAS_X_STEP
+        PULSE_PREP(X);
+      #endif
+      #if HAS_Y_STEP
+        PULSE_PREP(Y);
+      #endif
+      #if HAS_Z_STEP
+        PULSE_PREP(Z);
+      #endif
+
+      #if EITHER(LIN_ADVANCE, MIXING_EXTRUDER)
+        delta_error.e += advance_dividend.e;
+        if (delta_error.e >= 0) {
+          count_position.e += count_direction.e;
+          #if ENABLED(LIN_ADVANCE)
+            delta_error.e -= advance_divisor;
+            // Don't step E here - But remember the number of steps to perform
+            motor_direction(E_AXIS) ? --LA_steps : ++LA_steps;
+          #else
+            step_needed.e = true;
+          #endif
+        }
+      #elif HAS_E0_STEP
+        PULSE_PREP(E);
+      #endif
+    }
+
+    #if ISR_MULTI_STEPS
+      if (firstStep)
+        firstStep = false;
+      else
+        AWAIT_LOW_PULSE();
+    #endif
+
+    // Pulse start
+    #if HAS_X_STEP
+      PULSE_START(X);
+    #endif
+    #if HAS_Y_STEP
+      PULSE_START(Y);
+    #endif
+    #if HAS_Z_STEP
+      PULSE_START(Z);
+    #endif
+
+    #if DISABLED(LIN_ADVANCE)
+      #if ENABLED(MIXING_EXTRUDER)
+        if (step_needed.e) E_STEP_WRITE(mixer.get_next_stepper(), !INVERT_E_STEP_PIN);
+      #elif HAS_E0_STEP
+        PULSE_START(E);
+      #endif
+    #endif
+
+    #if ENABLED(I2S_STEPPER_STREAM)
+      i2s_push_sample();
+    #endif
+
+    // TODO: need to deal with MINIMUM_STEPPER_PULSE over i2s
+    #if ISR_MULTI_STEPS
+      START_HIGH_PULSE();
+      AWAIT_HIGH_PULSE();
+    #endif
+
+    // Pulse stop
+    #if HAS_X_STEP
+      PULSE_STOP(X);
+    #endif
+    #if HAS_Y_STEP
+      PULSE_STOP(Y);
+    #endif
+    #if HAS_Z_STEP
+      PULSE_STOP(Z);
+    #endif
+
+    #if DISABLED(LIN_ADVANCE)
+      #if ENABLED(MIXING_EXTRUDER)
+        if (delta_error.e >= 0) {
+          delta_error.e -= advance_divisor;
+          E_STEP_WRITE(mixer.get_stepper(), INVERT_E_STEP_PIN);
+        }
+      #elif HAS_E0_STEP
+        PULSE_STOP(E);
+      #endif
+    #endif
+
+    #if ISR_MULTI_STEPS
+      if (events_to_do) START_LOW_PULSE();
+    #endif
+
+  } while (--events_to_do);
+}
+
+// This is the last half of the stepper interrupt: This one processes and
+// properly schedules blocks from the planner. This is executed after creating
+// the step pulses, so it is not time critical, as pulses are already done.
+
+uint32_t Stepper::block_phase_isr() {
+
+  // If no queued movements, just wait 1ms for the next block
+  uint32_t interval = (STEPPER_TIMER_RATE) / 1000UL;
+
+  // If there is a current block
+  if (current_block) {
+
+    // If current block is finished, reset pointer and finalize state
+    if (step_events_completed >= step_event_count) {
+      #if ENABLED(DIRECT_STEPPING)
+        #if STEPPER_PAGE_FORMAT == SP_4x4D_128
+          #define PAGE_SEGMENT_UPDATE_POS(AXIS) \
+            count_position[_AXIS(AXIS)] += page_step_state.bd[_AXIS(AXIS)] - 128 * 7;
+        #elif STEPPER_PAGE_FORMAT == SP_4x1_512 || STEPPER_PAGE_FORMAT == SP_4x2_256
+          #define PAGE_SEGMENT_UPDATE_POS(AXIS) \
+            count_position[_AXIS(AXIS)] += page_step_state.bd[_AXIS(AXIS)] * count_direction[_AXIS(AXIS)];
+        #endif
+
+        if (IS_PAGE(current_block)) {
+          PAGE_SEGMENT_UPDATE_POS(X);
+          PAGE_SEGMENT_UPDATE_POS(Y);
+          PAGE_SEGMENT_UPDATE_POS(Z);
+          PAGE_SEGMENT_UPDATE_POS(E);
+        }
+      #endif
+      TERN_(HAS_FILAMENT_RUNOUT_DISTANCE, runout.block_completed(current_block));
+      discard_current_block();
+    }
+    else {
+      // Step events not completed yet...
+
+      // Are we in acceleration phase ?
+      if (step_events_completed <= accelerate_until) { // Calculate new timer value
+
+        #if ENABLED(S_CURVE_ACCELERATION)
+          // Get the next speed to use (Jerk limited!)
+          uint32_t acc_step_rate = acceleration_time < current_block->acceleration_time
+                                   ? _eval_bezier_curve(acceleration_time)
+                                   : current_block->cruise_rate;
+        #else
+          acc_step_rate = STEP_MULTIPLY(acceleration_time, current_block->acceleration_rate) + current_block->initial_rate;
+          NOMORE(acc_step_rate, current_block->nominal_rate);
+        #endif
+
+        // acc_step_rate is in steps/second
+
+        // step_rate to timer interval and steps per stepper isr
+        interval = calc_timer_interval(acc_step_rate, &steps_per_isr);
+        acceleration_time += interval;
+
+        #if ENABLED(LIN_ADVANCE)
+          if (LA_use_advance_lead) {
+            // Fire ISR if final adv_rate is reached
+            if (LA_steps && LA_isr_rate != current_block->advance_speed) nextAdvanceISR = 0;
+          }
+          else if (LA_steps) nextAdvanceISR = 0;
+        #endif
+
+        // Update laser - Accelerating
+        #if ENABLED(LASER_POWER_INLINE_TRAPEZOID)
+          if (laser_trap.enabled) {
+            #if DISABLED(LASER_POWER_INLINE_TRAPEZOID_CONT)
+              if (current_block->laser.entry_per) {
+                laser_trap.acc_step_count -= step_events_completed - laser_trap.last_step_count;
+                laser_trap.last_step_count = step_events_completed;
+
+                // Should be faster than a divide, since this should trip just once
+                if (laser_trap.acc_step_count < 0) {
+                  while (laser_trap.acc_step_count < 0) {
+                    laser_trap.acc_step_count += current_block->laser.entry_per;
+                    if (laser_trap.cur_power < current_block->laser.power) laser_trap.cur_power++;
+                  }
+                  cutter.set_ocr_power(laser_trap.cur_power);
+                }
+              }
+            #else
+              if (laser_trap.till_update)
+                laser_trap.till_update--;
+              else {
+                laser_trap.till_update = LASER_POWER_INLINE_TRAPEZOID_CONT_PER;
+                laser_trap.cur_power = (current_block->laser.power * acc_step_rate) / current_block->nominal_rate;
+                cutter.set_ocr_power(laser_trap.cur_power); // Cycle efficiency is irrelevant it the last line was many cycles
+              }
+            #endif
+          }
+        #endif
+      }
+      // Are we in Deceleration phase ?
+      else if (step_events_completed > decelerate_after) {
+        uint32_t step_rate;
+
+        #if ENABLED(S_CURVE_ACCELERATION)
+          // If this is the 1st time we process the 2nd half of the trapezoid...
+          if (!bezier_2nd_half) {
+            // Initialize the Bézier speed curve
+            _calc_bezier_curve_coeffs(current_block->cruise_rate, current_block->final_rate, current_block->deceleration_time_inverse);
+            bezier_2nd_half = true;
+            // The first point starts at cruise rate. Just save evaluation of the Bézier curve
+            step_rate = current_block->cruise_rate;
+          }
+          else {
+            // Calculate the next speed to use
+            step_rate = deceleration_time < current_block->deceleration_time
+              ? _eval_bezier_curve(deceleration_time)
+              : current_block->final_rate;
+          }
+        #else
+
+          // Using the old trapezoidal control
+          step_rate = STEP_MULTIPLY(deceleration_time, current_block->acceleration_rate);
+          if (step_rate < acc_step_rate) { // Still decelerating?
+            step_rate = acc_step_rate - step_rate;
+            NOLESS(step_rate, current_block->final_rate);
+          }
+          else
+            step_rate = current_block->final_rate;
+        #endif
+
+        // step_rate is in steps/second
+
+        // step_rate to timer interval and steps per stepper isr
+        interval = calc_timer_interval(step_rate, &steps_per_isr);
+        deceleration_time += interval;
+
+        #if ENABLED(LIN_ADVANCE)
+          if (LA_use_advance_lead) {
+            // Wake up eISR on first deceleration loop and fire ISR if final adv_rate is reached
+            if (step_events_completed <= decelerate_after + steps_per_isr || (LA_steps && LA_isr_rate != current_block->advance_speed)) {
+              initiateLA();
+              LA_isr_rate = current_block->advance_speed;
+            }
+          }
+          else if (LA_steps) nextAdvanceISR = 0;
+        #endif // LIN_ADVANCE
+
+        // Update laser - Decelerating
+        #if ENABLED(LASER_POWER_INLINE_TRAPEZOID)
+          if (laser_trap.enabled) {
+            #if DISABLED(LASER_POWER_INLINE_TRAPEZOID_CONT)
+              if (current_block->laser.exit_per) {
+                laser_trap.acc_step_count -= step_events_completed - laser_trap.last_step_count;
+                laser_trap.last_step_count = step_events_completed;
+
+                // Should be faster than a divide, since this should trip just once
+                if (laser_trap.acc_step_count < 0) {
+                  while (laser_trap.acc_step_count < 0) {
+                    laser_trap.acc_step_count += current_block->laser.exit_per;
+                    if (laser_trap.cur_power > current_block->laser.power_exit) laser_trap.cur_power--;
+                  }
+                  cutter.set_ocr_power(laser_trap.cur_power);
+                }
+              }
+            #else
+              if (laser_trap.till_update)
+                laser_trap.till_update--;
+              else {
+                laser_trap.till_update = LASER_POWER_INLINE_TRAPEZOID_CONT_PER;
+                laser_trap.cur_power = (current_block->laser.power * step_rate) / current_block->nominal_rate;
+                cutter.set_ocr_power(laser_trap.cur_power); // Cycle efficiency isn't relevant when the last line was many cycles
+              }
+            #endif
+          }
+        #endif
+      }
+      // Must be in cruise phase otherwise
+      else {
+
+        #if ENABLED(LIN_ADVANCE)
+          // If there are any esteps, fire the next advance_isr "now"
+          if (LA_steps && LA_isr_rate != current_block->advance_speed) initiateLA();
+        #endif
+
+        // Calculate the ticks_nominal for this nominal speed, if not done yet
+        if (ticks_nominal < 0) {
+          // step_rate to timer interval and loops for the nominal speed
+          ticks_nominal = calc_timer_interval(current_block->nominal_rate, &steps_per_isr);
+        }
+
+        // The timer interval is just the nominal value for the nominal speed
+        interval = ticks_nominal;
+
+        // Update laser - Cruising
+        #if ENABLED(LASER_POWER_INLINE_TRAPEZOID)
+          if (laser_trap.enabled) {
+            if (!laser_trap.cruise_set) {
+              laser_trap.cur_power = current_block->laser.power;
+              cutter.set_ocr_power(laser_trap.cur_power);
+              laser_trap.cruise_set = true;
+            }
+            #if ENABLED(LASER_POWER_INLINE_TRAPEZOID_CONT)
+              laser_trap.till_update = LASER_POWER_INLINE_TRAPEZOID_CONT_PER;
+            #else
+              laser_trap.last_step_count = step_events_completed;
+            #endif
+          }
+        #endif
+      }
+    }
+  }
+
+  // If there is no current block at this point, attempt to pop one from the buffer
+  // and prepare its movement
+  if (!current_block) {
+
+    // Anything in the buffer?
+    if ((current_block = planner.get_current_block())) {
+
+      // Sync block? Sync the stepper counts and return
+      while (TEST(current_block->flag, BLOCK_BIT_SYNC_POSITION)) {
+        _set_position(current_block->position);
+        discard_current_block();
+
+        // Try to get a new block
+        if (!(current_block = planner.get_current_block()))
+          return interval; // No more queued movements!
+      }
+
+      // For non-inline cutter, grossly apply power
+      #if ENABLED(LASER_FEATURE) && DISABLED(LASER_POWER_INLINE)
+        cutter.apply_power(current_block->cutter_power);
+      #endif
+
+      TERN_(POWER_LOSS_RECOVERY, recovery.info.sdpos = current_block->sdpos);
+
+      #if ENABLED(DIRECT_STEPPING)
+        if (IS_PAGE(current_block)) {
+          page_step_state.segment_steps = 0;
+          page_step_state.segment_idx = 0;
+          page_step_state.page = page_manager.get_page(current_block->page_idx);
+          page_step_state.bd.reset();
+
+          if (DirectStepping::Config::DIRECTIONAL)
+            current_block->direction_bits = last_direction_bits;
+
+          if (!page_step_state.page) {
+            discard_current_block();
+            return interval;
+          }
+        }
+      #endif
+
+      // Flag all moving axes for proper endstop handling
+
+      #if IS_CORE
+        // Define conditions for checking endstops
+        #define S_(N) current_block->steps[CORE_AXIS_##N]
+        #define D_(N) TEST(current_block->direction_bits, CORE_AXIS_##N)
+      #endif
+
+      #if CORE_IS_XY || CORE_IS_XZ
+        /**
+         * Head direction in -X axis for CoreXY and CoreXZ bots.
+         *
+         * If steps differ, both axes are moving.
+         * If DeltaA == -DeltaB, the movement is only in the 2nd axis (Y or Z, handled below)
+         * If DeltaA ==  DeltaB, the movement is only in the 1st axis (X)
+         */
+        #if EITHER(COREXY, COREXZ)
+          #define X_CMP(A,B) ((A)==(B))
+        #else
+          #define X_CMP(A,B) ((A)!=(B))
+        #endif
+        #define X_MOVE_TEST ( S_(1) != S_(2) || (S_(1) > 0 && X_CMP(D_(1),D_(2))) )
+      #elif ENABLED(MARKFORGED_XY)
+        #define X_MOVE_TEST (current_block->steps.a != current_block->steps.b)
+      #else
+        #define X_MOVE_TEST !!current_block->steps.a
+      #endif
+
+      #if CORE_IS_XY || CORE_IS_YZ
+        /**
+         * Head direction in -Y axis for CoreXY / CoreYZ bots.
+         *
+         * If steps differ, both axes are moving
+         * If DeltaA ==  DeltaB, the movement is only in the 1st axis (X or Y)
+         * If DeltaA == -DeltaB, the movement is only in the 2nd axis (Y or Z)
+         */
+        #if EITHER(COREYX, COREYZ)
+          #define Y_CMP(A,B) ((A)==(B))
+        #else
+          #define Y_CMP(A,B) ((A)!=(B))
+        #endif
+        #define Y_MOVE_TEST ( S_(1) != S_(2) || (S_(1) > 0 && Y_CMP(D_(1),D_(2))) )
+      #else
+        #define Y_MOVE_TEST !!current_block->steps.b
+      #endif
+
+      #if CORE_IS_XZ || CORE_IS_YZ
+        /**
+         * Head direction in -Z axis for CoreXZ or CoreYZ bots.
+         *
+         * If steps differ, both axes are moving
+         * If DeltaA ==  DeltaB, the movement is only in the 1st axis (X or Y, already handled above)
+         * If DeltaA == -DeltaB, the movement is only in the 2nd axis (Z)
+         */
+        #if EITHER(COREZX, COREZY)
+          #define Z_CMP(A,B) ((A)==(B))
+        #else
+          #define Z_CMP(A,B) ((A)!=(B))
+        #endif
+        #define Z_MOVE_TEST ( S_(1) != S_(2) || (S_(1) > 0 && Z_CMP(D_(1),D_(2))) )
+      #else
+        #define Z_MOVE_TEST !!current_block->steps.c
+      #endif
+
+      uint8_t axis_bits = 0;
+      if (X_MOVE_TEST) SBI(axis_bits, A_AXIS);
+      if (Y_MOVE_TEST) SBI(axis_bits, B_AXIS);
+      if (Z_MOVE_TEST) SBI(axis_bits, C_AXIS);
+      //if (!!current_block->steps.e) SBI(axis_bits, E_AXIS);
+      //if (!!current_block->steps.a) SBI(axis_bits, X_HEAD);
+      //if (!!current_block->steps.b) SBI(axis_bits, Y_HEAD);
+      //if (!!current_block->steps.c) SBI(axis_bits, Z_HEAD);
+      axis_did_move = axis_bits;
+
+      // No acceleration / deceleration time elapsed so far
+      acceleration_time = deceleration_time = 0;
+
+      #if ENABLED(ADAPTIVE_STEP_SMOOTHING)
+        uint8_t oversampling = 0;                           // Assume no axis smoothing (via oversampling)
+        // Decide if axis smoothing is possible
+        uint32_t max_rate = current_block->nominal_rate;    // Get the step event rate
+        while (max_rate < MIN_STEP_ISR_FREQUENCY) {         // As long as more ISRs are possible...
+          max_rate <<= 1;                                   // Try to double the rate
+          if (max_rate < MIN_STEP_ISR_FREQUENCY)            // Don't exceed the estimated ISR limit
+            ++oversampling;                                 // Increase the oversampling (used for left-shift)
+        }
+        oversampling_factor = oversampling;                 // For all timer interval calculations
+      #else
+        constexpr uint8_t oversampling = 0;
+      #endif
+
+      // Based on the oversampling factor, do the calculations
+      step_event_count = current_block->step_event_count << oversampling;
+
+      // Initialize Bresenham delta errors to 1/2
+      delta_error = -int32_t(step_event_count);
+
+      // Calculate Bresenham dividends and divisors
+      advance_dividend = current_block->steps << 1;
+      advance_divisor = step_event_count << 1;
+
+      // No step events completed so far
+      step_events_completed = 0;
+
+      // Compute the acceleration and deceleration points
+      accelerate_until = current_block->accelerate_until << oversampling;
+      decelerate_after = current_block->decelerate_after << oversampling;
+
+      #if ENABLED(MIXING_EXTRUDER)
+        MIXER_STEPPER_SETUP();
+      #endif
+
+      TERN_(HAS_MULTI_EXTRUDER, stepper_extruder = current_block->extruder);
+
+      // Initialize the trapezoid generator from the current block.
+      #if ENABLED(LIN_ADVANCE)
+        #if DISABLED(MIXING_EXTRUDER) && E_STEPPERS > 1
+          // If the now active extruder wasn't in use during the last move, its pressure is most likely gone.
+          if (stepper_extruder != last_moved_extruder) LA_current_adv_steps = 0;
+        #endif
+
+        if ((LA_use_advance_lead = current_block->use_advance_lead)) {
+          LA_final_adv_steps = current_block->final_adv_steps;
+          LA_max_adv_steps = current_block->max_adv_steps;
+          initiateLA(); // Start the ISR
+          LA_isr_rate = current_block->advance_speed;
+        }
+        else LA_isr_rate = LA_ADV_NEVER;
+      #endif
+
+      if ( ENABLED(HAS_L64XX)       // Always set direction for L64xx (Also enables the chips)
+        || ENABLED(DUAL_X_CARRIAGE) // TODO: Find out why this fixes "jittery" small circles
+        || current_block->direction_bits != last_direction_bits
+        || TERN(MIXING_EXTRUDER, false, stepper_extruder != last_moved_extruder)
+      ) {
+        TERN_(HAS_MULTI_EXTRUDER, last_moved_extruder = stepper_extruder);
+        TERN_(HAS_L64XX, L64XX_OK_to_power_up = true);
+        set_directions(current_block->direction_bits);
+      }
+
+      #if ENABLED(LASER_POWER_INLINE)
+        const power_status_t stat = current_block->laser.status;
+        #if ENABLED(LASER_POWER_INLINE_TRAPEZOID)
+          laser_trap.enabled = stat.isPlanned && stat.isEnabled;
+          laser_trap.cur_power = current_block->laser.power_entry; // RESET STATE
+          laser_trap.cruise_set = false;
+          #if DISABLED(LASER_POWER_INLINE_TRAPEZOID_CONT)
+            laser_trap.last_step_count = 0;
+            laser_trap.acc_step_count = current_block->laser.entry_per / 2;
+          #else
+            laser_trap.till_update = 0;
+          #endif
+          // Always have PWM in this case
+          if (stat.isPlanned) {                        // Planner controls the laser
+            cutter.set_ocr_power(
+              stat.isEnabled ? laser_trap.cur_power : 0 // ON with power or OFF
+            );
+          }
+        #else
+          if (stat.isPlanned) {                        // Planner controls the laser
+            #if ENABLED(SPINDLE_LASER_PWM)
+              cutter.set_ocr_power(
+                stat.isEnabled ? current_block->laser.power : 0 // ON with power or OFF
+              );
+            #else
+              cutter.set_enabled(stat.isEnabled);
+            #endif
+          }
+        #endif
+      #endif // LASER_POWER_INLINE
+
+      // At this point, we must ensure the movement about to execute isn't
+      // trying to force the head against a limit switch. If using interrupt-
+      // driven change detection, and already against a limit then no call to
+      // the endstop_triggered method will be done and the movement will be
+      // done against the endstop. So, check the limits here: If the movement
+      // is against the limits, the block will be marked as to be killed, and
+      // on the next call to this ISR, will be discarded.
+      endstops.update();
+
+      #if ENABLED(Z_LATE_ENABLE)
+        // If delayed Z enable, enable it now. This option will severely interfere with
+        // timing between pulses when chaining motion between blocks, and it could lead
+        // to lost steps in both X and Y axis, so avoid using it unless strictly necessary!!
+        if (current_block->steps.z) ENABLE_AXIS_Z();
+      #endif
+
+      // Mark the time_nominal as not calculated yet
+      ticks_nominal = -1;
+
+      #if ENABLED(S_CURVE_ACCELERATION)
+        // Initialize the Bézier speed curve
+        _calc_bezier_curve_coeffs(current_block->initial_rate, current_block->cruise_rate, current_block->acceleration_time_inverse);
+        // We haven't started the 2nd half of the trapezoid
+        bezier_2nd_half = false;
+      #else
+        // Set as deceleration point the initial rate of the block
+        acc_step_rate = current_block->initial_rate;
+      #endif
+
+      // Calculate the initial timer interval
+      interval = calc_timer_interval(current_block->initial_rate, &steps_per_isr);
+    }
+    #if ENABLED(LASER_POWER_INLINE_CONTINUOUS)
+      else { // No new block found; so apply inline laser parameters
+        // This should mean ending file with 'M5 I' will stop the laser; thus the inline flag isn't needed
+        const power_status_t stat = planner.laser_inline.status;
+        if (stat.isPlanned) {             // Planner controls the laser
+          #if ENABLED(SPINDLE_LASER_PWM)
+            cutter.set_ocr_power(
+              stat.isEnabled ? planner.laser_inline.power : 0 // ON with power or OFF
+            );
+          #else
+            cutter.set_enabled(stat.isEnabled);
+          #endif
+        }
+      }
+    #endif
+  }
+
+  // Return the interval to wait
+  return interval;
+}
+
+#if ENABLED(LIN_ADVANCE)
+
+  // Timer interrupt for E. LA_steps is set in the main routine
+  uint32_t Stepper::advance_isr() {
+    uint32_t interval;
+
+    if (LA_use_advance_lead) {
+      if (step_events_completed > decelerate_after && LA_current_adv_steps > LA_final_adv_steps) {
+        LA_steps--;
+        LA_current_adv_steps--;
+        interval = LA_isr_rate;
+      }
+      else if (step_events_completed < decelerate_after && LA_current_adv_steps < LA_max_adv_steps) {
+        LA_steps++;
+        LA_current_adv_steps++;
+        interval = LA_isr_rate;
+      }
+      else
+        interval = LA_isr_rate = LA_ADV_NEVER;
+    }
+    else
+      interval = LA_ADV_NEVER;
+
+    if (!LA_steps) return interval; // Leave pins alone if there are no steps!
+
+    DIR_WAIT_BEFORE();
+
+    #if ENABLED(MIXING_EXTRUDER)
+      // We don't know which steppers will be stepped because LA loop follows,
+      // with potentially multiple steps. Set all.
+      if (LA_steps > 0)
+        MIXER_STEPPER_LOOP(j) NORM_E_DIR(j);
+      else if (LA_steps < 0)
+        MIXER_STEPPER_LOOP(j) REV_E_DIR(j);
+    #else
+      if (LA_steps > 0)
+        NORM_E_DIR(stepper_extruder);
+      else if (LA_steps < 0)
+        REV_E_DIR(stepper_extruder);
+    #endif
+
+    DIR_WAIT_AFTER();
+
+    //const hal_timer_t added_step_ticks = hal_timer_t(ADDED_STEP_TICKS);
+
+    // Step E stepper if we have steps
+    #if ISR_MULTI_STEPS
+      bool firstStep = true;
+      USING_TIMED_PULSE();
+    #endif
+
+    while (LA_steps) {
+      #if ISR_MULTI_STEPS
+        if (firstStep)
+          firstStep = false;
+        else
+          AWAIT_LOW_PULSE();
+      #endif
+
+      // Set the STEP pulse ON
+      #if ENABLED(MIXING_EXTRUDER)
+        E_STEP_WRITE(mixer.get_next_stepper(), !INVERT_E_STEP_PIN);
+      #else
+        E_STEP_WRITE(stepper_extruder, !INVERT_E_STEP_PIN);
+      #endif
+
+      // Enforce a minimum duration for STEP pulse ON
+      #if ISR_PULSE_CONTROL
+        START_HIGH_PULSE();
+      #endif
+
+      LA_steps < 0 ? ++LA_steps : --LA_steps;
+
+      #if ISR_PULSE_CONTROL
+        AWAIT_HIGH_PULSE();
+      #endif
+
+      // Set the STEP pulse OFF
+      #if ENABLED(MIXING_EXTRUDER)
+        E_STEP_WRITE(mixer.get_stepper(), INVERT_E_STEP_PIN);
+      #else
+        E_STEP_WRITE(stepper_extruder, INVERT_E_STEP_PIN);
+      #endif
+
+      // For minimum pulse time wait before looping
+      // Just wait for the requested pulse duration
+      #if ISR_PULSE_CONTROL
+        if (LA_steps) START_LOW_PULSE();
+      #endif
+    } // LA_steps
+
+    return interval;
+  }
+
+#endif // LIN_ADVANCE
+
+#if ENABLED(INTEGRATED_BABYSTEPPING)
+
+  // Timer interrupt for baby-stepping
+  uint32_t Stepper::babystepping_isr() {
+    babystep.task();
+    return babystep.has_steps() ? BABYSTEP_TICKS : BABYSTEP_NEVER;
+  }
+
+#endif
+
+// Check if the given block is busy or not - Must not be called from ISR contexts
+// The current_block could change in the middle of the read by an Stepper ISR, so
+// we must explicitly prevent that!
+bool Stepper::is_block_busy(const block_t* const block) {
+  #ifdef __AVR__
+    // A SW memory barrier, to ensure GCC does not overoptimize loops
+    #define sw_barrier() asm volatile("": : :"memory");
+
+    // Keep reading until 2 consecutive reads return the same value,
+    // meaning there was no update in-between caused by an interrupt.
+    // This works because stepper ISRs happen at a slower rate than
+    // successive reads of a variable, so 2 consecutive reads with
+    // the same value means no interrupt updated it.
+    block_t* vold, *vnew = current_block;
+    sw_barrier();
+    do {
+      vold = vnew;
+      vnew = current_block;
+      sw_barrier();
+    } while (vold != vnew);
+  #else
+    block_t *vnew = current_block;
+  #endif
+
+  // Return if the block is busy or not
+  return block == vnew;
+}
+
+void Stepper::init() {
+
+  #if MB(ALLIGATOR)
+    const float motor_current[] = MOTOR_CURRENT;
+    unsigned int digipot_motor = 0;
+    LOOP_L_N(i, 3 + EXTRUDERS) {
+      digipot_motor = 255 * (motor_current[i] / 2.5);
+      dac084s085::setValue(i, digipot_motor);
+    }
+  #endif
+
+  // Init Microstepping Pins
+  TERN_(HAS_MICROSTEPS, microstep_init());
+
+  // Init Dir Pins
+  TERN_(HAS_X_DIR, X_DIR_INIT());
+  TERN_(HAS_X2_DIR, X2_DIR_INIT());
+  #if HAS_Y_DIR
+    Y_DIR_INIT();
+    #if BOTH(Y_DUAL_STEPPER_DRIVERS, HAS_Y2_DIR)
+      Y2_DIR_INIT();
+    #endif
+  #endif
+  #if HAS_Z_DIR
+    Z_DIR_INIT();
+    #if NUM_Z_STEPPER_DRIVERS >= 2 && HAS_Z2_DIR
+      Z2_DIR_INIT();
+    #endif
+    #if NUM_Z_STEPPER_DRIVERS >= 3 && HAS_Z3_DIR
+      Z3_DIR_INIT();
+    #endif
+    #if NUM_Z_STEPPER_DRIVERS >= 4 && HAS_Z4_DIR
+      Z4_DIR_INIT();
+    #endif
+  #endif
+  #if HAS_E0_DIR
+    E0_DIR_INIT();
+  #endif
+  #if HAS_E1_DIR
+    E1_DIR_INIT();
+  #endif
+  #if HAS_E2_DIR
+    E2_DIR_INIT();
+  #endif
+  #if HAS_E3_DIR
+    E3_DIR_INIT();
+  #endif
+  #if HAS_E4_DIR
+    E4_DIR_INIT();
+  #endif
+  #if HAS_E5_DIR
+    E5_DIR_INIT();
+  #endif
+  #if HAS_E6_DIR
+    E6_DIR_INIT();
+  #endif
+  #if HAS_E7_DIR
+    E7_DIR_INIT();
+  #endif
+
+  // Init Enable Pins - steppers default to disabled.
+  #if HAS_X_ENABLE
+    X_ENABLE_INIT();
+    if (!X_ENABLE_ON) X_ENABLE_WRITE(HIGH);
+    #if EITHER(DUAL_X_CARRIAGE, X_DUAL_STEPPER_DRIVERS) && HAS_X2_ENABLE
+      X2_ENABLE_INIT();
+      if (!X_ENABLE_ON) X2_ENABLE_WRITE(HIGH);
+    #endif
+  #endif
+  #if HAS_Y_ENABLE
+    Y_ENABLE_INIT();
+    if (!Y_ENABLE_ON) Y_ENABLE_WRITE(HIGH);
+    #if BOTH(Y_DUAL_STEPPER_DRIVERS, HAS_Y2_ENABLE)
+      Y2_ENABLE_INIT();
+      if (!Y_ENABLE_ON) Y2_ENABLE_WRITE(HIGH);
+    #endif
+  #endif
+  #if HAS_Z_ENABLE
+    Z_ENABLE_INIT();
+    if (!Z_ENABLE_ON) Z_ENABLE_WRITE(HIGH);
+    #if NUM_Z_STEPPER_DRIVERS >= 2 && HAS_Z2_ENABLE
+      Z2_ENABLE_INIT();
+      if (!Z_ENABLE_ON) Z2_ENABLE_WRITE(HIGH);
+    #endif
+    #if NUM_Z_STEPPER_DRIVERS >= 3 && HAS_Z3_ENABLE
+      Z3_ENABLE_INIT();
+      if (!Z_ENABLE_ON) Z3_ENABLE_WRITE(HIGH);
+    #endif
+    #if NUM_Z_STEPPER_DRIVERS >= 4 && HAS_Z4_ENABLE
+      Z4_ENABLE_INIT();
+      if (!Z_ENABLE_ON) Z4_ENABLE_WRITE(HIGH);
+    #endif
+  #endif
+  #if HAS_E0_ENABLE
+    E0_ENABLE_INIT();
+    if (!E_ENABLE_ON) E0_ENABLE_WRITE(HIGH);
+  #endif
+  #if HAS_E1_ENABLE
+    E1_ENABLE_INIT();
+    if (!E_ENABLE_ON) E1_ENABLE_WRITE(HIGH);
+  #endif
+  #if HAS_E2_ENABLE
+    E2_ENABLE_INIT();
+    if (!E_ENABLE_ON) E2_ENABLE_WRITE(HIGH);
+  #endif
+  #if HAS_E3_ENABLE
+    E3_ENABLE_INIT();
+    if (!E_ENABLE_ON) E3_ENABLE_WRITE(HIGH);
+  #endif
+  #if HAS_E4_ENABLE
+    E4_ENABLE_INIT();
+    if (!E_ENABLE_ON) E4_ENABLE_WRITE(HIGH);
+  #endif
+  #if HAS_E5_ENABLE
+    E5_ENABLE_INIT();
+    if (!E_ENABLE_ON) E5_ENABLE_WRITE(HIGH);
+  #endif
+  #if HAS_E6_ENABLE
+    E6_ENABLE_INIT();
+    if (!E_ENABLE_ON) E6_ENABLE_WRITE(HIGH);
+  #endif
+  #if HAS_E7_ENABLE
+    E7_ENABLE_INIT();
+    if (!E_ENABLE_ON) E7_ENABLE_WRITE(HIGH);
+  #endif
+
+  #define _STEP_INIT(AXIS) AXIS ##_STEP_INIT()
+  #define _WRITE_STEP(AXIS, HIGHLOW) AXIS ##_STEP_WRITE(HIGHLOW)
+  #define _DISABLE_AXIS(AXIS) DISABLE_AXIS_## AXIS()
+
+  #define AXIS_INIT(AXIS, PIN) \
+    _STEP_INIT(AXIS); \
+    _WRITE_STEP(AXIS, _INVERT_STEP_PIN(PIN)); \
+    _DISABLE_AXIS(AXIS)
+
+  #define E_AXIS_INIT(NUM) AXIS_INIT(E## NUM, E)
+
+  // Init Step Pins
+  #if HAS_X_STEP
+    #if EITHER(X_DUAL_STEPPER_DRIVERS, DUAL_X_CARRIAGE)
+      X2_STEP_INIT();
+      X2_STEP_WRITE(INVERT_X_STEP_PIN);
+    #endif
+    AXIS_INIT(X, X);
+  #endif
+
+  #if HAS_Y_STEP
+    #if ENABLED(Y_DUAL_STEPPER_DRIVERS)
+      Y2_STEP_INIT();
+      Y2_STEP_WRITE(INVERT_Y_STEP_PIN);
+    #endif
+    AXIS_INIT(Y, Y);
+  #endif
+
+  #if HAS_Z_STEP
+    #if NUM_Z_STEPPER_DRIVERS >= 2
+      Z2_STEP_INIT();
+      Z2_STEP_WRITE(INVERT_Z_STEP_PIN);
+    #endif
+    #if NUM_Z_STEPPER_DRIVERS >= 3
+      Z3_STEP_INIT();
+      Z3_STEP_WRITE(INVERT_Z_STEP_PIN);
+    #endif
+    #if NUM_Z_STEPPER_DRIVERS >= 4
+      Z4_STEP_INIT();
+      Z4_STEP_WRITE(INVERT_Z_STEP_PIN);
+    #endif
+    AXIS_INIT(Z, Z);
+  #endif
+
+  #if E_STEPPERS && HAS_E0_STEP
+    E_AXIS_INIT(0);
+  #endif
+  #if E_STEPPERS > 1 && HAS_E1_STEP
+    E_AXIS_INIT(1);
+  #endif
+  #if E_STEPPERS > 2 && HAS_E2_STEP
+    E_AXIS_INIT(2);
+  #endif
+  #if E_STEPPERS > 3 && HAS_E3_STEP
+    E_AXIS_INIT(3);
+  #endif
+  #if E_STEPPERS > 4 && HAS_E4_STEP
+    E_AXIS_INIT(4);
+  #endif
+  #if E_STEPPERS > 5 && HAS_E5_STEP
+    E_AXIS_INIT(5);
+  #endif
+  #if E_STEPPERS > 6 && HAS_E6_STEP
+    E_AXIS_INIT(6);
+  #endif
+  #if E_STEPPERS > 7 && HAS_E7_STEP
+    E_AXIS_INIT(7);
+  #endif
+
+  #if DISABLED(I2S_STEPPER_STREAM)
+    HAL_timer_start(STEP_TIMER_NUM, 122); // Init Stepper ISR to 122 Hz for quick starting
+    wake_up();
+    sei();
+  #endif
+
+  // Init direction bits for first moves
+  set_directions((INVERT_X_DIR ? _BV(X_AXIS) : 0)
+               | (INVERT_Y_DIR ? _BV(Y_AXIS) : 0)
+               | (INVERT_Z_DIR ? _BV(Z_AXIS) : 0));
+
+  #if HAS_MOTOR_CURRENT_SPI || HAS_MOTOR_CURRENT_PWM
+    initialized = true;
+    digipot_init();
+  #endif
+}
+
+/**
+ * Set the stepper positions directly in steps
+ *
+ * The input is based on the typical per-axis XYZ steps.
+ * For CORE machines XYZ needs to be translated to ABC.
+ *
+ * This allows get_axis_position_mm to correctly
+ * derive the current XYZ position later on.
+ */
+void Stepper::_set_position(const int32_t &a, const int32_t &b, const int32_t &c, const int32_t &e) {
+  #if CORE_IS_XY
+    // corexy positioning
+    // these equations follow the form of the dA and dB equations on https://www.corexy.com/theory.html
+    count_position.set(a + b, CORESIGN(a - b), c);
+  #elif CORE_IS_XZ
+    // corexz planning
+    count_position.set(a + c, b, CORESIGN(a - c));
+  #elif CORE_IS_YZ
+    // coreyz planning
+    count_position.set(a, b + c, CORESIGN(b - c));
+  #elif ENABLED(MARKFORGED_XY)
+    count_position.set(a - b, b, c);
+  #else
+    // default non-h-bot planning
+    count_position.set(a, b, c);
+  #endif
+  count_position.e = e;
+}
+
+/**
+ * Get a stepper's position in steps.
+ */
+int32_t Stepper::position(const AxisEnum axis) {
+  #ifdef __AVR__
+    // Protect the access to the position. Only required for AVR, as
+    //  any 32bit CPU offers atomic access to 32bit variables
+    const bool was_enabled = suspend();
+  #endif
+
+  const int32_t v = count_position[axis];
+
+  #ifdef __AVR__
+    // Reenable Stepper ISR
+    if (was_enabled) wake_up();
+  #endif
+  return v;
+}
+
+// Set the current position in steps
+void Stepper::set_position(const int32_t &a, const int32_t &b, const int32_t &c, const int32_t &e) {
+  planner.synchronize();
+  const bool was_enabled = suspend();
+  _set_position(a, b, c, e);
+  if (was_enabled) wake_up();
+}
+
+void Stepper::set_axis_position(const AxisEnum a, const int32_t &v) {
+  planner.synchronize();
+
+  #ifdef __AVR__
+    // Protect the access to the position. Only required for AVR, as
+    //  any 32bit CPU offers atomic access to 32bit variables
+    const bool was_enabled = suspend();
+  #endif
+
+  count_position[a] = v;
+
+  #ifdef __AVR__
+    // Reenable Stepper ISR
+    if (was_enabled) wake_up();
+  #endif
+}
+
+// Signal endstops were triggered - This function can be called from
+// an ISR context  (Temperature, Stepper or limits ISR), so we must
+// be very careful here. If the interrupt being preempted was the
+// Stepper ISR (this CAN happen with the endstop limits ISR) then
+// when the stepper ISR resumes, we must be very sure that the movement
+// is properly canceled
+void Stepper::endstop_triggered(const AxisEnum axis) {
+
+  const bool was_enabled = suspend();
+  endstops_trigsteps[axis] = (
+    #if IS_CORE
+      (axis == CORE_AXIS_2
+        ? CORESIGN(count_position[CORE_AXIS_1] - count_position[CORE_AXIS_2])
+        : count_position[CORE_AXIS_1] + count_position[CORE_AXIS_2]
+      ) * double(0.5)
+    #elif ENABLED(MARKFORGED_XY)
+      axis == CORE_AXIS_1
+        ? count_position[CORE_AXIS_1] - count_position[CORE_AXIS_2]
+        : count_position[CORE_AXIS_2]
+    #else // !IS_CORE
+      count_position[axis]
+    #endif
+  );
+
+  // Discard the rest of the move if there is a current block
+  quick_stop();
+
+  if (was_enabled) wake_up();
+}
+
+int32_t Stepper::triggered_position(const AxisEnum axis) {
+  #ifdef __AVR__
+    // Protect the access to the position. Only required for AVR, as
+    //  any 32bit CPU offers atomic access to 32bit variables
+    const bool was_enabled = suspend();
+  #endif
+
+  const int32_t v = endstops_trigsteps[axis];
+
+  #ifdef __AVR__
+    // Reenable Stepper ISR
+    if (was_enabled) wake_up();
+  #endif
+
+  return v;
+}
+
+void Stepper::report_a_position(const xyz_long_t &pos) {
+  #if ANY(CORE_IS_XY, CORE_IS_XZ, MARKFORGED_XY, DELTA, IS_SCARA)
+    SERIAL_ECHOPAIR(STR_COUNT_A, pos.x, " B:", pos.y);
+  #else
+    SERIAL_ECHOPAIR_P(PSTR(STR_COUNT_X), pos.x, SP_Y_LBL, pos.y);
+  #endif
+  #if ANY(CORE_IS_XZ, CORE_IS_YZ, DELTA)
+    SERIAL_ECHOLNPAIR(" C:", pos.z);
+  #else
+    SERIAL_ECHOLNPAIR_P(SP_Z_LBL, pos.z);
+  #endif
+}
+
+void Stepper::report_positions() {
+
+  #ifdef __AVR__
+    // Protect the access to the position.
+    const bool was_enabled = suspend();
+  #endif
+
+  const xyz_long_t pos = count_position;
+
+  #ifdef __AVR__
+    if (was_enabled) wake_up();
+  #endif
+
+  report_a_position(pos);
+}
+
+#if ENABLED(BABYSTEPPING)
+
+  #define _ENABLE_AXIS(AXIS) ENABLE_AXIS_## AXIS()
+  #define _READ_DIR(AXIS) AXIS ##_DIR_READ()
+  #define _INVERT_DIR(AXIS) INVERT_## AXIS ##_DIR
+  #define _APPLY_DIR(AXIS, INVERT) AXIS ##_APPLY_DIR(INVERT, true)
+
+  #if MINIMUM_STEPPER_PULSE
+    #define STEP_PULSE_CYCLES ((MINIMUM_STEPPER_PULSE) * CYCLES_PER_MICROSECOND)
+  #else
+    #define STEP_PULSE_CYCLES 0
+  #endif
+
+  #if ENABLED(DELTA)
+    #define CYCLES_EATEN_BABYSTEP (2 * 15)
+  #else
+    #define CYCLES_EATEN_BABYSTEP 0
+  #endif
+  #define EXTRA_CYCLES_BABYSTEP (STEP_PULSE_CYCLES - (CYCLES_EATEN_BABYSTEP))
+
+  #if EXTRA_CYCLES_BABYSTEP > 20
+    #define _SAVE_START() const hal_timer_t pulse_start = HAL_timer_get_count(PULSE_TIMER_NUM)
+    #define _PULSE_WAIT() while (EXTRA_CYCLES_BABYSTEP > (uint32_t)(HAL_timer_get_count(PULSE_TIMER_NUM) - pulse_start) * (PULSE_TIMER_PRESCALE)) { /* nada */ }
+  #else
+    #define _SAVE_START() NOOP
+    #if EXTRA_CYCLES_BABYSTEP > 0
+      #define _PULSE_WAIT() DELAY_NS(EXTRA_CYCLES_BABYSTEP * NANOSECONDS_PER_CYCLE)
+    #elif ENABLED(DELTA)
+      #define _PULSE_WAIT() DELAY_US(2);
+    #elif STEP_PULSE_CYCLES > 0
+      #define _PULSE_WAIT() NOOP
+    #else
+      #define _PULSE_WAIT() DELAY_US(4);
+    #endif
+  #endif
+
+  #if ENABLED(BABYSTEPPING_EXTRA_DIR_WAIT)
+    #define EXTRA_DIR_WAIT_BEFORE DIR_WAIT_BEFORE
+    #define EXTRA_DIR_WAIT_AFTER  DIR_WAIT_AFTER
+  #else
+    #define EXTRA_DIR_WAIT_BEFORE()
+    #define EXTRA_DIR_WAIT_AFTER()
+  #endif
+
+  #if DISABLED(DELTA)
+
+    #define BABYSTEP_AXIS(AXIS, INV, DIR) do{           \
+      const uint8_t old_dir = _READ_DIR(AXIS);          \
+      _ENABLE_AXIS(AXIS);                               \
+      DIR_WAIT_BEFORE();                                \
+      _APPLY_DIR(AXIS, _INVERT_DIR(AXIS)^DIR^INV);      \
+      DIR_WAIT_AFTER();                                 \
+      _SAVE_START();                                    \
+      _APPLY_STEP(AXIS, !_INVERT_STEP_PIN(AXIS), true); \
+      _PULSE_WAIT();                                    \
+      _APPLY_STEP(AXIS, _INVERT_STEP_PIN(AXIS), true);  \
+      EXTRA_DIR_WAIT_BEFORE();                          \
+      _APPLY_DIR(AXIS, old_dir);                        \
+      EXTRA_DIR_WAIT_AFTER();                           \
+    }while(0)
+
+  #endif
+
+  #if IS_CORE
+
+    #define BABYSTEP_CORE(A, B, INV, DIR, ALT) do{              \
+      const xy_byte_t old_dir = { _READ_DIR(A), _READ_DIR(B) }; \
+      _ENABLE_AXIS(A); _ENABLE_AXIS(B);                         \
+      DIR_WAIT_BEFORE();                                        \
+      _APPLY_DIR(A, _INVERT_DIR(A)^DIR^INV);                    \
+      _APPLY_DIR(B, _INVERT_DIR(B)^DIR^INV^ALT);                \
+      DIR_WAIT_AFTER();                                         \
+      _SAVE_START();                                            \
+      _APPLY_STEP(A, !_INVERT_STEP_PIN(A), true);               \
+      _APPLY_STEP(B, !_INVERT_STEP_PIN(B), true);               \
+      _PULSE_WAIT();                                            \
+      _APPLY_STEP(A, _INVERT_STEP_PIN(A), true);                \
+      _APPLY_STEP(B, _INVERT_STEP_PIN(B), true);                \
+      EXTRA_DIR_WAIT_BEFORE();                                  \
+      _APPLY_DIR(A, old_dir.a); _APPLY_DIR(B, old_dir.b);       \
+      EXTRA_DIR_WAIT_AFTER();                                   \
+    }while(0)
+
+  #endif
+
+  // MUST ONLY BE CALLED BY AN ISR,
+  // No other ISR should ever interrupt this!
+  void Stepper::do_babystep(const AxisEnum axis, const bool direction) {
+
+    #if DISABLED(INTEGRATED_BABYSTEPPING)
+      cli();
+    #endif
+
+    switch (axis) {
+
+      #if ENABLED(BABYSTEP_XY)
+
+        case X_AXIS:
+          #if CORE_IS_XY
+            BABYSTEP_CORE(X, Y, 0, direction, 0);
+          #elif CORE_IS_XZ
+            BABYSTEP_CORE(X, Z, 0, direction, 0);
+          #else
+            BABYSTEP_AXIS(X, 0, direction);
+          #endif
+          break;
+
+        case Y_AXIS:
+          #if CORE_IS_XY
+            BABYSTEP_CORE(X, Y, 1, !direction, (CORESIGN(1)>0));
+          #elif CORE_IS_YZ
+            BABYSTEP_CORE(Y, Z, 0, direction, (CORESIGN(1)<0));
+          #else
+            BABYSTEP_AXIS(Y, 0, direction);
+          #endif
+          break;
+
+      #endif
+
+      case Z_AXIS: {
+
+        #if CORE_IS_XZ
+          BABYSTEP_CORE(X, Z, BABYSTEP_INVERT_Z, direction, (CORESIGN(1)<0));
+        #elif CORE_IS_YZ
+          BABYSTEP_CORE(Y, Z, BABYSTEP_INVERT_Z, direction, (CORESIGN(1)<0));
+        #elif DISABLED(DELTA)
+          BABYSTEP_AXIS(Z, BABYSTEP_INVERT_Z, direction);
+
+        #else // DELTA
+
+          const bool z_direction = direction ^ BABYSTEP_INVERT_Z;
+
+          ENABLE_AXIS_X();
+          ENABLE_AXIS_Y();
+          ENABLE_AXIS_Z();
+
+          DIR_WAIT_BEFORE();
+
+          const xyz_byte_t old_dir = { X_DIR_READ(), Y_DIR_READ(), Z_DIR_READ() };
+
+          X_DIR_WRITE(INVERT_X_DIR ^ z_direction);
+          Y_DIR_WRITE(INVERT_Y_DIR ^ z_direction);
+          Z_DIR_WRITE(INVERT_Z_DIR ^ z_direction);
+
+          DIR_WAIT_AFTER();
+
+          _SAVE_START();
+
+          X_STEP_WRITE(!INVERT_X_STEP_PIN);
+          Y_STEP_WRITE(!INVERT_Y_STEP_PIN);
+          Z_STEP_WRITE(!INVERT_Z_STEP_PIN);
+
+          _PULSE_WAIT();
+
+          X_STEP_WRITE(INVERT_X_STEP_PIN);
+          Y_STEP_WRITE(INVERT_Y_STEP_PIN);
+          Z_STEP_WRITE(INVERT_Z_STEP_PIN);
+
+          // Restore direction bits
+          EXTRA_DIR_WAIT_BEFORE();
+
+          X_DIR_WRITE(old_dir.x);
+          Y_DIR_WRITE(old_dir.y);
+          Z_DIR_WRITE(old_dir.z);
+
+          EXTRA_DIR_WAIT_AFTER();
+
+        #endif
+
+      } break;
+
+      default: break;
+    }
+
+    #if DISABLED(INTEGRATED_BABYSTEPPING)
+      sei();
+    #endif
+  }
+
+#endif // BABYSTEPPING
+
+/**
+ * Software-controlled Stepper Motor Current
+ */
+
+#if HAS_MOTOR_CURRENT_SPI
+
+  // From Arduino DigitalPotControl example
+  void Stepper::set_digipot_value_spi(const int16_t address, const int16_t value) {
+    WRITE(DIGIPOTSS_PIN, LOW);  // Take the SS pin low to select the chip
+    SPI.transfer(address);      // Send the address and value via SPI
+    SPI.transfer(value);
+    WRITE(DIGIPOTSS_PIN, HIGH); // Take the SS pin high to de-select the chip
+    //delay(10);
+  }
+
+#endif // HAS_MOTOR_CURRENT_SPI
+
+#if HAS_MOTOR_CURRENT_PWM
+
+  void Stepper::refresh_motor_power() {
+    if (!initialized) return;
+    LOOP_L_N(i, COUNT(motor_current_setting)) {
+      switch (i) {
+        #if ANY_PIN(MOTOR_CURRENT_PWM_XY, MOTOR_CURRENT_PWM_X, MOTOR_CURRENT_PWM_Y)
+          case 0:
+        #endif
+        #if PIN_EXISTS(MOTOR_CURRENT_PWM_Z)
+          case 1:
+        #endif
+        #if ANY_PIN(MOTOR_CURRENT_PWM_E, MOTOR_CURRENT_PWM_E0, MOTOR_CURRENT_PWM_E1)
+          case 2:
+        #endif
+            set_digipot_current(i, motor_current_setting[i]);
+        default: break;
+      }
+    }
+  }
+
+#endif // HAS_MOTOR_CURRENT_PWM
+
+#if !MB(PRINTRBOARD_G2)
+
+  #if HAS_MOTOR_CURRENT_SPI || HAS_MOTOR_CURRENT_PWM
+
+    void Stepper::set_digipot_current(const uint8_t driver, const int16_t current) {
+      if (WITHIN(driver, 0, MOTOR_CURRENT_COUNT - 1))
+        motor_current_setting[driver] = current; // update motor_current_setting
+
+      if (!initialized) return;
+
+      #if HAS_MOTOR_CURRENT_SPI
+
+        //SERIAL_ECHOLNPAIR("Digipotss current ", current);
+
+        const uint8_t digipot_ch[] = DIGIPOT_CHANNELS;
+        set_digipot_value_spi(digipot_ch[driver], current);
+
+      #elif HAS_MOTOR_CURRENT_PWM
+
+        #define _WRITE_CURRENT_PWM(P) analogWrite(pin_t(MOTOR_CURRENT_PWM_## P ##_PIN), 255L * current / (MOTOR_CURRENT_PWM_RANGE))
+        switch (driver) {
+          case 0:
+            #if PIN_EXISTS(MOTOR_CURRENT_PWM_X)
+              _WRITE_CURRENT_PWM(X);
+            #endif
+            #if PIN_EXISTS(MOTOR_CURRENT_PWM_Y)
+              _WRITE_CURRENT_PWM(Y);
+            #endif
+            #if PIN_EXISTS(MOTOR_CURRENT_PWM_XY)
+              _WRITE_CURRENT_PWM(XY);
+            #endif
+            break;
+          case 1:
+            #if PIN_EXISTS(MOTOR_CURRENT_PWM_Z)
+              _WRITE_CURRENT_PWM(Z);
+            #endif
+            break;
+          case 2:
+            #if PIN_EXISTS(MOTOR_CURRENT_PWM_E)
+              _WRITE_CURRENT_PWM(E);
+            #endif
+            #if PIN_EXISTS(MOTOR_CURRENT_PWM_E0)
+              _WRITE_CURRENT_PWM(E0);
+            #endif
+            #if PIN_EXISTS(MOTOR_CURRENT_PWM_E1)
+              _WRITE_CURRENT_PWM(E1);
+            #endif
+            break;
+        }
+      #endif
+    }
+
+    void Stepper::digipot_init() {
+
+      #if HAS_MOTOR_CURRENT_SPI
+
+        SPI.begin();
+        SET_OUTPUT(DIGIPOTSS_PIN);
+
+        LOOP_L_N(i, COUNT(motor_current_setting))
+          set_digipot_current(i, motor_current_setting[i]);
+
+      #elif HAS_MOTOR_CURRENT_PWM
+
+        #if PIN_EXISTS(MOTOR_CURRENT_PWM_X)
+          SET_PWM(MOTOR_CURRENT_PWM_X_PIN);
+        #endif
+        #if PIN_EXISTS(MOTOR_CURRENT_PWM_Y)
+          SET_PWM(MOTOR_CURRENT_PWM_Y_PIN);
+        #endif
+        #if PIN_EXISTS(MOTOR_CURRENT_PWM_XY)
+          SET_PWM(MOTOR_CURRENT_PWM_XY_PIN);
+        #endif
+        #if PIN_EXISTS(MOTOR_CURRENT_PWM_Z)
+          SET_PWM(MOTOR_CURRENT_PWM_Z_PIN);
+        #endif
+        #if PIN_EXISTS(MOTOR_CURRENT_PWM_E)
+          SET_PWM(MOTOR_CURRENT_PWM_E_PIN);
+        #endif
+        #if PIN_EXISTS(MOTOR_CURRENT_PWM_E0)
+          SET_PWM(MOTOR_CURRENT_PWM_E0_PIN);
+        #endif
+        #if PIN_EXISTS(MOTOR_CURRENT_PWM_E1)
+          SET_PWM(MOTOR_CURRENT_PWM_E1_PIN);
+        #endif
+
+        refresh_motor_power();
+
+        // Set Timer5 to 31khz so the PWM of the motor power is as constant as possible. (removes a buzzing noise)
+        #ifdef __AVR__
+          SET_CS5(PRESCALER_1);
+        #endif
+      #endif
+    }
+
+  #endif
+
+#else // PRINTRBOARD_G2
+
+  #include HAL_PATH(../HAL, fastio/G2_PWM.h)
+
+#endif
+
+#if HAS_MICROSTEPS
+
+  /**
+   * Software-controlled Microstepping
+   */
+
+  void Stepper::microstep_init() {
+    #if HAS_X_MS_PINS
+      SET_OUTPUT(X_MS1_PIN);
+      SET_OUTPUT(X_MS2_PIN);
+      #if PIN_EXISTS(X_MS3)
+        SET_OUTPUT(X_MS3_PIN);
+      #endif
+    #endif
+    #if HAS_X2_MS_PINS
+      SET_OUTPUT(X2_MS1_PIN);
+      SET_OUTPUT(X2_MS2_PIN);
+      #if PIN_EXISTS(X2_MS3)
+        SET_OUTPUT(X2_MS3_PIN);
+      #endif
+    #endif
+    #if HAS_Y_MS_PINS
+      SET_OUTPUT(Y_MS1_PIN);
+      SET_OUTPUT(Y_MS2_PIN);
+      #if PIN_EXISTS(Y_MS3)
+        SET_OUTPUT(Y_MS3_PIN);
+      #endif
+    #endif
+    #if HAS_Y2_MS_PINS
+      SET_OUTPUT(Y2_MS1_PIN);
+      SET_OUTPUT(Y2_MS2_PIN);
+      #if PIN_EXISTS(Y2_MS3)
+        SET_OUTPUT(Y2_MS3_PIN);
+      #endif
+    #endif
+    #if HAS_Z_MS_PINS
+      SET_OUTPUT(Z_MS1_PIN);
+      SET_OUTPUT(Z_MS2_PIN);
+      #if PIN_EXISTS(Z_MS3)
+        SET_OUTPUT(Z_MS3_PIN);
+      #endif
+    #endif
+    #if HAS_Z2_MS_PINS
+      SET_OUTPUT(Z2_MS1_PIN);
+      SET_OUTPUT(Z2_MS2_PIN);
+      #if PIN_EXISTS(Z2_MS3)
+        SET_OUTPUT(Z2_MS3_PIN);
+      #endif
+    #endif
+    #if HAS_Z3_MS_PINS
+      SET_OUTPUT(Z3_MS1_PIN);
+      SET_OUTPUT(Z3_MS2_PIN);
+      #if PIN_EXISTS(Z3_MS3)
+        SET_OUTPUT(Z3_MS3_PIN);
+      #endif
+    #endif
+    #if HAS_Z4_MS_PINS
+      SET_OUTPUT(Z4_MS1_PIN);
+      SET_OUTPUT(Z4_MS2_PIN);
+      #if PIN_EXISTS(Z4_MS3)
+        SET_OUTPUT(Z4_MS3_PIN);
+      #endif
+    #endif
+    #if HAS_E0_MS_PINS
+      SET_OUTPUT(E0_MS1_PIN);
+      SET_OUTPUT(E0_MS2_PIN);
+      #if PIN_EXISTS(E0_MS3)
+        SET_OUTPUT(E0_MS3_PIN);
+      #endif
+    #endif
+    #if HAS_E1_MS_PINS
+      SET_OUTPUT(E1_MS1_PIN);
+      SET_OUTPUT(E1_MS2_PIN);
+      #if PIN_EXISTS(E1_MS3)
+        SET_OUTPUT(E1_MS3_PIN);
+      #endif
+    #endif
+    #if HAS_E2_MS_PINS
+      SET_OUTPUT(E2_MS1_PIN);
+      SET_OUTPUT(E2_MS2_PIN);
+      #if PIN_EXISTS(E2_MS3)
+        SET_OUTPUT(E2_MS3_PIN);
+      #endif
+    #endif
+    #if HAS_E3_MS_PINS
+      SET_OUTPUT(E3_MS1_PIN);
+      SET_OUTPUT(E3_MS2_PIN);
+      #if PIN_EXISTS(E3_MS3)
+        SET_OUTPUT(E3_MS3_PIN);
+      #endif
+    #endif
+    #if HAS_E4_MS_PINS
+      SET_OUTPUT(E4_MS1_PIN);
+      SET_OUTPUT(E4_MS2_PIN);
+      #if PIN_EXISTS(E4_MS3)
+        SET_OUTPUT(E4_MS3_PIN);
+      #endif
+    #endif
+    #if HAS_E5_MS_PINS
+      SET_OUTPUT(E5_MS1_PIN);
+      SET_OUTPUT(E5_MS2_PIN);
+      #if PIN_EXISTS(E5_MS3)
+        SET_OUTPUT(E5_MS3_PIN);
+      #endif
+    #endif
+    #if HAS_E6_MS_PINS
+      SET_OUTPUT(E6_MS1_PIN);
+      SET_OUTPUT(E6_MS2_PIN);
+      #if PIN_EXISTS(E6_MS3)
+        SET_OUTPUT(E6_MS3_PIN);
+      #endif
+    #endif
+    #if HAS_E7_MS_PINS
+      SET_OUTPUT(E7_MS1_PIN);
+      SET_OUTPUT(E7_MS2_PIN);
+      #if PIN_EXISTS(E7_MS3)
+        SET_OUTPUT(E7_MS3_PIN);
+      #endif
+    #endif
+
+    static const uint8_t microstep_modes[] = MICROSTEP_MODES;
+    for (uint16_t i = 0; i < COUNT(microstep_modes); i++)
+      microstep_mode(i, microstep_modes[i]);
+  }
+
+  void Stepper::microstep_ms(const uint8_t driver, const int8_t ms1, const int8_t ms2, const int8_t ms3) {
+    if (ms1 >= 0) switch (driver) {
+      #if HAS_X_MS_PINS || HAS_X2_MS_PINS
+        case 0:
+          #if HAS_X_MS_PINS
+            WRITE(X_MS1_PIN, ms1);
+          #endif
+          #if HAS_X2_MS_PINS
+            WRITE(X2_MS1_PIN, ms1);
+          #endif
+          break;
+      #endif
+      #if HAS_Y_MS_PINS || HAS_Y2_MS_PINS
+        case 1:
+          #if HAS_Y_MS_PINS
+            WRITE(Y_MS1_PIN, ms1);
+          #endif
+          #if HAS_Y2_MS_PINS
+            WRITE(Y2_MS1_PIN, ms1);
+          #endif
+          break;
+      #endif
+      #if HAS_SOME_Z_MS_PINS
+        case 2:
+          #if HAS_Z_MS_PINS
+            WRITE(Z_MS1_PIN, ms1);
+          #endif
+          #if HAS_Z2_MS_PINS
+            WRITE(Z2_MS1_PIN, ms1);
+          #endif
+          #if HAS_Z3_MS_PINS
+            WRITE(Z3_MS1_PIN, ms1);
+          #endif
+          #if HAS_Z4_MS_PINS
+            WRITE(Z4_MS1_PIN, ms1);
+          #endif
+          break;
+      #endif
+      #if HAS_E0_MS_PINS
+        case  3: WRITE(E0_MS1_PIN, ms1); break;
+      #endif
+      #if HAS_E1_MS_PINS
+        case  4: WRITE(E1_MS1_PIN, ms1); break;
+      #endif
+      #if HAS_E2_MS_PINS
+        case  5: WRITE(E2_MS1_PIN, ms1); break;
+      #endif
+      #if HAS_E3_MS_PINS
+        case  6: WRITE(E3_MS1_PIN, ms1); break;
+      #endif
+      #if HAS_E4_MS_PINS
+        case  7: WRITE(E4_MS1_PIN, ms1); break;
+      #endif
+      #if HAS_E5_MS_PINS
+        case  8: WRITE(E5_MS1_PIN, ms1); break;
+      #endif
+      #if HAS_E6_MS_PINS
+        case  9: WRITE(E6_MS1_PIN, ms1); break;
+      #endif
+      #if HAS_E7_MS_PINS
+        case 10: WRITE(E7_MS1_PIN, ms1); break;
+      #endif
+    }
+    if (ms2 >= 0) switch (driver) {
+      #if HAS_X_MS_PINS || HAS_X2_MS_PINS
+        case 0:
+          #if HAS_X_MS_PINS
+            WRITE(X_MS2_PIN, ms2);
+          #endif
+          #if HAS_X2_MS_PINS
+            WRITE(X2_MS2_PIN, ms2);
+          #endif
+          break;
+      #endif
+      #if HAS_Y_MS_PINS || HAS_Y2_MS_PINS
+        case 1:
+          #if HAS_Y_MS_PINS
+            WRITE(Y_MS2_PIN, ms2);
+          #endif
+          #if HAS_Y2_MS_PINS
+            WRITE(Y2_MS2_PIN, ms2);
+          #endif
+          break;
+      #endif
+      #if HAS_SOME_Z_MS_PINS
+        case 2:
+          #if HAS_Z_MS_PINS
+            WRITE(Z_MS2_PIN, ms2);
+          #endif
+          #if HAS_Z2_MS_PINS
+            WRITE(Z2_MS2_PIN, ms2);
+          #endif
+          #if HAS_Z3_MS_PINS
+            WRITE(Z3_MS2_PIN, ms2);
+          #endif
+          #if HAS_Z4_MS_PINS
+            WRITE(Z4_MS2_PIN, ms2);
+          #endif
+          break;
+      #endif
+      #if HAS_E0_MS_PINS
+        case  3: WRITE(E0_MS2_PIN, ms2); break;
+      #endif
+      #if HAS_E1_MS_PINS
+        case  4: WRITE(E1_MS2_PIN, ms2); break;
+      #endif
+      #if HAS_E2_MS_PINS
+        case  5: WRITE(E2_MS2_PIN, ms2); break;
+      #endif
+      #if HAS_E3_MS_PINS
+        case  6: WRITE(E3_MS2_PIN, ms2); break;
+      #endif
+      #if HAS_E4_MS_PINS
+        case  7: WRITE(E4_MS2_PIN, ms2); break;
+      #endif
+      #if HAS_E5_MS_PINS
+        case  8: WRITE(E5_MS2_PIN, ms2); break;
+      #endif
+      #if HAS_E6_MS_PINS
+        case  9: WRITE(E6_MS2_PIN, ms2); break;
+      #endif
+      #if HAS_E7_MS_PINS
+        case 10: WRITE(E7_MS2_PIN, ms2); break;
+      #endif
+    }
+    if (ms3 >= 0) switch (driver) {
+      #if HAS_X_MS_PINS || HAS_X2_MS_PINS
+        case 0:
+          #if HAS_X_MS_PINS && PIN_EXISTS(X_MS3)
+            WRITE(X_MS3_PIN, ms3);
+          #endif
+          #if HAS_X2_MS_PINS && PIN_EXISTS(X2_MS3)
+            WRITE(X2_MS3_PIN, ms3);
+          #endif
+          break;
+      #endif
+      #if HAS_Y_MS_PINS || HAS_Y2_MS_PINS
+        case 1:
+          #if HAS_Y_MS_PINS && PIN_EXISTS(Y_MS3)
+            WRITE(Y_MS3_PIN, ms3);
+          #endif
+          #if HAS_Y2_MS_PINS && PIN_EXISTS(Y2_MS3)
+            WRITE(Y2_MS3_PIN, ms3);
+          #endif
+          break;
+      #endif
+      #if HAS_SOME_Z_MS_PINS
+        case 2:
+          #if HAS_Z_MS_PINS && PIN_EXISTS(Z_MS3)
+            WRITE(Z_MS3_PIN, ms3);
+          #endif
+          #if HAS_Z2_MS_PINS && PIN_EXISTS(Z2_MS3)
+            WRITE(Z2_MS3_PIN, ms3);
+          #endif
+          #if HAS_Z3_MS_PINS && PIN_EXISTS(Z3_MS3)
+            WRITE(Z3_MS3_PIN, ms3);
+          #endif
+          #if HAS_Z4_MS_PINS && PIN_EXISTS(Z4_MS3)
+            WRITE(Z4_MS3_PIN, ms3);
+          #endif
+          break;
+      #endif
+      #if HAS_E0_MS_PINS && PIN_EXISTS(E0_MS3)
+        case  3: WRITE(E0_MS3_PIN, ms3); break;
+      #endif
+      #if HAS_E1_MS_PINS && PIN_EXISTS(E1_MS3)
+        case  4: WRITE(E1_MS3_PIN, ms3); break;
+      #endif
+      #if HAS_E2_MS_PINS && PIN_EXISTS(E2_MS3)
+        case  5: WRITE(E2_MS3_PIN, ms3); break;
+      #endif
+      #if HAS_E3_MS_PINS && PIN_EXISTS(E3_MS3)
+        case  6: WRITE(E3_MS3_PIN, ms3); break;
+      #endif
+      #if HAS_E4_MS_PINS && PIN_EXISTS(E4_MS3)
+        case  7: WRITE(E4_MS3_PIN, ms3); break;
+      #endif
+      #if HAS_E5_MS_PINS && PIN_EXISTS(E5_MS3)
+        case  8: WRITE(E5_MS3_PIN, ms3); break;
+      #endif
+      #if HAS_E6_MS_PINS && PIN_EXISTS(E6_MS3)
+        case  9: WRITE(E6_MS3_PIN, ms3); break;
+      #endif
+      #if HAS_E7_MS_PINS && PIN_EXISTS(E7_MS3)
+        case 10: WRITE(E7_MS3_PIN, ms3); break;
+      #endif
+    }
+  }
+
+  void Stepper::microstep_mode(const uint8_t driver, const uint8_t stepping_mode) {
+    switch (stepping_mode) {
+      #if HAS_MICROSTEP1
+        case 1: microstep_ms(driver, MICROSTEP1); break;
+      #endif
+      #if HAS_MICROSTEP2
+        case 2: microstep_ms(driver, MICROSTEP2); break;
+      #endif
+      #if HAS_MICROSTEP4
+        case 4: microstep_ms(driver, MICROSTEP4); break;
+      #endif
+      #if HAS_MICROSTEP8
+        case 8: microstep_ms(driver, MICROSTEP8); break;
+      #endif
+      #if HAS_MICROSTEP16
+        case 16: microstep_ms(driver, MICROSTEP16); break;
+      #endif
+      #if HAS_MICROSTEP32
+        case 32: microstep_ms(driver, MICROSTEP32); break;
+      #endif
+      #if HAS_MICROSTEP64
+        case 64: microstep_ms(driver, MICROSTEP64); break;
+      #endif
+      #if HAS_MICROSTEP128
+        case 128: microstep_ms(driver, MICROSTEP128); break;
+      #endif
+
+      default: SERIAL_ERROR_MSG("Microsteps unavailable"); break;
+    }
+  }
+
+  void Stepper::microstep_readings() {
+    #define PIN_CHAR(P) SERIAL_CHAR('0' + READ(P##_PIN))
+    #define MS_LINE(A)  do{ SERIAL_ECHOPGM(" " STRINGIFY(A) ":"); PIN_CHAR(A##_MS1); PIN_CHAR(A##_MS2); }while(0)
+    SERIAL_ECHOPGM("MS1|2|3 Pins");
+    #if HAS_X_MS_PINS
+      MS_LINE(X);
+      #if PIN_EXISTS(X_MS3)
+        PIN_CHAR(X_MS3);
+      #endif
+    #endif
+    #if HAS_Y_MS_PINS
+      MS_LINE(Y);
+      #if PIN_EXISTS(Y_MS3)
+        PIN_CHAR(Y_MS3);
+      #endif
+    #endif
+    #if HAS_Z_MS_PINS
+      MS_LINE(Z);
+      #if PIN_EXISTS(Z_MS3)
+        PIN_CHAR(Z_MS3);
+      #endif
+    #endif
+    #if HAS_E0_MS_PINS
+      MS_LINE(E0);
+      #if PIN_EXISTS(E0_MS3)
+        PIN_CHAR(E0_MS3);
+      #endif
+    #endif
+    #if HAS_E1_MS_PINS
+      MS_LINE(E1);
+      #if PIN_EXISTS(E1_MS3)
+        PIN_CHAR(E1_MS3);
+      #endif
+    #endif
+    #if HAS_E2_MS_PINS
+      MS_LINE(E2);
+      #if PIN_EXISTS(E2_MS3)
+        PIN_CHAR(E2_MS3);
+      #endif
+    #endif
+    #if HAS_E3_MS_PINS
+      MS_LINE(E3);
+      #if PIN_EXISTS(E3_MS3)
+        PIN_CHAR(E3_MS3);
+      #endif
+    #endif
+    #if HAS_E4_MS_PINS
+      MS_LINE(E4);
+      #if PIN_EXISTS(E4_MS3)
+        PIN_CHAR(E4_MS3);
+      #endif
+    #endif
+    #if HAS_E5_MS_PINS
+      MS_LINE(E5);
+      #if PIN_EXISTS(E5_MS3)
+        PIN_CHAR(E5_MS3);
+      #endif
+    #endif
+    #if HAS_E6_MS_PINS
+      MS_LINE(E6);
+      #if PIN_EXISTS(E6_MS3)
+        PIN_CHAR(E6_MS3);
+      #endif
+    #endif
+    #if HAS_E7_MS_PINS
+      MS_LINE(E7);
+      #if PIN_EXISTS(E7_MS3)
+        PIN_CHAR(E7_MS3);
+      #endif
+    #endif
+    SERIAL_EOL();
+  }
+
+#endif // HAS_MICROSTEPS
diff --git a/Marlin/src/module/stepper.h b/Marlin/src/module/stepper.h
new file mode 100644
index 0000000..639a1b2
--- /dev/null
+++ b/Marlin/src/module/stepper.h
@@ -0,0 +1,607 @@
+/**
+ * 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
+
+/**
+ * stepper.h - stepper motor driver: executes motion plans of planner.c using the stepper motors
+ * Derived from Grbl
+ *
+ * Copyright (c) 2009-2011 Simen Svale Skogsrud
+ *
+ * Grbl is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Grbl is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Grbl.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+#include "../inc/MarlinConfig.h"
+
+#include "planner.h"
+#include "stepper/indirection.h"
+#ifdef __AVR__
+  #include "speed_lookuptable.h"
+#endif
+
+// Disable multiple steps per ISR
+//#define DISABLE_MULTI_STEPPING
+
+//
+// Estimate the amount of time the Stepper ISR will take to execute
+//
+
+/**
+ * The method of calculating these cycle-constants is unclear.
+ * Most of them are no longer used directly for pulse timing, and exist
+ * only to estimate a maximum step rate based on the user's configuration.
+ * As 32-bit processors continue to diverge, maintaining cycle counts
+ * will become increasingly difficult and error-prone.
+ */
+
+#ifdef CPU_32_BIT
+  /**
+   * Duration of START_TIMED_PULSE
+   *
+   * ...as measured on an LPC1768 with a scope and converted to cycles.
+   * Not applicable to other 32-bit processors, but as long as others
+   * take longer, pulses will be longer. For example the SKR Pro
+   * (stm32f407zgt6) requires ~60 cyles.
+   */
+  #define TIMER_READ_ADD_AND_STORE_CYCLES 34UL
+
+  // The base ISR takes 792 cycles
+  #define ISR_BASE_CYCLES  792UL
+
+  // Linear advance base time is 64 cycles
+  #if ENABLED(LIN_ADVANCE)
+    #define ISR_LA_BASE_CYCLES 64UL
+  #else
+    #define ISR_LA_BASE_CYCLES 0UL
+  #endif
+
+  // S curve interpolation adds 40 cycles
+  #if ENABLED(S_CURVE_ACCELERATION)
+    #define ISR_S_CURVE_CYCLES 40UL
+  #else
+    #define ISR_S_CURVE_CYCLES 0UL
+  #endif
+
+  // Stepper Loop base cycles
+  #define ISR_LOOP_BASE_CYCLES 4UL
+
+  // To start the step pulse, in the worst case takes
+  #define ISR_START_STEPPER_CYCLES 13UL
+
+  // And each stepper (start + stop pulse) takes in worst case
+  #define ISR_STEPPER_CYCLES 16UL
+
+#else
+  // Cycles to perform actions in START_TIMED_PULSE
+  #define TIMER_READ_ADD_AND_STORE_CYCLES 13UL
+
+  // The base ISR takes 752 cycles
+  #define ISR_BASE_CYCLES  752UL
+
+  // Linear advance base time is 32 cycles
+  #if ENABLED(LIN_ADVANCE)
+    #define ISR_LA_BASE_CYCLES 32UL
+  #else
+    #define ISR_LA_BASE_CYCLES 0UL
+  #endif
+
+  // S curve interpolation adds 160 cycles
+  #if ENABLED(S_CURVE_ACCELERATION)
+    #define ISR_S_CURVE_CYCLES 160UL
+  #else
+    #define ISR_S_CURVE_CYCLES 0UL
+  #endif
+
+  // Stepper Loop base cycles
+  #define ISR_LOOP_BASE_CYCLES 32UL
+
+  // To start the step pulse, in the worst case takes
+  #define ISR_START_STEPPER_CYCLES 57UL
+
+  // And each stepper (start + stop pulse) takes in worst case
+  #define ISR_STEPPER_CYCLES 88UL
+
+#endif
+
+// Add time for each stepper
+#if HAS_X_STEP
+  #define ISR_X_STEPPER_CYCLES       ISR_STEPPER_CYCLES
+#else
+  #define ISR_X_STEPPER_CYCLES       0UL
+#endif
+#if HAS_Y_STEP
+  #define ISR_Y_STEPPER_CYCLES       ISR_STEPPER_CYCLES
+#else
+  #define ISR_START_Y_STEPPER_CYCLES 0UL
+  #define ISR_Y_STEPPER_CYCLES       0UL
+#endif
+#if HAS_Z_STEP
+  #define ISR_Z_STEPPER_CYCLES       ISR_STEPPER_CYCLES
+#else
+  #define ISR_Z_STEPPER_CYCLES       0UL
+#endif
+
+// E is always interpolated, even for mixing extruders
+#define ISR_E_STEPPER_CYCLES         ISR_STEPPER_CYCLES
+
+// If linear advance is disabled, the loop also handles them
+#if DISABLED(LIN_ADVANCE) && ENABLED(MIXING_EXTRUDER)
+  #define ISR_MIXING_STEPPER_CYCLES ((MIXING_STEPPERS) * (ISR_STEPPER_CYCLES))
+#else
+  #define ISR_MIXING_STEPPER_CYCLES  0UL
+#endif
+
+// And the total minimum loop time, not including the base
+#define MIN_ISR_LOOP_CYCLES (ISR_X_STEPPER_CYCLES + ISR_Y_STEPPER_CYCLES + ISR_Z_STEPPER_CYCLES + ISR_E_STEPPER_CYCLES + ISR_MIXING_STEPPER_CYCLES)
+
+// Calculate the minimum MPU cycles needed per pulse to enforce, limited to the max stepper rate
+#define _MIN_STEPPER_PULSE_CYCLES(N) _MAX(uint32_t((F_CPU) / (MAXIMUM_STEPPER_RATE)), ((F_CPU) / 500000UL) * (N))
+#if MINIMUM_STEPPER_PULSE
+  #define MIN_STEPPER_PULSE_CYCLES _MIN_STEPPER_PULSE_CYCLES(uint32_t(MINIMUM_STEPPER_PULSE))
+#elif HAS_DRIVER(LV8729)
+  #define MIN_STEPPER_PULSE_CYCLES uint32_t((((F_CPU) - 1) / 2000000) + 1) // 0.5µs, aka 500ns
+#else
+  #define MIN_STEPPER_PULSE_CYCLES _MIN_STEPPER_PULSE_CYCLES(1UL)
+#endif
+
+// Calculate the minimum pulse times (high and low)
+#if MINIMUM_STEPPER_PULSE && MAXIMUM_STEPPER_RATE
+  constexpr uint32_t _MIN_STEP_PERIOD_NS = 1000000000UL / MAXIMUM_STEPPER_RATE;
+  constexpr uint32_t _MIN_PULSE_HIGH_NS = 1000UL * MINIMUM_STEPPER_PULSE;
+  constexpr uint32_t _MIN_PULSE_LOW_NS = _MAX((_MIN_STEP_PERIOD_NS - _MIN(_MIN_STEP_PERIOD_NS, _MIN_PULSE_HIGH_NS)), _MIN_PULSE_HIGH_NS);
+#elif MINIMUM_STEPPER_PULSE
+  // Assume 50% duty cycle
+  constexpr uint32_t _MIN_PULSE_HIGH_NS = 1000UL * MINIMUM_STEPPER_PULSE;
+  constexpr uint32_t _MIN_PULSE_LOW_NS = _MIN_PULSE_HIGH_NS;
+#elif MAXIMUM_STEPPER_RATE
+  // Assume 50% duty cycle
+  constexpr uint32_t _MIN_PULSE_HIGH_NS = 500000000UL / MAXIMUM_STEPPER_RATE;
+  constexpr uint32_t _MIN_PULSE_LOW_NS = _MIN_PULSE_HIGH_NS;
+#else
+  #error "Expected at least one of MINIMUM_STEPPER_PULSE or MAXIMUM_STEPPER_RATE to be defined"
+#endif
+
+// But the user could be enforcing a minimum time, so the loop time is
+#define ISR_LOOP_CYCLES (ISR_LOOP_BASE_CYCLES + _MAX(MIN_STEPPER_PULSE_CYCLES, MIN_ISR_LOOP_CYCLES))
+
+// If linear advance is enabled, then it is handled separately
+#if ENABLED(LIN_ADVANCE)
+
+  // Estimate the minimum LA loop time
+  #if ENABLED(MIXING_EXTRUDER) // ToDo: ???
+    // HELP ME: What is what?
+    // Directions are set up for MIXING_STEPPERS - like before.
+    // Finding the right stepper may last up to MIXING_STEPPERS loops in get_next_stepper().
+    //   These loops are a bit faster than advancing a bresenham counter.
+    // Always only one e-stepper is stepped.
+    #define MIN_ISR_LA_LOOP_CYCLES ((MIXING_STEPPERS) * (ISR_STEPPER_CYCLES))
+  #else
+    #define MIN_ISR_LA_LOOP_CYCLES ISR_STEPPER_CYCLES
+  #endif
+
+  // And the real loop time
+  #define ISR_LA_LOOP_CYCLES _MAX(MIN_STEPPER_PULSE_CYCLES, MIN_ISR_LA_LOOP_CYCLES)
+
+#else
+  #define ISR_LA_LOOP_CYCLES 0UL
+#endif
+
+// Now estimate the total ISR execution time in cycles given a step per ISR multiplier
+#define ISR_EXECUTION_CYCLES(R) (((ISR_BASE_CYCLES + ISR_S_CURVE_CYCLES + (ISR_LOOP_CYCLES) * (R) + ISR_LA_BASE_CYCLES + ISR_LA_LOOP_CYCLES)) / (R))
+
+// The maximum allowable stepping frequency when doing x128-x1 stepping (in Hz)
+#define MAX_STEP_ISR_FREQUENCY_128X ((F_CPU) / ISR_EXECUTION_CYCLES(128))
+#define MAX_STEP_ISR_FREQUENCY_64X  ((F_CPU) / ISR_EXECUTION_CYCLES(64))
+#define MAX_STEP_ISR_FREQUENCY_32X  ((F_CPU) / ISR_EXECUTION_CYCLES(32))
+#define MAX_STEP_ISR_FREQUENCY_16X  ((F_CPU) / ISR_EXECUTION_CYCLES(16))
+#define MAX_STEP_ISR_FREQUENCY_8X   ((F_CPU) / ISR_EXECUTION_CYCLES(8))
+#define MAX_STEP_ISR_FREQUENCY_4X   ((F_CPU) / ISR_EXECUTION_CYCLES(4))
+#define MAX_STEP_ISR_FREQUENCY_2X   ((F_CPU) / ISR_EXECUTION_CYCLES(2))
+#define MAX_STEP_ISR_FREQUENCY_1X   ((F_CPU) / ISR_EXECUTION_CYCLES(1))
+
+// The minimum step ISR rate used by ADAPTIVE_STEP_SMOOTHING to target 50% CPU usage
+// This does not account for the possibility of multi-stepping.
+// Perhaps DISABLE_MULTI_STEPPING should be required with ADAPTIVE_STEP_SMOOTHING.
+#define MIN_STEP_ISR_FREQUENCY (MAX_STEP_ISR_FREQUENCY_1X / 2)
+
+//
+// Stepper class definition
+//
+class Stepper {
+
+  public:
+
+    #if EITHER(HAS_EXTRA_ENDSTOPS, Z_STEPPER_AUTO_ALIGN)
+      static bool separate_multi_axis;
+    #endif
+
+    #if HAS_MOTOR_CURRENT_SPI || HAS_MOTOR_CURRENT_PWM
+      #if HAS_MOTOR_CURRENT_PWM
+        #ifndef PWM_MOTOR_CURRENT
+          #define PWM_MOTOR_CURRENT DEFAULT_PWM_MOTOR_CURRENT
+        #endif
+        #define MOTOR_CURRENT_COUNT 3
+      #elif HAS_MOTOR_CURRENT_SPI
+        static constexpr uint32_t digipot_count[] = DIGIPOT_MOTOR_CURRENT;
+        #define MOTOR_CURRENT_COUNT COUNT(Stepper::digipot_count)
+      #endif
+      static bool initialized;
+      static uint32_t motor_current_setting[MOTOR_CURRENT_COUNT]; // Initialized by settings.load()
+    #endif
+
+    // Last-moved extruder, as set when the last movement was fetched from planner
+    #if HAS_MULTI_EXTRUDER
+      static uint8_t last_moved_extruder;
+    #else
+      static constexpr uint8_t last_moved_extruder = 0;
+    #endif
+
+  private:
+
+    static block_t* current_block;          // A pointer to the block currently being traced
+
+    static uint8_t last_direction_bits,     // The next stepping-bits to be output
+                   axis_did_move;           // Last Movement in the given direction is not null, as computed when the last movement was fetched from planner
+
+    static bool abort_current_block;        // Signals to the stepper that current block should be aborted
+
+    #if ENABLED(X_DUAL_ENDSTOPS)
+      static bool locked_X_motor, locked_X2_motor;
+    #endif
+    #if ENABLED(Y_DUAL_ENDSTOPS)
+      static bool locked_Y_motor, locked_Y2_motor;
+    #endif
+    #if EITHER(Z_MULTI_ENDSTOPS, Z_STEPPER_AUTO_ALIGN)
+      static bool locked_Z_motor, locked_Z2_motor
+                  #if NUM_Z_STEPPER_DRIVERS >= 3
+                    , locked_Z3_motor
+                    #if NUM_Z_STEPPER_DRIVERS >= 4
+                      , locked_Z4_motor
+                    #endif
+                  #endif
+                  ;
+    #endif
+
+    static uint32_t acceleration_time, deceleration_time; // time measured in Stepper Timer ticks
+    static uint8_t steps_per_isr;         // Count of steps to perform per Stepper ISR call
+
+    #if ENABLED(ADAPTIVE_STEP_SMOOTHING)
+      static uint8_t oversampling_factor; // Oversampling factor (log2(multiplier)) to increase temporal resolution of axis
+    #else
+      static constexpr uint8_t oversampling_factor = 0;
+    #endif
+
+    // Delta error variables for the Bresenham line tracer
+    static xyze_long_t delta_error;
+    static xyze_ulong_t advance_dividend;
+    static uint32_t advance_divisor,
+                    step_events_completed,  // The number of step events executed in the current block
+                    accelerate_until,       // The point from where we need to stop acceleration
+                    decelerate_after,       // The point from where we need to start decelerating
+                    step_event_count;       // The total event count for the current block
+
+    #if EITHER(HAS_MULTI_EXTRUDER, MIXING_EXTRUDER)
+      static uint8_t stepper_extruder;
+    #else
+      static constexpr uint8_t stepper_extruder = 0;
+    #endif
+
+    #if ENABLED(S_CURVE_ACCELERATION)
+      static int32_t bezier_A,     // A coefficient in Bézier speed curve
+                     bezier_B,     // B coefficient in Bézier speed curve
+                     bezier_C;     // C coefficient in Bézier speed curve
+      static uint32_t bezier_F,    // F coefficient in Bézier speed curve
+                      bezier_AV;   // AV coefficient in Bézier speed curve
+      #ifdef __AVR__
+        static bool A_negative;    // If A coefficient was negative
+      #endif
+      static bool bezier_2nd_half; // If Bézier curve has been initialized or not
+    #endif
+
+    #if ENABLED(LIN_ADVANCE)
+      static constexpr uint32_t LA_ADV_NEVER = 0xFFFFFFFF;
+      static uint32_t nextAdvanceISR, LA_isr_rate;
+      static uint16_t LA_current_adv_steps, LA_final_adv_steps, LA_max_adv_steps; // Copy from current executed block. Needed because current_block is set to NULL "too early".
+      static int8_t LA_steps;
+      static bool LA_use_advance_lead;
+    #endif
+
+    #if ENABLED(INTEGRATED_BABYSTEPPING)
+      static constexpr uint32_t BABYSTEP_NEVER = 0xFFFFFFFF;
+      static uint32_t nextBabystepISR;
+    #endif
+
+    #if ENABLED(DIRECT_STEPPING)
+      static page_step_state_t page_step_state;
+    #endif
+
+    static int32_t ticks_nominal;
+    #if DISABLED(S_CURVE_ACCELERATION)
+      static uint32_t acc_step_rate; // needed for deceleration start point
+    #endif
+
+    // Exact steps at which an endstop was triggered
+    static xyz_long_t endstops_trigsteps;
+
+    // Positions of stepper motors, in step units
+    static xyze_long_t count_position;
+
+    // Current stepper motor directions (+1 or -1)
+    static xyze_int8_t count_direction;
+
+    #if ENABLED(LASER_POWER_INLINE_TRAPEZOID)
+
+      typedef struct {
+        bool enabled;       // Trapezoid needed flag (i.e., laser on, planner in control)
+        uint8_t cur_power;  // Current laser power
+        bool cruise_set;    // Power set up for cruising?
+
+        #if DISABLED(LASER_POWER_INLINE_TRAPEZOID_CONT)
+          uint32_t last_step_count, // Step count from the last update
+                   acc_step_count;  // Bresenham counter for laser accel/decel
+        #else
+          uint16_t till_update;     // Countdown to the next update
+        #endif
+      } stepper_laser_t;
+
+      static stepper_laser_t laser_trap;
+
+    #endif
+
+  public:
+    // Initialize stepper hardware
+    static void init();
+
+    // Interrupt Service Routine and phases
+
+    // The stepper subsystem goes to sleep when it runs out of things to execute.
+    // Call this to notify the subsystem that it is time to go to work.
+    static inline void wake_up() { ENABLE_STEPPER_DRIVER_INTERRUPT(); }
+
+    static inline bool is_awake() { return STEPPER_ISR_ENABLED(); }
+
+    static inline bool suspend() {
+      const bool awake = is_awake();
+      if (awake) DISABLE_STEPPER_DRIVER_INTERRUPT();
+      return awake;
+    }
+
+    // The ISR scheduler
+    static void isr();
+
+    // The stepper pulse ISR phase
+    static void pulse_phase_isr();
+
+    // The stepper block processing ISR phase
+    static uint32_t block_phase_isr();
+
+    #if ENABLED(LIN_ADVANCE)
+      // The Linear advance ISR phase
+      static uint32_t advance_isr();
+      FORCE_INLINE static void initiateLA() { nextAdvanceISR = 0; }
+    #endif
+
+    #if ENABLED(INTEGRATED_BABYSTEPPING)
+      // The Babystepping ISR phase
+      static uint32_t babystepping_isr();
+      FORCE_INLINE static void initiateBabystepping() {
+        if (nextBabystepISR == BABYSTEP_NEVER) {
+          nextBabystepISR = 0;
+          wake_up();
+        }
+      }
+    #endif
+
+    // Check if the given block is busy or not - Must not be called from ISR contexts
+    static bool is_block_busy(const block_t* const block);
+
+    // Get the position of a stepper, in steps
+    static int32_t position(const AxisEnum axis);
+
+    // Set the current position in steps
+    static void set_position(const int32_t &a, const int32_t &b, const int32_t &c, const int32_t &e);
+    static inline void set_position(const xyze_long_t &abce) { set_position(abce.a, abce.b, abce.c, abce.e); }
+    static void set_axis_position(const AxisEnum a, const int32_t &v);
+
+    // Report the positions of the steppers, in steps
+    static void report_a_position(const xyz_long_t &pos);
+    static void report_positions();
+
+    // Discard current block and free any resources
+    FORCE_INLINE static void discard_current_block() {
+      #if ENABLED(DIRECT_STEPPING)
+        if (IS_PAGE(current_block))
+          page_manager.free_page(current_block->page_idx);
+      #endif
+      current_block = nullptr;
+      axis_did_move = 0;
+      planner.release_current_block();
+    }
+
+    // Quickly stop all steppers
+    FORCE_INLINE static void quick_stop() { abort_current_block = true; }
+
+    // The direction of a single motor
+    FORCE_INLINE static bool motor_direction(const AxisEnum axis) { return TEST(last_direction_bits, axis); }
+
+    // The last movement direction was not null on the specified axis. Note that motor direction is not necessarily the same.
+    FORCE_INLINE static bool axis_is_moving(const AxisEnum axis) { return TEST(axis_did_move, axis); }
+
+    // Handle a triggered endstop
+    static void endstop_triggered(const AxisEnum axis);
+
+    // Triggered position of an axis in steps
+    static int32_t triggered_position(const AxisEnum axis);
+
+    #if HAS_MOTOR_CURRENT_SPI || HAS_MOTOR_CURRENT_PWM
+      static void set_digipot_value_spi(const int16_t address, const int16_t value);
+      static void set_digipot_current(const uint8_t driver, const int16_t current);
+    #endif
+
+    #if HAS_MICROSTEPS
+      static void microstep_ms(const uint8_t driver, const int8_t ms1, const int8_t ms2, const int8_t ms3);
+      static void microstep_mode(const uint8_t driver, const uint8_t stepping);
+      static void microstep_readings();
+    #endif
+
+    #if EITHER(HAS_EXTRA_ENDSTOPS, Z_STEPPER_AUTO_ALIGN)
+      FORCE_INLINE static void set_separate_multi_axis(const bool state) { separate_multi_axis = state; }
+    #endif
+    #if ENABLED(X_DUAL_ENDSTOPS)
+      FORCE_INLINE static void set_x_lock(const bool state) { locked_X_motor = state; }
+      FORCE_INLINE static void set_x2_lock(const bool state) { locked_X2_motor = state; }
+    #endif
+    #if ENABLED(Y_DUAL_ENDSTOPS)
+      FORCE_INLINE static void set_y_lock(const bool state) { locked_Y_motor = state; }
+      FORCE_INLINE static void set_y2_lock(const bool state) { locked_Y2_motor = state; }
+    #endif
+    #if EITHER(Z_MULTI_ENDSTOPS, Z_STEPPER_AUTO_ALIGN)
+      FORCE_INLINE static void set_z1_lock(const bool state) { locked_Z_motor = state; }
+      FORCE_INLINE static void set_z2_lock(const bool state) { locked_Z2_motor = state; }
+      #if NUM_Z_STEPPER_DRIVERS >= 3
+        FORCE_INLINE static void set_z3_lock(const bool state) { locked_Z3_motor = state; }
+        #if NUM_Z_STEPPER_DRIVERS >= 4
+          FORCE_INLINE static void set_z4_lock(const bool state) { locked_Z4_motor = state; }
+        #endif
+      #endif
+      static inline void set_all_z_lock(const bool lock, const int8_t except=-1) {
+        set_z1_lock(lock ^ (except == 0));
+        set_z2_lock(lock ^ (except == 1));
+        #if NUM_Z_STEPPER_DRIVERS >= 3
+          set_z3_lock(lock ^ (except == 2));
+          #if NUM_Z_STEPPER_DRIVERS >= 4
+            set_z4_lock(lock ^ (except == 3));
+          #endif
+        #endif
+      }
+    #endif
+
+    #if ENABLED(BABYSTEPPING)
+      static void do_babystep(const AxisEnum axis, const bool direction); // perform a short step with a single stepper motor, outside of any convention
+    #endif
+
+    #if HAS_MOTOR_CURRENT_PWM
+      static void refresh_motor_power();
+    #endif
+
+    // Update direction states for all steppers
+    static void set_directions();
+
+    // Set direction bits and update all stepper DIR states
+    static void set_directions(const uint8_t bits) {
+      last_direction_bits = bits;
+      set_directions();
+    }
+
+  private:
+
+    // Set the current position in steps
+    static void _set_position(const int32_t &a, const int32_t &b, const int32_t &c, const int32_t &e);
+    FORCE_INLINE static void _set_position(const abce_long_t &spos) { _set_position(spos.a, spos.b, spos.c, spos.e); }
+
+    FORCE_INLINE static uint32_t calc_timer_interval(uint32_t step_rate, uint8_t* loops) {
+      uint32_t timer;
+
+      // Scale the frequency, as requested by the caller
+      step_rate <<= oversampling_factor;
+
+      uint8_t multistep = 1;
+      #if DISABLED(DISABLE_MULTI_STEPPING)
+
+        // The stepping frequency limits for each multistepping rate
+        static const uint32_t limit[] PROGMEM = {
+          (  MAX_STEP_ISR_FREQUENCY_1X     ),
+          (  MAX_STEP_ISR_FREQUENCY_2X >> 1),
+          (  MAX_STEP_ISR_FREQUENCY_4X >> 2),
+          (  MAX_STEP_ISR_FREQUENCY_8X >> 3),
+          ( MAX_STEP_ISR_FREQUENCY_16X >> 4),
+          ( MAX_STEP_ISR_FREQUENCY_32X >> 5),
+          ( MAX_STEP_ISR_FREQUENCY_64X >> 6),
+          (MAX_STEP_ISR_FREQUENCY_128X >> 7)
+        };
+
+        // Select the proper multistepping
+        uint8_t idx = 0;
+        while (idx < 7 && step_rate > (uint32_t)pgm_read_dword(&limit[idx])) {
+          step_rate >>= 1;
+          multistep <<= 1;
+          ++idx;
+        };
+      #else
+        NOMORE(step_rate, uint32_t(MAX_STEP_ISR_FREQUENCY_1X));
+      #endif
+      *loops = multistep;
+
+      #ifdef CPU_32_BIT
+        // In case of high-performance processor, it is able to calculate in real-time
+        timer = uint32_t(STEPPER_TIMER_RATE) / step_rate;
+      #else
+        constexpr uint32_t min_step_rate = (F_CPU) / 500000U;
+        NOLESS(step_rate, min_step_rate);
+        step_rate -= min_step_rate; // Correct for minimal speed
+        if (step_rate >= (8 * 256)) { // higher step rate
+          const uint8_t tmp_step_rate = (step_rate & 0x00FF);
+          const uint16_t table_address = (uint16_t)&speed_lookuptable_fast[(uint8_t)(step_rate >> 8)][0],
+                         gain = (uint16_t)pgm_read_word(table_address + 2);
+          timer = MultiU16X8toH16(tmp_step_rate, gain);
+          timer = (uint16_t)pgm_read_word(table_address) - timer;
+        }
+        else { // lower step rates
+          uint16_t table_address = (uint16_t)&speed_lookuptable_slow[0][0];
+          table_address += ((step_rate) >> 1) & 0xFFFC;
+          timer = (uint16_t)pgm_read_word(table_address)
+                - (((uint16_t)pgm_read_word(table_address + 2) * (uint8_t)(step_rate & 0x0007)) >> 3);
+        }
+        // (there is no need to limit the timer value here. All limits have been
+        // applied above, and AVR is able to keep up at 30khz Stepping ISR rate)
+      #endif
+
+      return timer;
+    }
+
+    #if ENABLED(S_CURVE_ACCELERATION)
+      static void _calc_bezier_curve_coeffs(const int32_t v0, const int32_t v1, const uint32_t av);
+      static int32_t _eval_bezier_curve(const uint32_t curr_step);
+    #endif
+
+    #if HAS_MOTOR_CURRENT_SPI || HAS_MOTOR_CURRENT_PWM
+      static void digipot_init();
+    #endif
+
+    #if HAS_MICROSTEPS
+      static void microstep_init();
+    #endif
+
+};
+
+extern Stepper stepper;
diff --git a/Marlin/src/module/stepper/L64xx.cpp b/Marlin/src/module/stepper/L64xx.cpp
new file mode 100644
index 0000000..3e2bf09
--- /dev/null
+++ b/Marlin/src/module/stepper/L64xx.cpp
@@ -0,0 +1,225 @@
+/**
+ * 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/>.
+ *
+ */
+
+/**
+ * stepper/L64xx.cpp
+ * Stepper driver indirection for L64XX drivers
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if HAS_L64XX
+
+#include "L64xx.h"
+
+#if AXIS_IS_L64XX(X)
+  L64XX_CLASS(X) stepperX(L6470_CHAIN_SS_PIN);
+#endif
+#if AXIS_IS_L64XX(X2)
+  L64XX_CLASS(X2) stepperX2(L6470_CHAIN_SS_PIN);
+#endif
+#if AXIS_IS_L64XX(Y)
+  L64XX_CLASS(Y) stepperY(L6470_CHAIN_SS_PIN);
+#endif
+#if AXIS_IS_L64XX(Y2)
+  L64XX_CLASS(Y2) stepperY2(L6470_CHAIN_SS_PIN);
+#endif
+#if AXIS_IS_L64XX(Z)
+  L64XX_CLASS(Z) stepperZ(L6470_CHAIN_SS_PIN);
+#endif
+#if AXIS_IS_L64XX(Z2)
+  L64XX_CLASS(Z2) stepperZ2(L6470_CHAIN_SS_PIN);
+#endif
+#if AXIS_IS_L64XX(Z3)
+  L64XX_CLASS(Z3) stepperZ3(L6470_CHAIN_SS_PIN);
+#endif
+#if AXIS_IS_L64XX(Z4)
+  L64XX_CLASS(Z4) stepperZ4(L6470_CHAIN_SS_PIN);
+#endif
+#if AXIS_IS_L64XX(E0)
+  L64XX_CLASS(E0) stepperE0(L6470_CHAIN_SS_PIN);
+#endif
+#if AXIS_IS_L64XX(E1)
+  L64XX_CLASS(E1) stepperE1(L6470_CHAIN_SS_PIN);
+#endif
+#if AXIS_IS_L64XX(E2)
+  L64XX_CLASS(E2) stepperE2(L6470_CHAIN_SS_PIN);
+#endif
+#if AXIS_IS_L64XX(E3)
+  L64XX_CLASS(E3) stepperE3(L6470_CHAIN_SS_PIN);
+#endif
+#if AXIS_IS_L64XX(E4)
+  L64XX_CLASS(E4) stepperE4(L6470_CHAIN_SS_PIN);
+#endif
+#if AXIS_IS_L64XX(E5)
+  L64XX_CLASS(E5) stepperE5(L6470_CHAIN_SS_PIN);
+#endif
+#if AXIS_IS_L64XX(E6)
+  L64XX_CLASS(E6) stepperE6(L6470_CHAIN_SS_PIN);
+#endif
+#if AXIS_IS_L64XX(E7)
+  L64XX_CLASS(E7) stepperE7(L6470_CHAIN_SS_PIN);
+#endif
+
+// Not using L64XX class init method because it
+// briefly sends power to the steppers
+
+inline void L6470_init_chip(L64XX &st, const int ms, const int oc, const int sc, const int mv, const int slew_rate) {
+  st.set_handlers(L64xxManager.spi_init, L64xxManager.transfer_single, L64xxManager.transfer_chain);  // specify which external SPI routines to use
+  switch (st.L6470_status_layout) {
+    case L6470_STATUS_LAYOUT: {
+      st.resetDev();
+      st.softFree();
+      st.SetParam(st.L64XX_CONFIG, CONFIG_PWM_DIV_1 | CONFIG_PWM_MUL_2 | CONFIG_OC_SD_DISABLE | CONFIG_VS_COMP_DISABLE | CONFIG_SW_HARD_STOP | CONFIG_INT_16MHZ);
+      st.SetParam(L6470_KVAL_RUN, 0xFF);
+      st.SetParam(L6470_KVAL_ACC, 0xFF);
+      st.SetParam(L6470_KVAL_DEC, 0xFF);
+      st.setMicroSteps(ms);
+      st.setOverCurrent(oc);
+      st.setStallCurrent(sc);
+      st.SetParam(L6470_KVAL_HOLD, mv);
+      st.SetParam(L6470_ABS_POS, 0);
+      uint32_t config_temp = st.GetParam(st.L64XX_CONFIG);
+      config_temp &= ~CONFIG_POW_SR;
+      switch (slew_rate) {
+        case 0: st.SetParam(st.L64XX_CONFIG, config_temp | CONFIG_SR_75V_us); break;
+        default:
+        case 1: st.SetParam(st.L64XX_CONFIG, config_temp | CONFIG_SR_110V_us); break;
+        case 3:
+        case 2: st.SetParam(st.L64XX_CONFIG, config_temp | CONFIG_SR_260V_us); break;
+      }
+      st.getStatus();
+      st.getStatus();
+      break;
+    }
+
+    case L6474_STATUS_LAYOUT: {
+      st.free();
+      //st.SetParam(st.L64XX_CONFIG, CONFIG_PWM_DIV_1 | CONFIG_PWM_MUL_2 | CONFIG_OC_SD_DISABLE | CONFIG_VS_COMP_DISABLE | CONFIG_SW_HARD_STOP | CONFIG_INT_16MHZ);
+      //st.SetParam(L6474_TVAL, 0xFF);
+      st.setMicroSteps(ms);
+      st.setOverCurrent(oc);
+      st.setTVALCurrent(sc);
+      st.SetParam(L6470_ABS_POS, 0);
+      uint32_t config_temp = st.GetParam(st.L64XX_CONFIG);
+      config_temp &= ~CONFIG_POW_SR & ~CONFIG_EN_TQREG;  // clear out slew rate and set current to be controlled by TVAL register
+      switch (slew_rate) {
+        case 0: st.SetParam(st.L64XX_CONFIG, config_temp | CONFIG_SR_75V_us); break;
+        default:
+        case 1: st.SetParam(st.L64XX_CONFIG, config_temp | CONFIG_SR_110V_us); break;
+        case 3:
+        case 2: st.SetParam(st.L64XX_CONFIG, config_temp | CONFIG_SR_260V_us); break;
+        //case 0: st.SetParam(st.L64XX_CONFIG, 0x2E88 | CONFIG_EN_TQREG | CONFIG_SR_75V_us); break;
+        //default:
+        //case 1: st.SetParam(st.L64XX_CONFIG, 0x2E88 | CONFIG_EN_TQREG | CONFIG_SR_110V_us); break;
+        //case 3:
+        //case 2: st.SetParam(st.L64XX_CONFIG, 0x2E88 | CONFIG_EN_TQREG | CONFIG_SR_260V_us); break;
+
+        //case 0: st.SetParam(st.L64XX_CONFIG, 0x2E88 ); break;
+        //default:
+        //case 1: st.SetParam(st.L64XX_CONFIG, 0x2E88 ); break;
+        //case 3:
+        //case 2: st.SetParam(st.L64XX_CONFIG, 0x2E88 ); break;
+      }
+      st.getStatus();
+      st.getStatus();
+      break;
+    }
+
+    case L6480_STATUS_LAYOUT: {
+      st.resetDev();
+      st.softFree();
+      st.SetParam(st.L64XX_CONFIG, CONFIG_PWM_DIV_1 | CONFIG_PWM_MUL_2 | CONFIG_OC_SD_DISABLE | CONFIG_VS_COMP_DISABLE | CONFIG_SW_HARD_STOP | CONFIG_INT_16MHZ);
+      st.SetParam(L6470_KVAL_RUN, 0xFF);
+      st.SetParam(L6470_KVAL_ACC, 0xFF);
+      st.SetParam(L6470_KVAL_DEC, 0xFF);
+      st.setMicroSteps(ms);
+      st.setOverCurrent(oc);
+      st.setStallCurrent(sc);
+      st.SetParam(+-L6470_KVAL_HOLD, mv);
+      st.SetParam(L6470_ABS_POS, 0);
+      st.SetParam(st.L64XX_CONFIG,(st.GetParam(st.L64XX_CONFIG) | PWR_VCC_7_5V));
+      st.getStatus();     // must clear out status bits before can set slew rate
+      st.getStatus();
+      switch (slew_rate) {
+        case 0: st.SetParam(L6470_GATECFG1, CONFIG1_SR_220V_us); st.SetParam(L6470_GATECFG2, CONFIG2_SR_220V_us); break;
+        default:
+        case 1: st.SetParam(L6470_GATECFG1, CONFIG1_SR_400V_us); st.SetParam(L6470_GATECFG2, CONFIG2_SR_400V_us); break;
+        case 2: st.SetParam(L6470_GATECFG1, CONFIG1_SR_520V_us); st.SetParam(L6470_GATECFG2, CONFIG2_SR_520V_us); break;
+        case 3: st.SetParam(L6470_GATECFG1, CONFIG1_SR_980V_us); st.SetParam(L6470_GATECFG2, CONFIG2_SR_980V_us); break;
+      }
+      break;
+    }
+  }
+}
+
+#define L6470_INIT_CHIP(Q) L6470_init_chip(stepper##Q, Q##_MICROSTEPS, Q##_OVERCURRENT, Q##_STALLCURRENT, Q##_MAX_VOLTAGE, Q##_SLEW_RATE)
+
+void L64XX_Marlin::init_to_defaults() {
+  #if AXIS_IS_L64XX(X)
+    L6470_INIT_CHIP(X);
+  #endif
+  #if AXIS_IS_L64XX(X2)
+    L6470_INIT_CHIP(X2);
+  #endif
+  #if AXIS_IS_L64XX(Y)
+    L6470_INIT_CHIP(Y);
+  #endif
+  #if AXIS_IS_L64XX(Y2)
+    L6470_INIT_CHIP(Y2);
+  #endif
+  #if AXIS_IS_L64XX(Z)
+    L6470_INIT_CHIP(Z);
+  #endif
+  #if AXIS_IS_L64XX(Z2)
+    L6470_INIT_CHIP(Z2);
+  #endif
+  #if AXIS_IS_L64XX(Z3)
+    L6470_INIT_CHIP(Z3);
+  #endif
+  #if AXIS_IS_L64XX(E0)
+    L6470_INIT_CHIP(E0);
+  #endif
+  #if AXIS_IS_L64XX(E1)
+    L6470_INIT_CHIP(E1);
+  #endif
+  #if AXIS_IS_L64XX(E2)
+    L6470_INIT_CHIP(E2);
+  #endif
+  #if AXIS_IS_L64XX(E3)
+    L6470_INIT_CHIP(E3);
+  #endif
+  #if AXIS_IS_L64XX(E4)
+    L6470_INIT_CHIP(E4);
+  #endif
+  #if AXIS_IS_L64XX(E5)
+    L6470_INIT_CHIP(E5);
+  #endif
+  #if AXIS_IS_L64XX(E6)
+    L6470_INIT_CHIP(E6);
+  #endif
+  #if AXIS_IS_L64XX(E7)
+    L6470_INIT_CHIP(E7);
+  #endif
+}
+
+#endif // HAS_L64XX
diff --git a/Marlin/src/module/stepper/L64xx.h b/Marlin/src/module/stepper/L64xx.h
new file mode 100644
index 0000000..9c8b0b1
--- /dev/null
+++ b/Marlin/src/module/stepper/L64xx.h
@@ -0,0 +1,364 @@
+/**
+ * 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
+
+/**
+ * stepper/L64xx.h
+ * Stepper driver indirection for L64XX drivers
+ */
+
+#include "../../inc/MarlinConfig.h"
+#include "../../libs/L64XX/L64XX_Marlin.h"
+
+// Convert option names to L64XX classes
+#define CLASS_L6470       L6470
+#define CLASS_L6474       L6474
+#define CLASS_POWERSTEP01 powerSTEP01
+
+#define __L64XX_CLASS(TYPE) CLASS_##TYPE
+#define _L64XX_CLASS(TYPE)  __L64XX_CLASS(TYPE)
+#define L64XX_CLASS(ST)     _L64XX_CLASS(ST##_DRIVER_TYPE)
+
+#define L6474_DIR_WRITE(A,STATE) do{ L64xxManager.dir_commands[A] = dSPIN_L6474_ENABLE; WRITE(A##_DIR_PIN, STATE); }while(0)
+#define L64XX_DIR_WRITE(A,STATE) do{ L64xxManager.dir_commands[A] = (STATE) ? dSPIN_STEP_CLOCK_REV : dSPIN_STEP_CLOCK_FWD; }while(0)
+
+// X Stepper
+#if AXIS_IS_L64XX(X)
+  extern L64XX_CLASS(X)         stepperX;
+  #define X_ENABLE_INIT()       NOOP
+  #define X_ENABLE_WRITE(STATE) (STATE ? stepperX.hardStop() : stepperX.free())
+  #define X_ENABLE_READ()       (stepperX.getStatus() & STATUS_HIZ)
+  #if AXIS_DRIVER_TYPE_X(L6474)
+    #define X_DIR_INIT()        SET_OUTPUT(X_DIR_PIN)
+    #define X_DIR_WRITE(STATE)  L6474_DIR_WRITE(X, STATE)
+    #define X_DIR_READ()        READ(X_DIR_PIN)
+  #else
+    #define X_DIR_INIT()        NOOP
+    #define X_DIR_WRITE(STATE)  L64XX_DIR_WRITE(X, STATE)
+    #define X_DIR_READ()        (stepper##X.getStatus() & STATUS_DIR);
+    #if AXIS_DRIVER_TYPE_X(L6470)
+      #define DISABLE_STEPPER_X() stepperX.free()
+    #endif
+  #endif
+#endif
+
+// Y Stepper
+#if AXIS_IS_L64XX(Y)
+  extern L64XX_CLASS(Y)         stepperY;
+  #define Y_ENABLE_INIT()       NOOP
+  #define Y_ENABLE_WRITE(STATE) (STATE ? stepperY.hardStop() : stepperY.free())
+  #define Y_ENABLE_READ()       (stepperY.getStatus() & STATUS_HIZ)
+  #if AXIS_DRIVER_TYPE_Y(L6474)
+    #define Y_DIR_INIT()        SET_OUTPUT(Y_DIR_PIN)
+    #define Y_DIR_WRITE(STATE)  L6474_DIR_WRITE(Y, STATE)
+    #define Y_DIR_READ()        READ(Y_DIR_PIN)
+  #else
+    #define Y_DIR_INIT()        NOOP
+    #define Y_DIR_WRITE(STATE)  L64XX_DIR_WRITE(Y, STATE)
+    #define Y_DIR_READ()        (stepper##Y.getStatus() & STATUS_DIR);
+    #if AXIS_DRIVER_TYPE_Y(L6470)
+      #define DISABLE_STEPPER_Y() stepperY.free()
+    #endif
+  #endif
+#endif
+
+// Z Stepper
+#if AXIS_IS_L64XX(Z)
+  extern L64XX_CLASS(Z)         stepperZ;
+  #define Z_ENABLE_INIT()       NOOP
+  #define Z_ENABLE_WRITE(STATE) (STATE ? stepperZ.hardStop() : stepperZ.free())
+  #define Z_ENABLE_READ()       (stepperZ.getStatus() & STATUS_HIZ)
+  #if AXIS_DRIVER_TYPE_Z(L6474)
+    #define Z_DIR_INIT()        SET_OUTPUT(Z_DIR_PIN)
+    #define Z_DIR_WRITE(STATE)  L6474_DIR_WRITE(Z, STATE)
+    #define Z_DIR_READ()        READ(Z_DIR_PIN)
+  #else
+    #define Z_DIR_INIT()        NOOP
+    #define Z_DIR_WRITE(STATE)  L64XX_DIR_WRITE(Z, STATE)
+    #define Z_DIR_READ()        (stepper##Z.getStatus() & STATUS_DIR);
+    #if AXIS_DRIVER_TYPE_Z(L6470)
+      #define DISABLE_STEPPER_Z() stepperZ.free()
+    #endif
+  #endif
+#endif
+
+// X2 Stepper
+#if HAS_X2_ENABLE && AXIS_IS_L64XX(X2)
+  extern L64XX_CLASS(X2)         stepperX2;
+  #define X2_ENABLE_INIT()       NOOP
+  #define X2_ENABLE_WRITE(STATE) (STATE ? stepperX2.hardStop() : stepperX2.free())
+  #define X2_ENABLE_READ()       (stepperX2.getStatus() & STATUS_HIZ)
+  #if AXIS_DRIVER_TYPE_X2(L6474)
+    #define X2_DIR_INIT()        SET_OUTPUT(X2_DIR_PIN)
+    #define X2_DIR_WRITE(STATE)  L6474_DIR_WRITE(X2, STATE)
+    #define X2_DIR_READ()        READ(X2_DIR_PIN)
+  #else
+    #define X2_DIR_INIT()        NOOP
+    #define X2_DIR_WRITE(STATE)  L64XX_DIR_WRITE(X2, STATE)
+    #define X2_DIR_READ()        (stepper##X2.getStatus() & STATUS_DIR);
+  #endif
+#endif
+
+#if AXIS_DRIVER_TYPE_X2(L6470)
+  #define DISABLE_STEPPER_X2() stepperX2.free()
+#endif
+
+// Y2 Stepper
+#if HAS_Y2_ENABLE && AXIS_IS_L64XX(Y2)
+  extern L64XX_CLASS(Y2)         stepperY2;
+  #define Y2_ENABLE_INIT()       NOOP
+  #define Y2_ENABLE_WRITE(STATE) (STATE ? stepperY2.hardStop() : stepperY2.free())
+  #define Y2_ENABLE_READ()       (stepperY2.getStatus() & STATUS_HIZ)
+  #if AXIS_DRIVER_TYPE_Y2(L6474)
+    #define Y2_DIR_INIT()        SET_OUTPUT(Y2_DIR_PIN)
+    #define Y2_DIR_WRITE(STATE)  L6474_DIR_WRITE(Y2, STATE)
+    #define Y2_DIR_READ()        READ(Y2_DIR_PIN)
+  #else
+    #define Y2_DIR_INIT()        NOOP
+    #define Y2_DIR_WRITE(STATE)  L64XX_DIR_WRITE(Y2, STATE)
+    #define Y2_DIR_READ()        (stepper##Y2.getStatus() & STATUS_DIR);
+  #endif
+#endif
+
+#if AXIS_DRIVER_TYPE_Y2(L6470)
+  #define DISABLE_STEPPER_Y2() stepperY2.free()
+#endif
+
+// Z2 Stepper
+#if HAS_Z2_ENABLE && AXIS_IS_L64XX(Z2)
+  extern L64XX_CLASS(Z2)         stepperZ2;
+  #define Z2_ENABLE_INIT()       NOOP
+  #define Z2_ENABLE_WRITE(STATE) (STATE ? stepperZ2.hardStop() : stepperZ2.free())
+  #define Z2_ENABLE_READ()       (stepperZ2.getStatus() & STATUS_HIZ)
+  #if AXIS_DRIVER_TYPE_Z2(L6474)
+    #define Z2_DIR_INIT()        SET_OUTPUT(Z2_DIR_PIN)
+    #define Z2_DIR_WRITE(STATE)  L6474_DIR_WRITE(Z2, STATE)
+    #define Z2_DIR_READ()        READ(Z2_DIR_PIN)
+  #else
+    #define Z2_DIR_INIT()        NOOP
+    #define Z2_DIR_WRITE(STATE)  L64XX_DIR_WRITE(Z2, STATE)
+    #define Z2_DIR_READ()        (stepper##Z2.getStatus() & STATUS_DIR);
+  #endif
+#endif
+
+#if AXIS_DRIVER_TYPE_Z2(L6470)
+  #define DISABLE_STEPPER_Z2() stepperZ2.free()
+#endif
+
+// Z3 Stepper
+#if HAS_Z3_ENABLE && AXIS_IS_L64XX(Z3)
+  extern L64XX_CLASS(Z3)         stepperZ3;
+  #define Z3_ENABLE_INIT()       NOOP
+  #define Z3_ENABLE_WRITE(STATE) (STATE ? stepperZ3.hardStop() : stepperZ3.free())
+  #define Z3_ENABLE_READ()       (stepperZ3.getStatus() & STATUS_HIZ)
+  #if AXIS_DRIVER_TYPE_Z3(L6474)
+    #define Z3_DIR_INIT()        SET_OUTPUT(Z3_DIR_PIN)
+    #define Z3_DIR_WRITE(STATE)  L6474_DIR_WRITE(Z3, STATE)
+    #define Z3_DIR_READ()        READ(Z3_DIR_PIN)
+  #else
+    #define Z3_DIR_INIT()        NOOP
+    #define Z3_DIR_WRITE(STATE)  L64XX_DIR_WRITE(Z3, STATE)
+    #define Z3_DIR_READ()        (stepper##Z3.getStatus() & STATUS_DIR);
+  #endif
+#endif
+
+#if AXIS_DRIVER_TYPE_Z3(L6470)
+  #define DISABLE_STEPPER_Z3() stepperZ3.free()
+#endif
+
+// Z4 Stepper
+#if HAS_Z4_ENABLE && AXIS_IS_L64XX(Z4)
+  extern L64XX_CLASS(Z4)         stepperZ4;
+  #define Z4_ENABLE_INIT()       NOOP
+  #define Z4_ENABLE_WRITE(STATE) (STATE ? stepperZ4.hardStop() : stepperZ4.free())
+  #define Z4_ENABLE_READ()       (stepperZ4.getStatus() & STATUS_HIZ)
+  #if AXIS_DRIVER_TYPE_Z4(L6474)
+    #define Z4_DIR_INIT()        SET_OUTPUT(Z4_DIR_PIN)
+    #define Z4_DIR_WRITE(STATE)  L6474_DIR_WRITE(Z4, STATE)
+    #define Z4_DIR_READ()        READ(Z4_DIR_PIN)
+  #else
+    #define Z4_DIR_INIT()        NOOP
+    #define Z4_DIR_WRITE(STATE)  L64XX_DIR_WRITE(Z4, STATE)
+    #define Z4_DIR_READ()        (stepper##Z4.getStatus() & STATUS_DIR);
+  #endif
+#endif
+
+#if AXIS_DRIVER_TYPE_Z4(L6470)
+  #define DISABLE_STEPPER_Z4() stepperZ4.free()
+#endif
+
+// E0 Stepper
+#if AXIS_IS_L64XX(E0)
+  extern L64XX_CLASS(E0)         stepperE0;
+  #define E0_ENABLE_INIT()       NOOP
+  #define E0_ENABLE_WRITE(STATE)  (STATE ? stepperE0.hardStop() : stepperE0.free())
+  #define E0_ENABLE_READ()       (stepperE0.getStatus() & STATUS_HIZ)
+  #if AXIS_DRIVER_TYPE_E0(L6474)
+    #define E0_DIR_INIT()        SET_OUTPUT(E0_DIR_PIN)
+    #define E0_DIR_WRITE(STATE)  L6474_DIR_WRITE(E0, STATE)
+    #define E0_DIR_READ()        READ(E0_DIR_PIN)
+  #else
+    #define E0_DIR_INIT()        NOOP
+    #define E0_DIR_WRITE(STATE)  L64XX_DIR_WRITE(E0, STATE)
+    #define E0_DIR_READ()        (stepper##E0.getStatus() & STATUS_DIR);
+    #if AXIS_DRIVER_TYPE_E0(L6470)
+      #define DISABLE_STEPPER_E0() do{ stepperE0.free(); }while(0)
+    #endif
+  #endif
+#endif
+
+// E1 Stepper
+#if AXIS_IS_L64XX(E1)
+  extern L64XX_CLASS(E1)         stepperE1;
+  #define E1_ENABLE_INIT()       NOOP
+  #define E1_ENABLE_WRITE(STATE) (STATE ? stepperE1.hardStop() : stepperE1.free())
+  #define E1_ENABLE_READ()       (stepperE1.getStatus() & STATUS_HIZ)
+  #if AXIS_DRIVER_TYPE_E1(L6474)
+    #define E1_DIR_INIT()        SET_OUTPUT(E1_DIR_PIN)
+    #define E1_DIR_WRITE(STATE)  L6474_DIR_WRITE(E1, STATE)
+    #define E1_DIR_READ()        READ(E1_DIR_PIN)
+  #else
+    #define E1_DIR_INIT()        NOOP
+    #define E1_DIR_WRITE(STATE)  L64XX_DIR_WRITE(E1, STATE)
+    #define E1_DIR_READ()        (stepper##E1.getStatus() & STATUS_DIR);
+    #if AXIS_DRIVER_TYPE_E1(L6470)
+      #define DISABLE_STEPPER_E1() do{ stepperE1.free(); }while(0)
+    #endif
+  #endif
+#endif
+
+// E2 Stepper
+#if AXIS_IS_L64XX(E2)
+  extern L64XX_CLASS(E2)         stepperE2;
+  #define E2_ENABLE_INIT()       NOOP
+  #define E2_ENABLE_WRITE(STATE) (STATE ? stepperE2.hardStop() : stepperE2.free())
+  #define E2_ENABLE_READ()       (stepperE2.getStatus() & STATUS_HIZ)
+  #if AXIS_DRIVER_TYPE_E2(L6474)
+    #define E2_DIR_INIT()        SET_OUTPUT(E2_DIR_PIN)
+    #define E2_DIR_WRITE(STATE)  L6474_DIR_WRITE(E2, STATE)
+    #define E2_DIR_READ()        READ(E2_DIR_PIN)
+  #else
+    #define E2_DIR_INIT()        NOOP
+    #define E2_DIR_WRITE(STATE)  L64XX_DIR_WRITE(E2, STATE)
+    #define E2_DIR_READ()        (stepper##E2.getStatus() & STATUS_DIR);
+    #if AXIS_DRIVER_TYPE_E2(L6470)
+      #define DISABLE_STEPPER_E2() do{ stepperE2.free(); }while(0)
+    #endif
+  #endif
+#endif
+
+// E3 Stepper
+#if AXIS_IS_L64XX(E3)
+  extern L64XX_CLASS(E3)         stepperE3;
+  #define E3_ENABLE_INIT()       NOOP
+  #define E3_ENABLE_WRITE(STATE) (STATE ? stepperE3.hardStop() : stepperE3.free())
+  #define E3_ENABLE_READ()       (stepperE3.getStatus() & STATUS_HIZ)
+  #if AXIS_DRIVER_TYPE_E3(L6474)
+    #define E3_DIR_INIT()        SET_OUTPUT(E3_DIR_PIN)
+    #define E3_DIR_WRITE(STATE)  L6474_DIR_WRITE(E3, STATE)
+    #define E3_DIR_READ()        READ(E3_DIR_PIN)
+  #else
+    #define E3_DIR_INIT()        NOOP
+    #define E3_DIR_WRITE(STATE)  L64XX_DIR_WRITE(E3, STATE)
+    #define E3_DIR_READ()        (stepper##E3.getStatus() & STATUS_DIR);
+  #endif
+#endif
+
+// E4 Stepper
+#if AXIS_IS_L64XX(E4)
+  extern L64XX_CLASS(E4)         stepperE4;
+  #define E4_ENABLE_INIT()       NOOP
+  #define E4_ENABLE_WRITE(STATE) (STATE ? stepperE4.hardStop() : stepperE4.free())
+  #define E4_ENABLE_READ()       (stepperE4.getStatus() & STATUS_HIZ)
+  #if AXIS_DRIVER_TYPE_E4(L6474)
+    #define E4_DIR_INIT()        SET_OUTPUT(E4_DIR_PIN)
+    #define E4_DIR_WRITE(STATE)  L6474_DIR_WRITE(E4, STATE)
+    #define E4_DIR_READ()        READ(E4_DIR_PIN)
+  #else
+    #define E4_DIR_INIT()        NOOP
+    #define E4_DIR_WRITE(STATE)  L64XX_DIR_WRITE(E4, STATE)
+    #define E4_DIR_READ()        (stepper##E4.getStatus() & STATUS_DIR);
+    #if AXIS_DRIVER_TYPE_E4(L6470)
+      #define DISABLE_STEPPER_E4() do{ stepperE4.free(); }while(0)
+    #endif
+  #endif
+#endif
+
+// E5 Stepper
+#if AXIS_IS_L64XX(E5)
+  extern L64XX_CLASS(E5)         stepperE5;
+  #define E5_ENABLE_INIT()       NOOP
+  #define E5_ENABLE_WRITE(STATE) (STATE ? stepperE5.hardStop() : stepperE5.free())
+  #define E5_ENABLE_READ()       (stepperE5.getStatus() & STATUS_HIZ)
+  #if AXIS_DRIVER_TYPE_E5(L6474)
+    #define E5_DIR_INIT()        SET_OUTPUT(E5_DIR_PIN)
+    #define E5_DIR_WRITE(STATE)  L6474_DIR_WRITE(E5, STATE)
+    #define E5_DIR_READ()        READ(E5_DIR_PIN)
+  #else
+    #define E5_DIR_INIT()        NOOP
+    #define E5_DIR_WRITE(STATE)  L64XX_DIR_WRITE(E5, STATE)
+    #define E5_DIR_READ()        (stepper##E5.getStatus() & STATUS_DIR);
+    #if AXIS_DRIVER_TYPE_E5(L6470)
+      #define DISABLE_STEPPER_E5() do{ stepperE5.free(); }while(0)
+    #endif
+  #endif
+#endif
+
+// E6 Stepper
+#if AXIS_IS_L64XX(E6)
+  extern L64XX_CLASS(E6)         stepperE6;
+  #define E6_ENABLE_INIT()       NOOP
+  #define E6_ENABLE_WRITE(STATE) (STATE ? stepperE6.hardStop() : stepperE6.free())
+  #define E6_ENABLE_READ()       (stepperE6.getStatus() & STATUS_HIZ)
+  #if AXIS_DRIVER_TYPE_E6(L6474)
+    #define E6_DIR_INIT()        SET_OUTPUT(E6_DIR_PIN)
+    #define E6_DIR_WRITE(STATE)  L6474_DIR_WRITE(E6, STATE)
+    #define E6_DIR_READ()        READ(E6_DIR_PIN)
+  #else
+    #define E6_DIR_INIT()        NOOP
+    #define E6_DIR_WRITE(STATE)  L64XX_DIR_WRITE(E6, STATE)
+    #define E6_DIR_READ()        (stepper##E6.getStatus() & STATUS_DIR);
+    #if AXIS_DRIVER_TYPE_E6(L6470)
+      #define DISABLE_STEPPER_E6() do{ stepperE6.free(); }while(0)
+    #endif
+  #endif
+#endif
+
+// E7 Stepper
+#if AXIS_IS_L64XX(E7)
+  extern L64XX_CLASS(E7)         stepperE7;
+  #define E7_ENABLE_INIT()       NOOP
+  #define E7_ENABLE_WRITE(STATE) (STATE ? stepperE7.hardStop() : stepperE7.free())
+  #define E7_ENABLE_READ()       (stepperE7.getStatus() & STATUS_HIZ)
+  #if AXIS_DRIVER_TYPE_E7(L6474)
+    #define E7_DIR_INIT()        SET_OUTPUT(E7_DIR_PIN)
+    #define E7_DIR_WRITE(STATE)  L6474_DIR_WRITE(E7, STATE)
+    #define E7_DIR_READ()        READ(E7_DIR_PIN)
+  #else
+    #define E7_DIR_INIT()        NOOP
+    #define E7_DIR_WRITE(STATE)  L64XX_DIR_WRITE(E7, STATE)
+    #define E7_DIR_READ()        (stepper##E7.getStatus() & STATUS_DIR);
+    #if AXIS_DRIVER_TYPE_E7(L6470)
+      #define DISABLE_STEPPER_E7() do{ stepperE7.free(); }while(0)
+    #endif
+  #endif
+#endif
diff --git a/Marlin/src/module/stepper/TMC26X.cpp b/Marlin/src/module/stepper/TMC26X.cpp
new file mode 100644
index 0000000..926f1a4
--- /dev/null
+++ b/Marlin/src/module/stepper/TMC26X.cpp
@@ -0,0 +1,144 @@
+/**
+ * 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/>.
+ *
+ */
+
+/**
+ * stepper/TMC26X.cpp
+ * Stepper driver indirection for TMC26X drivers
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+//
+// TMC26X Driver objects and inits
+//
+#if HAS_TMC26X
+
+#include "TMC26X.h"
+
+#define _TMC26X_DEFINE(ST) TMC26XStepper stepper##ST(200, ST##_CS_PIN, ST##_STEP_PIN, ST##_DIR_PIN, ST##_MAX_CURRENT, ST##_SENSE_RESISTOR)
+
+#if AXIS_DRIVER_TYPE_X(TMC26X)
+  _TMC26X_DEFINE(X);
+#endif
+#if AXIS_DRIVER_TYPE_X2(TMC26X)
+  _TMC26X_DEFINE(X2);
+#endif
+#if AXIS_DRIVER_TYPE_Y(TMC26X)
+  _TMC26X_DEFINE(Y);
+#endif
+#if AXIS_DRIVER_TYPE_Y2(TMC26X)
+  _TMC26X_DEFINE(Y2);
+#endif
+#if AXIS_DRIVER_TYPE_Z(TMC26X)
+  _TMC26X_DEFINE(Z);
+#endif
+#if AXIS_DRIVER_TYPE_Z2(TMC26X)
+  _TMC26X_DEFINE(Z2);
+#endif
+#if AXIS_DRIVER_TYPE_Z3(TMC26X)
+  _TMC26X_DEFINE(Z3);
+#endif
+#if AXIS_DRIVER_TYPE_Z4(TMC26X)
+  _TMC26X_DEFINE(Z4);
+#endif
+#if AXIS_DRIVER_TYPE_E0(TMC26X)
+  _TMC26X_DEFINE(E0);
+#endif
+#if AXIS_DRIVER_TYPE_E1(TMC26X)
+  _TMC26X_DEFINE(E1);
+#endif
+#if AXIS_DRIVER_TYPE_E2(TMC26X)
+  _TMC26X_DEFINE(E2);
+#endif
+#if AXIS_DRIVER_TYPE_E3(TMC26X)
+  _TMC26X_DEFINE(E3);
+#endif
+#if AXIS_DRIVER_TYPE_E4(TMC26X)
+  _TMC26X_DEFINE(E4);
+#endif
+#if AXIS_DRIVER_TYPE_E5(TMC26X)
+  _TMC26X_DEFINE(E5);
+#endif
+#if AXIS_DRIVER_TYPE_E6(TMC26X)
+  _TMC26X_DEFINE(E6);
+#endif
+#if AXIS_DRIVER_TYPE_E7(TMC26X)
+  _TMC26X_DEFINE(E7);
+#endif
+
+#define _TMC26X_INIT(A) do{ \
+  stepper##A.setMicrosteps(A##_MICROSTEPS); \
+  stepper##A.start(); \
+}while(0)
+
+void tmc26x_init_to_defaults() {
+  #if AXIS_DRIVER_TYPE_X(TMC26X)
+    _TMC26X_INIT(X);
+  #endif
+  #if AXIS_DRIVER_TYPE_X2(TMC26X)
+    _TMC26X_INIT(X2);
+  #endif
+  #if AXIS_DRIVER_TYPE_Y(TMC26X)
+    _TMC26X_INIT(Y);
+  #endif
+  #if AXIS_DRIVER_TYPE_Y2(TMC26X)
+    _TMC26X_INIT(Y2);
+  #endif
+  #if AXIS_DRIVER_TYPE_Z(TMC26X)
+    _TMC26X_INIT(Z);
+  #endif
+  #if AXIS_DRIVER_TYPE_Z2(TMC26X)
+    _TMC26X_INIT(Z2);
+  #endif
+  #if AXIS_DRIVER_TYPE_Z3(TMC26X)
+    _TMC26X_INIT(Z3);
+  #endif
+  #if AXIS_DRIVER_TYPE_Z4(TMC26X)
+    _TMC26X_INIT(Z4);
+  #endif
+  #if AXIS_DRIVER_TYPE_E0(TMC26X)
+    _TMC26X_INIT(E0);
+  #endif
+  #if AXIS_DRIVER_TYPE_E1(TMC26X)
+    _TMC26X_INIT(E1);
+  #endif
+  #if AXIS_DRIVER_TYPE_E2(TMC26X)
+    _TMC26X_INIT(E2);
+  #endif
+  #if AXIS_DRIVER_TYPE_E3(TMC26X)
+    _TMC26X_INIT(E3);
+  #endif
+  #if AXIS_DRIVER_TYPE_E4(TMC26X)
+    _TMC26X_INIT(E4);
+  #endif
+  #if AXIS_DRIVER_TYPE_E5(TMC26X)
+    _TMC26X_INIT(E5);
+  #endif
+  #if AXIS_DRIVER_TYPE_E6(TMC26X)
+    _TMC26X_INIT(E6);
+  #endif
+  #if AXIS_DRIVER_TYPE_E7(TMC26X)
+    _TMC26X_INIT(E7);
+  #endif
+}
+
+#endif // HAS_TMC26X
diff --git a/Marlin/src/module/stepper/TMC26X.h b/Marlin/src/module/stepper/TMC26X.h
new file mode 100644
index 0000000..547eb65
--- /dev/null
+++ b/Marlin/src/module/stepper/TMC26X.h
@@ -0,0 +1,164 @@
+/**
+ * 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
+
+/**
+ * stepper/TMC26X.h
+ * Stepper driver indirection for TMC26X drivers
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+// TMC26X drivers have STEP/DIR on normal pins, but ENABLE via SPI
+
+#include <SPI.h>
+#include <TMC26XStepper.h>
+
+void tmc26x_init_to_defaults();
+
+// X Stepper
+#if AXIS_DRIVER_TYPE_X(TMC26X)
+  extern TMC26XStepper stepperX;
+  #define X_ENABLE_INIT() NOOP
+  #define X_ENABLE_WRITE(STATE) stepperX.setEnabled(STATE)
+  #define X_ENABLE_READ() stepperX.isEnabled()
+#endif
+
+// Y Stepper
+#if AXIS_DRIVER_TYPE_Y(TMC26X)
+  extern TMC26XStepper stepperY;
+  #define Y_ENABLE_INIT() NOOP
+  #define Y_ENABLE_WRITE(STATE) stepperY.setEnabled(STATE)
+  #define Y_ENABLE_READ() stepperY.isEnabled()
+#endif
+
+// Z Stepper
+#if AXIS_DRIVER_TYPE_Z(TMC26X)
+  extern TMC26XStepper stepperZ;
+  #define Z_ENABLE_INIT() NOOP
+  #define Z_ENABLE_WRITE(STATE) stepperZ.setEnabled(STATE)
+  #define Z_ENABLE_READ() stepperZ.isEnabled()
+#endif
+
+// X2 Stepper
+#if HAS_X2_ENABLE && AXIS_DRIVER_TYPE_X2(TMC26X)
+  extern TMC26XStepper stepperX2;
+  #define X2_ENABLE_INIT() NOOP
+  #define X2_ENABLE_WRITE(STATE) stepperX2.setEnabled(STATE)
+  #define X2_ENABLE_READ() stepperX2.isEnabled()
+#endif
+
+// Y2 Stepper
+#if HAS_Y2_ENABLE && AXIS_DRIVER_TYPE_Y2(TMC26X)
+  extern TMC26XStepper stepperY2;
+  #define Y2_ENABLE_INIT() NOOP
+  #define Y2_ENABLE_WRITE(STATE) stepperY2.setEnabled(STATE)
+  #define Y2_ENABLE_READ() stepperY2.isEnabled()
+#endif
+
+// Z2 Stepper
+#if HAS_Z2_ENABLE && AXIS_DRIVER_TYPE_Z2(TMC26X)
+  extern TMC26XStepper stepperZ2;
+  #define Z2_ENABLE_INIT() NOOP
+  #define Z2_ENABLE_WRITE(STATE) stepperZ2.setEnabled(STATE)
+  #define Z2_ENABLE_READ() stepperZ2.isEnabled()
+#endif
+
+// Z3 Stepper
+#if HAS_Z3_ENABLE && AXIS_DRIVER_TYPE_Z3(TMC26X)
+  extern TMC26XStepper stepperZ3;
+  #define Z3_ENABLE_INIT() NOOP
+  #define Z3_ENABLE_WRITE(STATE) stepperZ3.setEnabled(STATE)
+  #define Z3_ENABLE_READ() stepperZ3.isEnabled()
+#endif
+
+// Z4 Stepper
+#if HAS_Z4_ENABLE && AXIS_DRIVER_TYPE_Z4(TMC26X)
+  extern TMC26XStepper stepperZ4;
+  #define Z4_ENABLE_INIT() NOOP
+  #define Z4_ENABLE_WRITE(STATE) stepperZ4.setEnabled(STATE)
+  #define Z4_ENABLE_READ() stepperZ4.isEnabled()
+#endif
+
+// E0 Stepper
+#if AXIS_DRIVER_TYPE_E0(TMC26X)
+  extern TMC26XStepper stepperE0;
+  #define E0_ENABLE_INIT() NOOP
+  #define E0_ENABLE_WRITE(STATE) stepperE0.setEnabled(STATE)
+  #define E0_ENABLE_READ() stepperE0.isEnabled()
+#endif
+
+// E1 Stepper
+#if AXIS_DRIVER_TYPE_E1(TMC26X)
+  extern TMC26XStepper stepperE1;
+  #define E1_ENABLE_INIT() NOOP
+  #define E1_ENABLE_WRITE(STATE) stepperE1.setEnabled(STATE)
+  #define E1_ENABLE_READ() stepperE1.isEnabled()
+#endif
+
+// E2 Stepper
+#if AXIS_DRIVER_TYPE_E2(TMC26X)
+  extern TMC26XStepper stepperE2;
+  #define E2_ENABLE_INIT() NOOP
+  #define E2_ENABLE_WRITE(STATE) stepperE2.setEnabled(STATE)
+  #define E2_ENABLE_READ() stepperE2.isEnabled()
+#endif
+
+// E3 Stepper
+#if AXIS_DRIVER_TYPE_E3(TMC26X)
+  extern TMC26XStepper stepperE3;
+  #define E3_ENABLE_INIT() NOOP
+  #define E3_ENABLE_WRITE(STATE) stepperE3.setEnabled(STATE)
+  #define E3_ENABLE_READ() stepperE3.isEnabled()
+#endif
+
+// E4 Stepper
+#if AXIS_DRIVER_TYPE_E4(TMC26X)
+  extern TMC26XStepper stepperE4;
+  #define E4_ENABLE_INIT() NOOP
+  #define E4_ENABLE_WRITE(STATE) stepperE4.setEnabled(STATE)
+  #define E4_ENABLE_READ() stepperE4.isEnabled()
+#endif
+
+// E5 Stepper
+#if AXIS_DRIVER_TYPE_E5(TMC26X)
+  extern TMC26XStepper stepperE5;
+  #define E5_ENABLE_INIT() NOOP
+  #define E5_ENABLE_WRITE(STATE) stepperE5.setEnabled(STATE)
+  #define E5_ENABLE_READ() stepperE5.isEnabled()
+#endif
+
+// E6 Stepper
+#if AXIS_DRIVER_TYPE_E6(TMC26X)
+  extern TMC26XStepper stepperE6;
+  #define E6_ENABLE_INIT() NOOP
+  #define E6_ENABLE_WRITE(STATE) stepperE6.setEnabled(STATE)
+  #define E6_ENABLE_READ() stepperE6.isEnabled()
+#endif
+
+// E7 Stepper
+#if AXIS_DRIVER_TYPE_E7(TMC26X)
+  extern TMC26XStepper stepperE7;
+  #define E7_ENABLE_INIT() NOOP
+  #define E7_ENABLE_WRITE(STATE) stepperE7.setEnabled(STATE)
+  #define E7_ENABLE_READ() stepperE7.isEnabled()
+#endif
diff --git a/Marlin/src/module/stepper/indirection.cpp b/Marlin/src/module/stepper/indirection.cpp
new file mode 100644
index 0000000..6297d83
--- /dev/null
+++ b/Marlin/src/module/stepper/indirection.cpp
@@ -0,0 +1,50 @@
+/**
+ * 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/>.
+ *
+ */
+
+/**
+ * stepper/indirection.cpp
+ *
+ * Stepper motor driver indirection to allow some stepper functions to
+ * be done via SPI/I2c instead of direct pin manipulation.
+ *
+ * Copyright (c) 2015 Dominik Wenger
+ */
+
+#include "../../inc/MarlinConfig.h"
+#include "indirection.h"
+
+void restore_stepper_drivers() {
+  TERN_(HAS_TRINAMIC_CONFIG, restore_trinamic_drivers());
+}
+
+void reset_stepper_drivers() {
+  #if HAS_DRIVER(TMC26X)
+    tmc26x_init_to_defaults();
+  #endif
+  TERN_(HAS_L64XX, L64xxManager.init_to_defaults());
+  TERN_(HAS_TRINAMIC_CONFIG, reset_trinamic_drivers());
+}
+
+#if ENABLED(SOFTWARE_DRIVER_ENABLE)
+  // Flags to optimize XYZ Enabled state
+  xyz_bool_t axis_sw_enabled; // = { false, false, false }
+#endif
diff --git a/Marlin/src/module/stepper/indirection.h b/Marlin/src/module/stepper/indirection.h
new file mode 100644
index 0000000..4346e9d
--- /dev/null
+++ b/Marlin/src/module/stepper/indirection.h
@@ -0,0 +1,1003 @@
+/**
+ * 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
+
+/**
+ * stepper/indirection.h
+ *
+ * Stepper motor driver indirection to allow some stepper functions to
+ * be done via SPI/I2c instead of direct pin manipulation.
+ *
+ * Copyright (c) 2015 Dominik Wenger
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if HAS_L64XX
+  #include "L64xx.h"
+#endif
+
+#if HAS_DRIVER(TMC26X)
+  #include "TMC26X.h"
+#endif
+
+#if HAS_TRINAMIC_CONFIG
+  #include "trinamic.h"
+#endif
+
+void restore_stepper_drivers();  // Called by PSU_ON
+void reset_stepper_drivers();    // Called by settings.load / settings.reset
+
+// X Stepper
+#ifndef X_ENABLE_INIT
+  #define X_ENABLE_INIT() SET_OUTPUT(X_ENABLE_PIN)
+  #define X_ENABLE_WRITE(STATE) WRITE(X_ENABLE_PIN,STATE)
+  #define X_ENABLE_READ() bool(READ(X_ENABLE_PIN))
+#endif
+#ifndef X_DIR_INIT
+  #define X_DIR_INIT() SET_OUTPUT(X_DIR_PIN)
+  #define X_DIR_WRITE(STATE) WRITE(X_DIR_PIN,STATE)
+  #define X_DIR_READ() bool(READ(X_DIR_PIN))
+#endif
+#define X_STEP_INIT() SET_OUTPUT(X_STEP_PIN)
+#ifndef X_STEP_WRITE
+  #define X_STEP_WRITE(STATE) WRITE(X_STEP_PIN,STATE)
+#endif
+#define X_STEP_READ() bool(READ(X_STEP_PIN))
+
+// Y Stepper
+#ifndef Y_ENABLE_INIT
+  #define Y_ENABLE_INIT() SET_OUTPUT(Y_ENABLE_PIN)
+  #define Y_ENABLE_WRITE(STATE) WRITE(Y_ENABLE_PIN,STATE)
+  #define Y_ENABLE_READ() bool(READ(Y_ENABLE_PIN))
+#endif
+#ifndef Y_DIR_INIT
+  #define Y_DIR_INIT() SET_OUTPUT(Y_DIR_PIN)
+  #define Y_DIR_WRITE(STATE) WRITE(Y_DIR_PIN,STATE)
+  #define Y_DIR_READ() bool(READ(Y_DIR_PIN))
+#endif
+#define Y_STEP_INIT() SET_OUTPUT(Y_STEP_PIN)
+#ifndef Y_STEP_WRITE
+  #define Y_STEP_WRITE(STATE) WRITE(Y_STEP_PIN,STATE)
+#endif
+#define Y_STEP_READ() bool(READ(Y_STEP_PIN))
+
+// Z Stepper
+#ifndef Z_ENABLE_INIT
+  #define Z_ENABLE_INIT() SET_OUTPUT(Z_ENABLE_PIN)
+  #define Z_ENABLE_WRITE(STATE) WRITE(Z_ENABLE_PIN,STATE)
+  #define Z_ENABLE_READ() bool(READ(Z_ENABLE_PIN))
+#endif
+#ifndef Z_DIR_INIT
+  #define Z_DIR_INIT() SET_OUTPUT(Z_DIR_PIN)
+  #define Z_DIR_WRITE(STATE) WRITE(Z_DIR_PIN,STATE)
+  #define Z_DIR_READ() bool(READ(Z_DIR_PIN))
+#endif
+#define Z_STEP_INIT() SET_OUTPUT(Z_STEP_PIN)
+#ifndef Z_STEP_WRITE
+  #define Z_STEP_WRITE(STATE) WRITE(Z_STEP_PIN,STATE)
+#endif
+#define Z_STEP_READ() bool(READ(Z_STEP_PIN))
+
+// X2 Stepper
+#if HAS_X2_ENABLE
+  #ifndef X2_ENABLE_INIT
+    #define X2_ENABLE_INIT() SET_OUTPUT(X2_ENABLE_PIN)
+    #define X2_ENABLE_WRITE(STATE) WRITE(X2_ENABLE_PIN,STATE)
+    #define X2_ENABLE_READ() bool(READ(X2_ENABLE_PIN))
+  #endif
+  #ifndef X2_DIR_INIT
+    #define X2_DIR_INIT() SET_OUTPUT(X2_DIR_PIN)
+    #define X2_DIR_WRITE(STATE) WRITE(X2_DIR_PIN,STATE)
+    #define X2_DIR_READ() bool(READ(X2_DIR_PIN))
+  #endif
+  #define X2_STEP_INIT() SET_OUTPUT(X2_STEP_PIN)
+  #ifndef X2_STEP_WRITE
+    #define X2_STEP_WRITE(STATE) WRITE(X2_STEP_PIN,STATE)
+  #endif
+  #define X2_STEP_READ() bool(READ(X2_STEP_PIN))
+#endif
+
+// Y2 Stepper
+#if HAS_Y2_ENABLE
+  #ifndef Y2_ENABLE_INIT
+    #define Y2_ENABLE_INIT() SET_OUTPUT(Y2_ENABLE_PIN)
+    #define Y2_ENABLE_WRITE(STATE) WRITE(Y2_ENABLE_PIN,STATE)
+    #define Y2_ENABLE_READ() bool(READ(Y2_ENABLE_PIN))
+  #endif
+  #ifndef Y2_DIR_INIT
+    #define Y2_DIR_INIT() SET_OUTPUT(Y2_DIR_PIN)
+    #define Y2_DIR_WRITE(STATE) WRITE(Y2_DIR_PIN,STATE)
+    #define Y2_DIR_READ() bool(READ(Y2_DIR_PIN))
+  #endif
+  #define Y2_STEP_INIT() SET_OUTPUT(Y2_STEP_PIN)
+  #ifndef Y2_STEP_WRITE
+    #define Y2_STEP_WRITE(STATE) WRITE(Y2_STEP_PIN,STATE)
+  #endif
+  #define Y2_STEP_READ() bool(READ(Y2_STEP_PIN))
+#else
+  #define Y2_DIR_WRITE(STATE) NOOP
+#endif
+
+// Z2 Stepper
+#if HAS_Z2_ENABLE
+  #ifndef Z2_ENABLE_INIT
+    #define Z2_ENABLE_INIT() SET_OUTPUT(Z2_ENABLE_PIN)
+    #define Z2_ENABLE_WRITE(STATE) WRITE(Z2_ENABLE_PIN,STATE)
+    #define Z2_ENABLE_READ() bool(READ(Z2_ENABLE_PIN))
+  #endif
+  #ifndef Z2_DIR_INIT
+    #define Z2_DIR_INIT() SET_OUTPUT(Z2_DIR_PIN)
+    #define Z2_DIR_WRITE(STATE) WRITE(Z2_DIR_PIN,STATE)
+    #define Z2_DIR_READ() bool(READ(Z2_DIR_PIN))
+  #endif
+  #define Z2_STEP_INIT() SET_OUTPUT(Z2_STEP_PIN)
+  #ifndef Z2_STEP_WRITE
+    #define Z2_STEP_WRITE(STATE) WRITE(Z2_STEP_PIN,STATE)
+  #endif
+  #define Z2_STEP_READ() bool(READ(Z2_STEP_PIN))
+#else
+  #define Z2_DIR_WRITE(STATE) NOOP
+#endif
+
+// Z3 Stepper
+#if HAS_Z3_ENABLE
+  #ifndef Z3_ENABLE_INIT
+    #define Z3_ENABLE_INIT() SET_OUTPUT(Z3_ENABLE_PIN)
+    #define Z3_ENABLE_WRITE(STATE) WRITE(Z3_ENABLE_PIN,STATE)
+    #define Z3_ENABLE_READ() bool(READ(Z3_ENABLE_PIN))
+  #endif
+  #ifndef Z3_DIR_INIT
+    #define Z3_DIR_INIT() SET_OUTPUT(Z3_DIR_PIN)
+    #define Z3_DIR_WRITE(STATE) WRITE(Z3_DIR_PIN,STATE)
+    #define Z3_DIR_READ() bool(READ(Z3_DIR_PIN))
+  #endif
+  #define Z3_STEP_INIT() SET_OUTPUT(Z3_STEP_PIN)
+  #ifndef Z3_STEP_WRITE
+    #define Z3_STEP_WRITE(STATE) WRITE(Z3_STEP_PIN,STATE)
+  #endif
+  #define Z3_STEP_READ() bool(READ(Z3_STEP_PIN))
+#else
+  #define Z3_DIR_WRITE(STATE) NOOP
+#endif
+
+// Z4 Stepper
+#if HAS_Z4_ENABLE
+  #ifndef Z4_ENABLE_INIT
+    #define Z4_ENABLE_INIT() SET_OUTPUT(Z4_ENABLE_PIN)
+    #define Z4_ENABLE_WRITE(STATE) WRITE(Z4_ENABLE_PIN,STATE)
+    #define Z4_ENABLE_READ() bool(READ(Z4_ENABLE_PIN))
+  #endif
+  #ifndef Z4_DIR_INIT
+    #define Z4_DIR_INIT() SET_OUTPUT(Z4_DIR_PIN)
+    #define Z4_DIR_WRITE(STATE) WRITE(Z4_DIR_PIN,STATE)
+    #define Z4_DIR_READ() bool(READ(Z4_DIR_PIN))
+  #endif
+  #define Z4_STEP_INIT() SET_OUTPUT(Z4_STEP_PIN)
+  #ifndef Z4_STEP_WRITE
+    #define Z4_STEP_WRITE(STATE) WRITE(Z4_STEP_PIN,STATE)
+  #endif
+  #define Z4_STEP_READ() bool(READ(Z4_STEP_PIN))
+#else
+  #define Z4_DIR_WRITE(STATE) NOOP
+#endif
+
+// E0 Stepper
+#ifndef E0_ENABLE_INIT
+  #define E0_ENABLE_INIT() SET_OUTPUT(E0_ENABLE_PIN)
+  #define E0_ENABLE_WRITE(STATE) WRITE(E0_ENABLE_PIN,STATE)
+  #define E0_ENABLE_READ() bool(READ(E0_ENABLE_PIN))
+#endif
+#ifndef E0_DIR_INIT
+  #define E0_DIR_INIT() SET_OUTPUT(E0_DIR_PIN)
+  #define E0_DIR_WRITE(STATE) WRITE(E0_DIR_PIN,STATE)
+  #define E0_DIR_READ() bool(READ(E0_DIR_PIN))
+#endif
+#define E0_STEP_INIT() SET_OUTPUT(E0_STEP_PIN)
+#ifndef E0_STEP_WRITE
+  #define E0_STEP_WRITE(STATE) WRITE(E0_STEP_PIN,STATE)
+#endif
+#define E0_STEP_READ() bool(READ(E0_STEP_PIN))
+
+// E1 Stepper
+#ifndef E1_ENABLE_INIT
+  #define E1_ENABLE_INIT() SET_OUTPUT(E1_ENABLE_PIN)
+  #define E1_ENABLE_WRITE(STATE) WRITE(E1_ENABLE_PIN,STATE)
+  #define E1_ENABLE_READ() bool(READ(E1_ENABLE_PIN))
+#endif
+#ifndef E1_DIR_INIT
+  #define E1_DIR_INIT() SET_OUTPUT(E1_DIR_PIN)
+  #define E1_DIR_WRITE(STATE) WRITE(E1_DIR_PIN,STATE)
+  #define E1_DIR_READ() bool(READ(E1_DIR_PIN))
+#endif
+#define E1_STEP_INIT() SET_OUTPUT(E1_STEP_PIN)
+#ifndef E1_STEP_WRITE
+  #define E1_STEP_WRITE(STATE) WRITE(E1_STEP_PIN,STATE)
+#endif
+#define E1_STEP_READ() bool(READ(E1_STEP_PIN))
+
+// E2 Stepper
+#ifndef E2_ENABLE_INIT
+  #define E2_ENABLE_INIT() SET_OUTPUT(E2_ENABLE_PIN)
+  #define E2_ENABLE_WRITE(STATE) WRITE(E2_ENABLE_PIN,STATE)
+  #define E2_ENABLE_READ() bool(READ(E2_ENABLE_PIN))
+#endif
+#ifndef E2_DIR_INIT
+  #define E2_DIR_INIT() SET_OUTPUT(E2_DIR_PIN)
+  #define E2_DIR_WRITE(STATE) WRITE(E2_DIR_PIN,STATE)
+  #define E2_DIR_READ() bool(READ(E2_DIR_PIN))
+#endif
+#define E2_STEP_INIT() SET_OUTPUT(E2_STEP_PIN)
+#ifndef E2_STEP_WRITE
+  #define E2_STEP_WRITE(STATE) WRITE(E2_STEP_PIN,STATE)
+#endif
+#define E2_STEP_READ() bool(READ(E2_STEP_PIN))
+
+// E3 Stepper
+#ifndef E3_ENABLE_INIT
+  #define E3_ENABLE_INIT() SET_OUTPUT(E3_ENABLE_PIN)
+  #define E3_ENABLE_WRITE(STATE) WRITE(E3_ENABLE_PIN,STATE)
+  #define E3_ENABLE_READ() bool(READ(E3_ENABLE_PIN))
+#endif
+#ifndef E3_DIR_INIT
+  #define E3_DIR_INIT() SET_OUTPUT(E3_DIR_PIN)
+  #define E3_DIR_WRITE(STATE) WRITE(E3_DIR_PIN,STATE)
+  #define E3_DIR_READ() bool(READ(E3_DIR_PIN))
+#endif
+#define E3_STEP_INIT() SET_OUTPUT(E3_STEP_PIN)
+#ifndef E3_STEP_WRITE
+  #define E3_STEP_WRITE(STATE) WRITE(E3_STEP_PIN,STATE)
+#endif
+#define E3_STEP_READ() bool(READ(E3_STEP_PIN))
+
+// E4 Stepper
+#ifndef E4_ENABLE_INIT
+  #define E4_ENABLE_INIT() SET_OUTPUT(E4_ENABLE_PIN)
+  #define E4_ENABLE_WRITE(STATE) WRITE(E4_ENABLE_PIN,STATE)
+  #define E4_ENABLE_READ() bool(READ(E4_ENABLE_PIN))
+#endif
+#ifndef E4_DIR_INIT
+  #define E4_DIR_INIT() SET_OUTPUT(E4_DIR_PIN)
+  #define E4_DIR_WRITE(STATE) WRITE(E4_DIR_PIN,STATE)
+  #define E4_DIR_READ() bool(READ(E4_DIR_PIN))
+#endif
+#define E4_STEP_INIT() SET_OUTPUT(E4_STEP_PIN)
+#ifndef E4_STEP_WRITE
+  #define E4_STEP_WRITE(STATE) WRITE(E4_STEP_PIN,STATE)
+#endif
+#define E4_STEP_READ() bool(READ(E4_STEP_PIN))
+
+// E5 Stepper
+#ifndef E5_ENABLE_INIT
+  #define E5_ENABLE_INIT() SET_OUTPUT(E5_ENABLE_PIN)
+  #define E5_ENABLE_WRITE(STATE) WRITE(E5_ENABLE_PIN,STATE)
+  #define E5_ENABLE_READ() bool(READ(E5_ENABLE_PIN))
+#endif
+#ifndef E5_DIR_INIT
+  #define E5_DIR_INIT() SET_OUTPUT(E5_DIR_PIN)
+  #define E5_DIR_WRITE(STATE) WRITE(E5_DIR_PIN,STATE)
+  #define E5_DIR_READ() bool(READ(E5_DIR_PIN))
+#endif
+#define E5_STEP_INIT() SET_OUTPUT(E5_STEP_PIN)
+#ifndef E5_STEP_WRITE
+  #define E5_STEP_WRITE(STATE) WRITE(E5_STEP_PIN,STATE)
+#endif
+#define E5_STEP_READ() bool(READ(E5_STEP_PIN))
+
+// E6 Stepper
+#ifndef E6_ENABLE_INIT
+  #define E6_ENABLE_INIT() SET_OUTPUT(E6_ENABLE_PIN)
+  #define E6_ENABLE_WRITE(STATE) WRITE(E6_ENABLE_PIN,STATE)
+  #define E6_ENABLE_READ() bool(READ(E6_ENABLE_PIN))
+#endif
+#ifndef E6_DIR_INIT
+  #define E6_DIR_INIT() SET_OUTPUT(E6_DIR_PIN)
+  #define E6_DIR_WRITE(STATE) WRITE(E6_DIR_PIN,STATE)
+  #define E6_DIR_READ() bool(READ(E6_DIR_PIN))
+#endif
+#define E6_STEP_INIT() SET_OUTPUT(E6_STEP_PIN)
+#ifndef E6_STEP_WRITE
+  #define E6_STEP_WRITE(STATE) WRITE(E6_STEP_PIN,STATE)
+#endif
+#define E6_STEP_READ() bool(READ(E6_STEP_PIN))
+
+// E7 Stepper
+#ifndef E7_ENABLE_INIT
+  #define E7_ENABLE_INIT() SET_OUTPUT(E7_ENABLE_PIN)
+  #define E7_ENABLE_WRITE(STATE) WRITE(E7_ENABLE_PIN,STATE)
+  #define E7_ENABLE_READ() bool(READ(E7_ENABLE_PIN))
+#endif
+#ifndef E7_DIR_INIT
+  #define E7_DIR_INIT() SET_OUTPUT(E7_DIR_PIN)
+  #define E7_DIR_WRITE(STATE) WRITE(E7_DIR_PIN,STATE)
+  #define E7_DIR_READ() bool(READ(E7_DIR_PIN))
+#endif
+#define E7_STEP_INIT() SET_OUTPUT(E7_STEP_PIN)
+#ifndef E7_STEP_WRITE
+  #define E7_STEP_WRITE(STATE) WRITE(E7_STEP_PIN,STATE)
+#endif
+#define E7_STEP_READ() bool(READ(E7_STEP_PIN))
+
+/**
+ * Extruder indirection for the single E axis
+ */
+#if ENABLED(SWITCHING_EXTRUDER) // One stepper driver per two extruders, reversed on odd index
+  #if EXTRUDERS > 7
+    #define E_STEP_WRITE(E,V) do{ if (E < 2) { E0_STEP_WRITE(V); } else if (E < 4) { E1_STEP_WRITE(V); } else if (E < 6) { E2_STEP_WRITE(V); } else { E3_STEP_WRITE(V); } }while(0)
+    #define   NORM_E_DIR(E)   do{ switch (E) { \
+        case 0: E0_DIR_WRITE(!INVERT_E0_DIR); break; case 1: E0_DIR_WRITE( INVERT_E0_DIR); break; \
+        case 2: E1_DIR_WRITE(!INVERT_E1_DIR); break; case 3: E1_DIR_WRITE( INVERT_E1_DIR); break; \
+        case 4: E2_DIR_WRITE(!INVERT_E2_DIR); break; case 5: E2_DIR_WRITE( INVERT_E2_DIR); break; \
+        case 6: E3_DIR_WRITE( INVERT_E3_DIR); break; case 7: E3_DIR_WRITE( INVERT_E3_DIR); break; \
+      } }while(0)
+    #define    REV_E_DIR(E)   do{ switch (E) { \
+        case 0: E0_DIR_WRITE( INVERT_E0_DIR); break; case 1: E0_DIR_WRITE(!INVERT_E0_DIR); break; \
+        case 2: E1_DIR_WRITE( INVERT_E1_DIR); break; case 3: E1_DIR_WRITE(!INVERT_E1_DIR); break; \
+        case 4: E2_DIR_WRITE( INVERT_E2_DIR); break; case 5: E2_DIR_WRITE(!INVERT_E2_DIR); break; \
+        case 6: E3_DIR_WRITE(!INVERT_E3_DIR); break; case 7: E3_DIR_WRITE(!INVERT_E3_DIR); break; \
+      } }while(0)
+  #elif EXTRUDERS > 6
+    #define E_STEP_WRITE(E,V) do{ if (E < 2) { E0_STEP_WRITE(V); } else if (E < 4) { E1_STEP_WRITE(V); } else if (E < 6) { E2_STEP_WRITE(V); } else { E3_STEP_WRITE(V); } }while(0)
+    #define   NORM_E_DIR(E)   do{ switch (E) { \
+        case 0: E0_DIR_WRITE(!INVERT_E0_DIR); break; case 1: E0_DIR_WRITE( INVERT_E0_DIR); break; \
+        case 2: E1_DIR_WRITE(!INVERT_E1_DIR); break; case 3: E1_DIR_WRITE( INVERT_E1_DIR); break; \
+        case 4: E2_DIR_WRITE(!INVERT_E2_DIR); break; case 5: E2_DIR_WRITE( INVERT_E2_DIR); break; \
+        case 6: E3_DIR_WRITE( INVERT_E3_DIR); break; \
+      } }while(0)
+    #define    REV_E_DIR(E)   do{ switch (E) { \
+        case 0: E0_DIR_WRITE( INVERT_E0_DIR); break; case 1: E0_DIR_WRITE(!INVERT_E0_DIR); break; \
+        case 2: E1_DIR_WRITE( INVERT_E1_DIR); break; case 3: E1_DIR_WRITE(!INVERT_E1_DIR); break; \
+        case 4: E2_DIR_WRITE( INVERT_E2_DIR); break; case 5: E2_DIR_WRITE(!INVERT_E2_DIR); break; \
+        case 6: E3_DIR_WRITE(!INVERT_E3_DIR); } }while(0)
+  #elif EXTRUDERS > 5
+    #define E_STEP_WRITE(E,V) do{ if (E < 2) { E0_STEP_WRITE(V); } else if (E < 4) { E1_STEP_WRITE(V); } else { E2_STEP_WRITE(V); } }while(0)
+    #define   NORM_E_DIR(E)   do{ switch (E) { \
+        case 0: E0_DIR_WRITE(!INVERT_E0_DIR); break; case 1: E0_DIR_WRITE( INVERT_E0_DIR); break; \
+        case 2: E1_DIR_WRITE(!INVERT_E1_DIR); break; case 3: E1_DIR_WRITE( INVERT_E1_DIR); break; \
+        case 4: E2_DIR_WRITE(!INVERT_E2_DIR); break; case 5: E2_DIR_WRITE( INVERT_E2_DIR); break; \
+      } }while(0)
+    #define    REV_E_DIR(E)   do{ switch (E) { \
+        case 0: E0_DIR_WRITE( INVERT_E0_DIR); break; case 1: E0_DIR_WRITE(!INVERT_E0_DIR); break; \
+        case 2: E1_DIR_WRITE( INVERT_E1_DIR); break; case 3: E1_DIR_WRITE(!INVERT_E1_DIR); break; \
+        case 4: E2_DIR_WRITE( INVERT_E2_DIR); break; case 5: E2_DIR_WRITE(!INVERT_E2_DIR); break; \
+      } }while(0)
+  #elif EXTRUDERS > 4
+    #define E_STEP_WRITE(E,V) do{ if (E < 2) { E0_STEP_WRITE(V); } else if (E < 4) { E1_STEP_WRITE(V); } else { E2_STEP_WRITE(V); } }while(0)
+    #define   NORM_E_DIR(E)   do{ switch (E) { \
+        case 0: E0_DIR_WRITE(!INVERT_E0_DIR); break; case 1: E0_DIR_WRITE( INVERT_E0_DIR); break; \
+        case 2: E1_DIR_WRITE(!INVERT_E1_DIR); break; case 3: E1_DIR_WRITE( INVERT_E1_DIR); break; \
+        case 4: E2_DIR_WRITE(!INVERT_E2_DIR); break; \
+      } }while(0)
+    #define    REV_E_DIR(E)   do{ switch (E) { \
+        case 0: E0_DIR_WRITE( INVERT_E0_DIR); break; case 1: E0_DIR_WRITE(!INVERT_E0_DIR); break; \
+        case 2: E1_DIR_WRITE( INVERT_E1_DIR); break; case 3: E1_DIR_WRITE(!INVERT_E1_DIR); break; \
+        case 4: E2_DIR_WRITE( INVERT_E2_DIR); break; \
+      } }while(0)
+  #elif EXTRUDERS > 3
+    #define E_STEP_WRITE(E,V) do{ if (E < 2) { E0_STEP_WRITE(V); } else { E1_STEP_WRITE(V); } }while(0)
+    #define   NORM_E_DIR(E)   do{ switch (E) { \
+        case 0: E0_DIR_WRITE(!INVERT_E0_DIR); break; case 1: E0_DIR_WRITE( INVERT_E0_DIR); break; \
+        case 2: E1_DIR_WRITE(!INVERT_E1_DIR); break; case 3: E1_DIR_WRITE( INVERT_E1_DIR); break; \
+      } }while(0)
+    #define    REV_E_DIR(E)   do{ switch (E) { \
+        case 0: E0_DIR_WRITE( INVERT_E0_DIR); break; case 1: E0_DIR_WRITE(!INVERT_E0_DIR); break; \
+        case 2: E1_DIR_WRITE( INVERT_E1_DIR); break; case 3: E1_DIR_WRITE(!INVERT_E1_DIR); break; \
+      } }while(0)
+  #elif EXTRUDERS > 2
+    #define E_STEP_WRITE(E,V) do{ if (E < 2) { E0_STEP_WRITE(V); } else { E1_STEP_WRITE(V); } }while(0)
+    #define   NORM_E_DIR(E)   do{ switch (E) { \
+        case 0: E0_DIR_WRITE(!INVERT_E0_DIR); break; case 1: E0_DIR_WRITE( INVERT_E0_DIR); break; \
+        case 2: E1_DIR_WRITE(!INVERT_E1_DIR); break; \
+      } }while(0)
+    #define    REV_E_DIR(E)   do{ switch (E) { \
+        case 0: E0_DIR_WRITE( INVERT_E0_DIR); break; case 1: E0_DIR_WRITE(!INVERT_E0_DIR); break; \
+        case 2: E1_DIR_WRITE( INVERT_E1_DIR); break; \
+      } }while(0)
+  #else
+    #define E_STEP_WRITE(E,V) E0_STEP_WRITE(V)
+    #define   NORM_E_DIR(E)   do{ E0_DIR_WRITE(E ?  INVERT_E0_DIR : !INVERT_E0_DIR); }while(0)
+    #define    REV_E_DIR(E)   do{ E0_DIR_WRITE(E ? !INVERT_E0_DIR :  INVERT_E0_DIR); }while(0)
+  #endif
+
+#elif HAS_PRUSA_MMU2
+
+  #define E_STEP_WRITE(E,V) E0_STEP_WRITE(V)
+  #define   NORM_E_DIR(E)   E0_DIR_WRITE(!INVERT_E0_DIR)
+  #define    REV_E_DIR(E)   E0_DIR_WRITE( INVERT_E0_DIR)
+
+#elif HAS_PRUSA_MMU1  // One multiplexed stepper driver, reversed on odd index
+
+  #define E_STEP_WRITE(E,V) E0_STEP_WRITE(V)
+  #define   NORM_E_DIR(E)   do{ E0_DIR_WRITE(TEST(E, 0) ? !INVERT_E0_DIR:  INVERT_E0_DIR); }while(0)
+  #define    REV_E_DIR(E)   do{ E0_DIR_WRITE(TEST(E, 0) ?  INVERT_E0_DIR: !INVERT_E0_DIR); }while(0)
+
+#elif E_STEPPERS > 1
+
+  #if E_STEPPERS > 7
+
+    #define _E_STEP_WRITE(E,V) do{ switch (E) { \
+        case 0: E0_STEP_WRITE(V); break; case 1: E1_STEP_WRITE(V); break; case 2: E2_STEP_WRITE(V); break; case 3: E3_STEP_WRITE(V); break; \
+        case 4: E4_STEP_WRITE(V); break; case 5: E5_STEP_WRITE(V); break; case 6: E6_STEP_WRITE(V); break; case 7: E7_STEP_WRITE(V); break; \
+      } }while(0)
+    #define   _NORM_E_DIR(E)   do{ switch (E) { \
+        case 0: E0_DIR_WRITE(!INVERT_E0_DIR); break; case 1: E1_DIR_WRITE(!INVERT_E1_DIR); break; \
+        case 2: E2_DIR_WRITE(!INVERT_E2_DIR); break; case 3: E3_DIR_WRITE(!INVERT_E3_DIR); break; \
+        case 4: E4_DIR_WRITE(!INVERT_E4_DIR); break; case 5: E5_DIR_WRITE(!INVERT_E5_DIR); break; \
+        case 6: E6_DIR_WRITE(!INVERT_E6_DIR); break; case 7: E7_DIR_WRITE(!INVERT_E7_DIR); break; \
+      } }while(0)
+    #define    _REV_E_DIR(E)   do{ switch (E) { \
+        case 0: E0_DIR_WRITE( INVERT_E0_DIR); break; case 1: E1_DIR_WRITE( INVERT_E1_DIR); break; \
+        case 2: E2_DIR_WRITE( INVERT_E2_DIR); break; case 3: E3_DIR_WRITE( INVERT_E3_DIR); break; \
+        case 4: E4_DIR_WRITE( INVERT_E4_DIR); break; case 5: E5_DIR_WRITE( INVERT_E5_DIR); break; \
+        case 6: E6_DIR_WRITE( INVERT_E6_DIR); break; case 7: E7_DIR_WRITE( INVERT_E7_DIR); break; \
+      } }while(0)
+
+  #elif E_STEPPERS > 6
+
+    #define _E_STEP_WRITE(E,V) do{ switch (E) { \
+        case 0: E0_STEP_WRITE(V); break; case 1: E1_STEP_WRITE(V); break; case 2: E2_STEP_WRITE(V); break; case 3: E3_STEP_WRITE(V); break; \
+        case 4: E4_STEP_WRITE(V); break; case 5: E5_STEP_WRITE(V); break; case 6: E6_STEP_WRITE(V); break; \
+      } }while(0)
+    #define   _NORM_E_DIR(E)   do{ switch (E) { \
+        case 0: E0_DIR_WRITE(!INVERT_E0_DIR); break; case 1: E1_DIR_WRITE(!INVERT_E1_DIR); break; \
+        case 2: E2_DIR_WRITE(!INVERT_E2_DIR); break; case 3: E3_DIR_WRITE(!INVERT_E3_DIR); break; \
+        case 4: E4_DIR_WRITE(!INVERT_E4_DIR); break; case 5: E5_DIR_WRITE(!INVERT_E5_DIR); break; \
+        case 6: E6_DIR_WRITE(!INVERT_E6_DIR); break; \
+      } }while(0)
+    #define    _REV_E_DIR(E)   do{ switch (E) { \
+        case 0: E0_DIR_WRITE( INVERT_E0_DIR); break; case 1: E1_DIR_WRITE( INVERT_E1_DIR); break; \
+        case 2: E2_DIR_WRITE( INVERT_E2_DIR); break; case 3: E3_DIR_WRITE( INVERT_E3_DIR); break; \
+        case 4: E4_DIR_WRITE( INVERT_E4_DIR); break; case 5: E5_DIR_WRITE( INVERT_E5_DIR); break; \
+        case 6: E6_DIR_WRITE( INVERT_E6_DIR); break; \
+      } }while(0)
+
+  #elif E_STEPPERS > 5
+
+    #define _E_STEP_WRITE(E,V) do{ switch (E) { \
+        case 0: E0_STEP_WRITE(V); break; case 1: E1_STEP_WRITE(V); break; case 2: E2_STEP_WRITE(V); break; case 3: E3_STEP_WRITE(V); break; \
+        case 4: E4_STEP_WRITE(V); break; case 5: E5_STEP_WRITE(V); break; \
+      } }while(0)
+    #define   _NORM_E_DIR(E)   do{ switch (E) { \
+        case 0: E0_DIR_WRITE(!INVERT_E0_DIR); break; case 1: E1_DIR_WRITE(!INVERT_E1_DIR); break; \
+        case 2: E2_DIR_WRITE(!INVERT_E2_DIR); break; case 3: E3_DIR_WRITE(!INVERT_E3_DIR); break; \
+        case 4: E4_DIR_WRITE(!INVERT_E4_DIR); break; case 5: E5_DIR_WRITE(!INVERT_E5_DIR); break; \
+      } }while(0)
+    #define    _REV_E_DIR(E)   do{ switch (E) { \
+        case 0: E0_DIR_WRITE( INVERT_E0_DIR); break; case 1: E1_DIR_WRITE( INVERT_E1_DIR); break; \
+        case 2: E2_DIR_WRITE( INVERT_E2_DIR); break; case 3: E3_DIR_WRITE( INVERT_E3_DIR); break; \
+        case 4: E4_DIR_WRITE( INVERT_E4_DIR); break; case 5: E5_DIR_WRITE( INVERT_E5_DIR); break; \
+      } }while(0)
+
+  #elif E_STEPPERS > 4
+
+    #define _E_STEP_WRITE(E,V) do{ switch (E) { \
+        case 0: E0_STEP_WRITE(V); break; case 1: E1_STEP_WRITE(V); break; case 2: E2_STEP_WRITE(V); break; case 3: E3_STEP_WRITE(V); break; \
+        case 4: E4_STEP_WRITE(V); break; \
+      } }while(0)
+    #define   _NORM_E_DIR(E)   do{ switch (E) { \
+        case 0: E0_DIR_WRITE(!INVERT_E0_DIR); break; case 1: E1_DIR_WRITE(!INVERT_E1_DIR); break; \
+        case 2: E2_DIR_WRITE(!INVERT_E2_DIR); break; case 3: E3_DIR_WRITE(!INVERT_E3_DIR); break; \
+        case 4: E4_DIR_WRITE(!INVERT_E4_DIR); break; \
+      } }while(0)
+    #define    _REV_E_DIR(E)   do{ switch (E) { \
+        case 0: E0_DIR_WRITE( INVERT_E0_DIR); break; case 1: E1_DIR_WRITE( INVERT_E1_DIR); break; \
+        case 2: E2_DIR_WRITE( INVERT_E2_DIR); break; case 3: E3_DIR_WRITE( INVERT_E3_DIR); break; \
+        case 4: E4_DIR_WRITE( INVERT_E4_DIR); break; \
+      } }while(0)
+
+  #elif E_STEPPERS > 3
+
+    #define _E_STEP_WRITE(E,V) do{ switch (E) { \
+        case 0: E0_STEP_WRITE(V); break; case 1: E1_STEP_WRITE(V); break; case 2: E2_STEP_WRITE(V); break; case 3: E3_STEP_WRITE(V); break; \
+      } }while(0)
+    #define   _NORM_E_DIR(E)   do{ switch (E) { \
+        case 0: E0_DIR_WRITE(!INVERT_E0_DIR); break; case 1: E1_DIR_WRITE(!INVERT_E1_DIR); break; \
+        case 2: E2_DIR_WRITE(!INVERT_E2_DIR); break; case 3: E3_DIR_WRITE(!INVERT_E3_DIR); break; \
+      } }while(0)
+    #define    _REV_E_DIR(E)   do{ switch (E) { \
+        case 0: E0_DIR_WRITE( INVERT_E0_DIR); break; case 1: E1_DIR_WRITE( INVERT_E1_DIR); break; \
+        case 2: E2_DIR_WRITE( INVERT_E2_DIR); break; case 3: E3_DIR_WRITE( INVERT_E3_DIR); break; \
+      } }while(0)
+
+  #elif E_STEPPERS > 2
+
+    #define _E_STEP_WRITE(E,V) do{ switch (E) { case 0: E0_STEP_WRITE(V); break; case 1: E1_STEP_WRITE(V); break; case 2: E2_STEP_WRITE(V); } }while(0)
+    #define   _NORM_E_DIR(E)   do{ switch (E) { case 0: E0_DIR_WRITE(!INVERT_E0_DIR); break; case 1: E1_DIR_WRITE(!INVERT_E1_DIR); break; case 2: E2_DIR_WRITE(!INVERT_E2_DIR); } }while(0)
+    #define    _REV_E_DIR(E)   do{ switch (E) { case 0: E0_DIR_WRITE( INVERT_E0_DIR); break; case 1: E1_DIR_WRITE( INVERT_E1_DIR); break; case 2: E2_DIR_WRITE( INVERT_E2_DIR); } }while(0)
+
+  #else
+
+    #define _E_STEP_WRITE(E,V) do{ if (E == 0) { E0_STEP_WRITE(V); } else { E1_STEP_WRITE(V); } }while(0)
+    #define   _NORM_E_DIR(E)   do{ if (E == 0) { E0_DIR_WRITE(!INVERT_E0_DIR); } else { E1_DIR_WRITE(!INVERT_E1_DIR); } }while(0)
+    #define    _REV_E_DIR(E)   do{ if (E == 0) { E0_DIR_WRITE( INVERT_E0_DIR); } else { E1_DIR_WRITE( INVERT_E1_DIR); } }while(0)
+  #endif
+
+  #if HAS_DUPLICATION_MODE
+
+    #if ENABLED(MULTI_NOZZLE_DUPLICATION)
+      #define _DUPE(N,T,V)  do{ if (TEST(duplication_e_mask, N)) E##N##_##T##_WRITE(V); }while(0)
+    #else
+      #define _DUPE(N,T,V)  E##N##_##T##_WRITE(V)
+    #endif
+
+    #define NDIR(N) _DUPE(N,DIR,!INVERT_E##N##_DIR)
+    #define RDIR(N) _DUPE(N,DIR, INVERT_E##N##_DIR)
+
+    #define E_STEP_WRITE(E,V) do{ if (extruder_duplication_enabled) { DUPE(STEP,V); } else _E_STEP_WRITE(E,V); }while(0)
+
+    #if E_STEPPERS > 2
+      #if E_STEPPERS > 7
+        #define DUPE(T,V)     do{ _DUPE(0,T,V); _DUPE(1,T,V); _DUPE(2,T,V); _DUPE(3,T,V); _DUPE(4,T,V); _DUPE(5,T,V); _DUPE(6,T,V); _DUPE(7,T,V); }while(0)
+        #define NORM_E_DIR(E) do{ if (extruder_duplication_enabled) { NDIR(0); NDIR(1); NDIR(2); NDIR(3); NDIR(4); NDIR(5); NDIR(6); NDIR(7); } else _NORM_E_DIR(E); }while(0)
+        #define REV_E_DIR(E)  do{ if (extruder_duplication_enabled) { RDIR(0); RDIR(1); RDIR(2); RDIR(3); RDIR(4); RDIR(5); RDIR(6); RDIR(7); } else  _REV_E_DIR(E); }while(0)
+      #elif E_STEPPERS > 6
+        #define DUPE(T,V)     do{ _DUPE(0,T,V); _DUPE(1,T,V); _DUPE(2,T,V); _DUPE(3,T,V); _DUPE(4,T,V); _DUPE(5,T,V); _DUPE(6,T,V); }while(0)
+        #define NORM_E_DIR(E) do{ if (extruder_duplication_enabled) { NDIR(0); NDIR(1); NDIR(2); NDIR(3); NDIR(4); NDIR(5); NDIR(6); } else _NORM_E_DIR(E); }while(0)
+        #define REV_E_DIR(E)  do{ if (extruder_duplication_enabled) { RDIR(0); RDIR(1); RDIR(2); RDIR(3); RDIR(4); RDIR(5); RDIR(6); } else  _REV_E_DIR(E); }while(0)
+      #elif E_STEPPERS > 5
+        #define DUPE(T,V)     do{ _DUPE(0,T,V); _DUPE(1,T,V); _DUPE(2,T,V); _DUPE(3,T,V); _DUPE(4,T,V); _DUPE(5,T,V); }while(0)
+        #define NORM_E_DIR(E) do{ if (extruder_duplication_enabled) { NDIR(0); NDIR(1); NDIR(2); NDIR(3); NDIR(4); NDIR(5); } else _NORM_E_DIR(E); }while(0)
+        #define REV_E_DIR(E)  do{ if (extruder_duplication_enabled) { RDIR(0); RDIR(1); RDIR(2); RDIR(3); RDIR(4); RDIR(5); } else  _REV_E_DIR(E); }while(0)
+      #elif E_STEPPERS > 4
+        #define DUPE(T,V)     do{ _DUPE(0,T,V); _DUPE(1,T,V); _DUPE(2,T,V); _DUPE(3,T,V); _DUPE(4,T,V); }while(0)
+        #define NORM_E_DIR(E) do{ if (extruder_duplication_enabled) { NDIR(0); NDIR(1); NDIR(2); NDIR(3); NDIR(4); } else _NORM_E_DIR(E); }while(0)
+        #define REV_E_DIR(E)  do{ if (extruder_duplication_enabled) { RDIR(0); RDIR(1); RDIR(2); RDIR(3); RDIR(4); } else  _REV_E_DIR(E); }while(0)
+      #elif E_STEPPERS > 3
+        #define DUPE(T,V)     do{ _DUPE(0,T,V); _DUPE(1,T,V); _DUPE(2,T,V); _DUPE(3,T,V); }while(0)
+        #define NORM_E_DIR(E) do{ if (extruder_duplication_enabled) { NDIR(0); NDIR(1); NDIR(2); NDIR(3); } else _NORM_E_DIR(E); }while(0)
+        #define REV_E_DIR(E)  do{ if (extruder_duplication_enabled) { RDIR(0); RDIR(1); RDIR(2); RDIR(3); } else  _REV_E_DIR(E); }while(0)
+      #else
+        #define DUPE(T,V)     do{ _DUPE(0,T,V); _DUPE(1,T,V); _DUPE(2,T,V); }while(0)
+        #define NORM_E_DIR(E) do{ if (extruder_duplication_enabled) { NDIR(0); NDIR(1); NDIR(2); } else _NORM_E_DIR(E); }while(0)
+        #define REV_E_DIR(E)  do{ if (extruder_duplication_enabled) { RDIR(0); RDIR(1); RDIR(2); } else  _REV_E_DIR(E); }while(0)
+      #endif
+    #else
+      #define DUPE(T,V)     do{ _DUPE(0,T,V); _DUPE(1,T,V); }while(0)
+      #define NORM_E_DIR(E) do{ if (extruder_duplication_enabled) { NDIR(0); NDIR(1); } else _NORM_E_DIR(E); }while(0)
+      #define REV_E_DIR(E)  do{ if (extruder_duplication_enabled) { RDIR(0); RDIR(1); } else  _REV_E_DIR(E); }while(0)
+    #endif
+
+  #else
+
+    #define E_STEP_WRITE(E,V) _E_STEP_WRITE(E,V)
+    #define   NORM_E_DIR(E)   _NORM_E_DIR(E)
+    #define    REV_E_DIR(E)   _REV_E_DIR(E)
+
+  #endif
+
+#elif E_STEPPERS
+  #define E_STEP_WRITE(E,V) E0_STEP_WRITE(V)
+  #define   NORM_E_DIR(E)   E0_DIR_WRITE(!INVERT_E0_DIR)
+  #define    REV_E_DIR(E)   E0_DIR_WRITE( INVERT_E0_DIR)
+
+#else
+  #define E_STEP_WRITE(E,V) NOOP
+  #define   NORM_E_DIR(E)   NOOP
+  #define    REV_E_DIR(E)   NOOP
+
+#endif
+
+//
+// Individual stepper enable / disable macros
+//
+
+#ifndef ENABLE_STEPPER_X
+  #if HAS_X_ENABLE
+    #define  ENABLE_STEPPER_X() X_ENABLE_WRITE( X_ENABLE_ON)
+  #else
+    #define  ENABLE_STEPPER_X() NOOP
+  #endif
+#endif
+#ifndef DISABLE_STEPPER_X
+  #if HAS_X_ENABLE
+    #define DISABLE_STEPPER_X() X_ENABLE_WRITE(!X_ENABLE_ON)
+  #else
+    #define DISABLE_STEPPER_X() NOOP
+  #endif
+#endif
+
+#ifndef ENABLE_STEPPER_X2
+  #if HAS_X2_ENABLE
+    #define  ENABLE_STEPPER_X2() X2_ENABLE_WRITE( X_ENABLE_ON)
+  #else
+    #define  ENABLE_STEPPER_X2() NOOP
+  #endif
+#endif
+#ifndef DISABLE_STEPPER_X2
+  #if HAS_X2_ENABLE
+    #define DISABLE_STEPPER_X2() X2_ENABLE_WRITE(!X_ENABLE_ON)
+  #else
+    #define DISABLE_STEPPER_X2() NOOP
+  #endif
+#endif
+
+#ifndef ENABLE_STEPPER_Y
+  #if HAS_Y_ENABLE
+    #define  ENABLE_STEPPER_Y() Y_ENABLE_WRITE( Y_ENABLE_ON)
+  #else
+    #define  ENABLE_STEPPER_Y() NOOP
+  #endif
+#endif
+#ifndef DISABLE_STEPPER_Y
+  #if HAS_Y_ENABLE
+    #define DISABLE_STEPPER_Y() Y_ENABLE_WRITE(!Y_ENABLE_ON)
+  #else
+    #define DISABLE_STEPPER_Y() NOOP
+  #endif
+#endif
+
+#ifndef ENABLE_STEPPER_Y2
+  #if HAS_Y2_ENABLE
+    #define  ENABLE_STEPPER_Y2() Y2_ENABLE_WRITE( Y_ENABLE_ON)
+  #else
+    #define  ENABLE_STEPPER_Y2() NOOP
+  #endif
+#endif
+#ifndef DISABLE_STEPPER_Y2
+  #if HAS_Y2_ENABLE
+    #define DISABLE_STEPPER_Y2() Y2_ENABLE_WRITE(!Y_ENABLE_ON)
+  #else
+    #define DISABLE_STEPPER_Y2() NOOP
+  #endif
+#endif
+
+#ifndef ENABLE_STEPPER_Z
+  #if HAS_Z_ENABLE
+    #define  ENABLE_STEPPER_Z() Z_ENABLE_WRITE( Z_ENABLE_ON)
+  #else
+    #define  ENABLE_STEPPER_Z() NOOP
+  #endif
+#endif
+#ifndef DISABLE_STEPPER_Z
+  #if HAS_Z_ENABLE
+    #define DISABLE_STEPPER_Z() Z_ENABLE_WRITE(!Z_ENABLE_ON)
+  #else
+    #define DISABLE_STEPPER_Z() NOOP
+  #endif
+#endif
+
+#ifndef ENABLE_STEPPER_Z2
+  #if HAS_Z2_ENABLE
+    #define  ENABLE_STEPPER_Z2() Z2_ENABLE_WRITE( Z_ENABLE_ON)
+  #else
+    #define  ENABLE_STEPPER_Z2() NOOP
+  #endif
+#endif
+#ifndef DISABLE_STEPPER_Z2
+  #if HAS_Z2_ENABLE
+    #define DISABLE_STEPPER_Z2() Z2_ENABLE_WRITE(!Z_ENABLE_ON)
+  #else
+    #define DISABLE_STEPPER_Z2() NOOP
+  #endif
+#endif
+
+#ifndef ENABLE_STEPPER_Z3
+  #if HAS_Z3_ENABLE
+    #define  ENABLE_STEPPER_Z3() Z3_ENABLE_WRITE( Z_ENABLE_ON)
+  #else
+    #define  ENABLE_STEPPER_Z3() NOOP
+  #endif
+#endif
+#ifndef DISABLE_STEPPER_Z3
+  #if HAS_Z3_ENABLE
+    #define DISABLE_STEPPER_Z3() Z3_ENABLE_WRITE(!Z_ENABLE_ON)
+  #else
+    #define DISABLE_STEPPER_Z3() NOOP
+  #endif
+#endif
+
+#ifndef ENABLE_STEPPER_Z4
+  #if HAS_Z4_ENABLE
+    #define  ENABLE_STEPPER_Z4() Z4_ENABLE_WRITE( Z_ENABLE_ON)
+  #else
+    #define  ENABLE_STEPPER_Z4() NOOP
+  #endif
+#endif
+#ifndef DISABLE_STEPPER_Z4
+  #if HAS_Z4_ENABLE
+    #define DISABLE_STEPPER_Z4() Z4_ENABLE_WRITE(!Z_ENABLE_ON)
+  #else
+    #define DISABLE_STEPPER_Z4() NOOP
+  #endif
+#endif
+
+#ifndef ENABLE_STEPPER_E0
+  #if HAS_E0_ENABLE
+    #define  ENABLE_STEPPER_E0() E0_ENABLE_WRITE( E_ENABLE_ON)
+  #else
+    #define  ENABLE_STEPPER_E0() NOOP
+  #endif
+#endif
+#ifndef DISABLE_STEPPER_E0
+  #if HAS_E0_ENABLE
+    #define DISABLE_STEPPER_E0() E0_ENABLE_WRITE(!E_ENABLE_ON)
+  #else
+    #define DISABLE_STEPPER_E0() NOOP
+  #endif
+#endif
+
+#ifndef ENABLE_STEPPER_E1
+  #if E_STEPPERS > 1 && HAS_E1_ENABLE
+    #define  ENABLE_STEPPER_E1() E1_ENABLE_WRITE( E_ENABLE_ON)
+  #else
+    #define  ENABLE_STEPPER_E1() NOOP
+  #endif
+#endif
+#ifndef DISABLE_STEPPER_E1
+  #if E_STEPPERS > 1 && HAS_E1_ENABLE
+    #define DISABLE_STEPPER_E1() E1_ENABLE_WRITE(!E_ENABLE_ON)
+  #else
+    #define DISABLE_STEPPER_E1() NOOP
+  #endif
+#endif
+
+#ifndef ENABLE_STEPPER_E2
+  #if E_STEPPERS > 2 && HAS_E2_ENABLE
+    #define  ENABLE_STEPPER_E2() E2_ENABLE_WRITE( E_ENABLE_ON)
+  #else
+    #define  ENABLE_STEPPER_E2() NOOP
+  #endif
+#endif
+#ifndef DISABLE_STEPPER_E2
+  #if E_STEPPERS > 2 && HAS_E2_ENABLE
+    #define DISABLE_STEPPER_E2() E2_ENABLE_WRITE(!E_ENABLE_ON)
+  #else
+    #define DISABLE_STEPPER_E2() NOOP
+  #endif
+#endif
+
+#ifndef ENABLE_STEPPER_E3
+  #if E_STEPPERS > 3 && HAS_E3_ENABLE
+    #define  ENABLE_STEPPER_E3() E3_ENABLE_WRITE( E_ENABLE_ON)
+  #else
+    #define  ENABLE_STEPPER_E3() NOOP
+  #endif
+#endif
+#ifndef DISABLE_STEPPER_E3
+  #if E_STEPPERS > 3 && HAS_E3_ENABLE
+    #define DISABLE_STEPPER_E3() E3_ENABLE_WRITE(!E_ENABLE_ON)
+  #else
+    #define DISABLE_STEPPER_E3() NOOP
+  #endif
+#endif
+
+#ifndef ENABLE_STEPPER_E4
+  #if E_STEPPERS > 4 && HAS_E4_ENABLE
+    #define  ENABLE_STEPPER_E4() E4_ENABLE_WRITE( E_ENABLE_ON)
+  #else
+    #define  ENABLE_STEPPER_E4() NOOP
+  #endif
+#endif
+#ifndef DISABLE_STEPPER_E4
+  #if E_STEPPERS > 4 && HAS_E4_ENABLE
+    #define DISABLE_STEPPER_E4() E4_ENABLE_WRITE(!E_ENABLE_ON)
+  #else
+    #define DISABLE_STEPPER_E4() NOOP
+  #endif
+#endif
+
+#ifndef ENABLE_STEPPER_E5
+  #if E_STEPPERS > 5 && HAS_E5_ENABLE
+    #define  ENABLE_STEPPER_E5() E5_ENABLE_WRITE( E_ENABLE_ON)
+  #else
+    #define  ENABLE_STEPPER_E5() NOOP
+  #endif
+#endif
+#ifndef DISABLE_STEPPER_E5
+  #if E_STEPPERS > 5 && HAS_E5_ENABLE
+    #define DISABLE_STEPPER_E5() E5_ENABLE_WRITE(!E_ENABLE_ON)
+  #else
+    #define DISABLE_STEPPER_E5() NOOP
+  #endif
+#endif
+
+#ifndef ENABLE_STEPPER_E6
+  #if E_STEPPERS > 6 && HAS_E6_ENABLE
+    #define  ENABLE_STEPPER_E6() E6_ENABLE_WRITE( E_ENABLE_ON)
+  #else
+    #define  ENABLE_STEPPER_E6() NOOP
+  #endif
+#endif
+#ifndef DISABLE_STEPPER_E6
+  #if E_STEPPERS > 6 && HAS_E6_ENABLE
+    #define DISABLE_STEPPER_E6() E6_ENABLE_WRITE(!E_ENABLE_ON)
+  #else
+    #define DISABLE_STEPPER_E6() NOOP
+  #endif
+#endif
+
+#ifndef ENABLE_STEPPER_E7
+  #if E_STEPPERS > 7 && HAS_E7_ENABLE
+    #define  ENABLE_STEPPER_E7() E7_ENABLE_WRITE( E_ENABLE_ON)
+  #else
+    #define  ENABLE_STEPPER_E7() NOOP
+  #endif
+#endif
+#ifndef DISABLE_STEPPER_E7
+  #if E_STEPPERS > 7 && HAS_E7_ENABLE
+    #define DISABLE_STEPPER_E7() E7_ENABLE_WRITE(!E_ENABLE_ON)
+  #else
+    #define DISABLE_STEPPER_E7() NOOP
+  #endif
+#endif
+
+//
+// Axis steppers enable / disable macros
+//
+#if ENABLED(SOFTWARE_DRIVER_ENABLE)
+  // Avoid expensive calls to enable / disable steppers
+  extern xyz_bool_t axis_sw_enabled;
+  #define SHOULD_ENABLE(N)  !axis_sw_enabled.N
+  #define SHOULD_DISABLE(N)  axis_sw_enabled.N
+  #define AFTER_CHANGE(N,TF) axis_sw_enabled.N = TF
+#else
+  #define SHOULD_ENABLE(N)   true
+  #define SHOULD_DISABLE(N)  true
+  #define AFTER_CHANGE(N,TF) NOOP
+#endif
+
+#define  ENABLE_AXIS_X() if (SHOULD_ENABLE(x))  {  ENABLE_STEPPER_X();  ENABLE_STEPPER_X2(); AFTER_CHANGE(x, true); }
+#define DISABLE_AXIS_X() if (SHOULD_DISABLE(x)) { DISABLE_STEPPER_X(); DISABLE_STEPPER_X2(); AFTER_CHANGE(x, false); set_axis_untrusted(X_AXIS); }
+#define  ENABLE_AXIS_Y() if (SHOULD_ENABLE(y))  {  ENABLE_STEPPER_Y();  ENABLE_STEPPER_Y2(); AFTER_CHANGE(y, true); }
+#define DISABLE_AXIS_Y() if (SHOULD_DISABLE(y)) { DISABLE_STEPPER_Y(); DISABLE_STEPPER_Y2(); AFTER_CHANGE(y, false); set_axis_untrusted(Y_AXIS); }
+#define  ENABLE_AXIS_Z() if (SHOULD_ENABLE(z))  {  ENABLE_STEPPER_Z();  ENABLE_STEPPER_Z2();  ENABLE_STEPPER_Z3();  ENABLE_STEPPER_Z4(); AFTER_CHANGE(z, true); }
+#define DISABLE_AXIS_Z() if (SHOULD_DISABLE(z)) { DISABLE_STEPPER_Z(); DISABLE_STEPPER_Z2(); DISABLE_STEPPER_Z3(); DISABLE_STEPPER_Z4(); AFTER_CHANGE(z, false); set_axis_untrusted(Z_AXIS); Z_RESET(); }
+
+#ifdef Z_AFTER_DEACTIVATE
+  #define Z_RESET() do{ current_position.z = Z_AFTER_DEACTIVATE; sync_plan_position(); }while(0)
+#else
+  #define Z_RESET()
+#endif
+
+//
+// Extruder steppers enable / disable macros
+//
+
+#if ENABLED(MIXING_EXTRUDER)
+  /**
+   * Mixing steppers keep all their enable (and direction) states synchronized
+   */
+  #define _CALL_ENA_E(N)  ENABLE_STEPPER_E##N () ;
+  #define _CALL_DIS_E(N) DISABLE_STEPPER_E##N () ;
+  #define  ENABLE_AXIS_E0() { RREPEAT(MIXING_STEPPERS, _CALL_ENA_E) }
+  #define DISABLE_AXIS_E0() { RREPEAT(MIXING_STEPPERS, _CALL_DIS_E) }
+#endif
+
+#ifndef ENABLE_AXIS_E0
+  #if E_STEPPERS && HAS_E0_ENABLE
+    #define  ENABLE_AXIS_E0() ENABLE_STEPPER_E0()
+  #else
+    #define  ENABLE_AXIS_E0() NOOP
+  #endif
+#endif
+#ifndef DISABLE_AXIS_E0
+  #if E_STEPPERS && HAS_E0_ENABLE
+    #define DISABLE_AXIS_E0() DISABLE_STEPPER_E0()
+  #else
+    #define DISABLE_AXIS_E0() NOOP
+  #endif
+#endif
+
+#ifndef ENABLE_AXIS_E1
+  #if E_STEPPERS > 1 && HAS_E1_ENABLE
+    #define  ENABLE_AXIS_E1() ENABLE_STEPPER_E1()
+  #else
+    #define  ENABLE_AXIS_E1() NOOP
+  #endif
+#endif
+#ifndef DISABLE_AXIS_E1
+  #if E_STEPPERS > 1 && HAS_E1_ENABLE
+    #define DISABLE_AXIS_E1() DISABLE_STEPPER_E1()
+  #else
+    #define DISABLE_AXIS_E1() NOOP
+  #endif
+#endif
+
+#ifndef ENABLE_AXIS_E2
+  #if E_STEPPERS > 2 && HAS_E2_ENABLE
+    #define  ENABLE_AXIS_E2() ENABLE_STEPPER_E2()
+  #else
+    #define  ENABLE_AXIS_E2() NOOP
+  #endif
+#endif
+#ifndef DISABLE_AXIS_E2
+  #if E_STEPPERS > 2 && HAS_E2_ENABLE
+    #define DISABLE_AXIS_E2() DISABLE_STEPPER_E2()
+  #else
+    #define DISABLE_AXIS_E2() NOOP
+  #endif
+#endif
+
+#ifndef ENABLE_AXIS_E3
+  #if E_STEPPERS > 3 && HAS_E3_ENABLE
+    #define  ENABLE_AXIS_E3() ENABLE_STEPPER_E3()
+  #else
+    #define  ENABLE_AXIS_E3() NOOP
+  #endif
+#endif
+#ifndef DISABLE_AXIS_E3
+  #if E_STEPPERS > 3 && HAS_E3_ENABLE
+    #define DISABLE_AXIS_E3() DISABLE_STEPPER_E3()
+  #else
+    #define DISABLE_AXIS_E3() NOOP
+  #endif
+#endif
+
+#ifndef ENABLE_AXIS_E4
+  #if E_STEPPERS > 4 && HAS_E4_ENABLE
+    #define  ENABLE_AXIS_E4() ENABLE_STEPPER_E4()
+  #else
+    #define  ENABLE_AXIS_E4() NOOP
+  #endif
+#endif
+#ifndef DISABLE_AXIS_E4
+  #if E_STEPPERS > 4 && HAS_E4_ENABLE
+    #define DISABLE_AXIS_E4() DISABLE_STEPPER_E4()
+  #else
+    #define DISABLE_AXIS_E4() NOOP
+  #endif
+#endif
+
+#ifndef ENABLE_AXIS_E5
+  #if E_STEPPERS > 5 && HAS_E5_ENABLE
+    #define  ENABLE_AXIS_E5() ENABLE_STEPPER_E5()
+  #else
+    #define  ENABLE_AXIS_E5() NOOP
+  #endif
+#endif
+#ifndef DISABLE_AXIS_E5
+  #if E_STEPPERS > 5 && HAS_E5_ENABLE
+    #define DISABLE_AXIS_E5() DISABLE_STEPPER_E5()
+  #else
+    #define DISABLE_AXIS_E5() NOOP
+  #endif
+#endif
+
+#ifndef ENABLE_AXIS_E6
+  #if E_STEPPERS > 6 && HAS_E6_ENABLE
+    #define  ENABLE_AXIS_E6() ENABLE_STEPPER_E6()
+  #else
+    #define  ENABLE_AXIS_E6() NOOP
+  #endif
+#endif
+#ifndef DISABLE_AXIS_E6
+  #if E_STEPPERS > 6 && HAS_E6_ENABLE
+    #define DISABLE_AXIS_E6() DISABLE_STEPPER_E6()
+  #else
+    #define DISABLE_AXIS_E6() NOOP
+  #endif
+#endif
+
+#ifndef ENABLE_AXIS_E7
+  #if E_STEPPERS > 7 && HAS_E7_ENABLE
+    #define  ENABLE_AXIS_E7() ENABLE_STEPPER_E7()
+  #else
+    #define  ENABLE_AXIS_E7() NOOP
+  #endif
+#endif
+#ifndef DISABLE_AXIS_E7
+  #if E_STEPPERS > 7 && HAS_E7_ENABLE
+    #define DISABLE_AXIS_E7() DISABLE_STEPPER_E7()
+  #else
+    #define DISABLE_AXIS_E7() NOOP
+  #endif
+#endif
diff --git a/Marlin/src/module/stepper/trinamic.cpp b/Marlin/src/module/stepper/trinamic.cpp
new file mode 100644
index 0000000..c33581d
--- /dev/null
+++ b/Marlin/src/module/stepper/trinamic.cpp
@@ -0,0 +1,877 @@
+/**
+ * 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/>.
+ *
+ */
+
+/**
+ * stepper/trinamic.cpp
+ * Stepper driver indirection for Trinamic
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if HAS_TRINAMIC_CONFIG
+
+#include "trinamic.h"
+#include "../stepper.h"
+
+#include <HardwareSerial.h>
+#include <SPI.h>
+
+enum StealthIndex : uint8_t { STEALTH_AXIS_XY, STEALTH_AXIS_Z, STEALTH_AXIS_E };
+#define TMC_INIT(ST, STEALTH_INDEX) tmc_init(stepper##ST, ST##_CURRENT, ST##_MICROSTEPS, ST##_HYBRID_THRESHOLD, stealthchop_by_axis[STEALTH_INDEX], chopper_timing_##ST, ST##_INTERPOLATE)
+
+//   IC = TMC model number
+//   ST = Stepper object letter
+//   L  = Label characters
+//   AI = Axis Enum Index
+// SWHW = SW/SH UART selection
+#if ENABLED(TMC_USE_SW_SPI)
+  #define __TMC_SPI_DEFINE(IC, ST, L, AI) TMCMarlin<IC##Stepper, L, AI> stepper##ST(ST##_CS_PIN, float(ST##_RSENSE), TMC_SW_MOSI, TMC_SW_MISO, TMC_SW_SCK, ST##_CHAIN_POS)
+#else
+  #define __TMC_SPI_DEFINE(IC, ST, L, AI) TMCMarlin<IC##Stepper, L, AI> stepper##ST(ST##_CS_PIN, float(ST##_RSENSE), ST##_CHAIN_POS)
+#endif
+
+#if ENABLED(TMC_SERIAL_MULTIPLEXER)
+  #define TMC_UART_HW_DEFINE(IC, ST, L, AI) TMCMarlin<IC##Stepper, L, AI> stepper##ST(&ST##_HARDWARE_SERIAL, float(ST##_RSENSE), ST##_SLAVE_ADDRESS, SERIAL_MUL_PIN1, SERIAL_MUL_PIN2)
+#else
+  #define TMC_UART_HW_DEFINE(IC, ST, L, AI) TMCMarlin<IC##Stepper, L, AI> stepper##ST(&ST##_HARDWARE_SERIAL, float(ST##_RSENSE), ST##_SLAVE_ADDRESS)
+#endif
+#define TMC_UART_SW_DEFINE(IC, ST, L, AI) TMCMarlin<IC##Stepper, L, AI> stepper##ST(ST##_SERIAL_RX_PIN, ST##_SERIAL_TX_PIN, float(ST##_RSENSE), ST##_SLAVE_ADDRESS)
+
+#define _TMC_SPI_DEFINE(IC, ST, AI) __TMC_SPI_DEFINE(IC, ST, TMC_##ST##_LABEL, AI)
+#define TMC_SPI_DEFINE(ST, AI) _TMC_SPI_DEFINE(ST##_DRIVER_TYPE, ST, AI##_AXIS)
+
+#define _TMC_UART_DEFINE(SWHW, IC, ST, AI) TMC_UART_##SWHW##_DEFINE(IC, ST, TMC_##ST##_LABEL, AI)
+#define TMC_UART_DEFINE(SWHW, ST, AI) _TMC_UART_DEFINE(SWHW, ST##_DRIVER_TYPE, ST, AI##_AXIS)
+
+#if DISTINCT_E > 1
+  #define TMC_SPI_DEFINE_E(AI) TMC_SPI_DEFINE(E##AI, E##AI)
+  #define TMC_UART_DEFINE_E(SWHW, AI) TMC_UART_DEFINE(SWHW, E##AI, E##AI)
+#else
+  #define TMC_SPI_DEFINE_E(AI) TMC_SPI_DEFINE(E##AI, E)
+  #define TMC_UART_DEFINE_E(SWHW, AI) TMC_UART_DEFINE(SWHW, E##AI, E)
+#endif
+
+// Stepper objects of TMC2130/TMC2160/TMC2660/TMC5130/TMC5160 steppers used
+#if AXIS_HAS_SPI(X)
+  TMC_SPI_DEFINE(X, X);
+#endif
+#if AXIS_HAS_SPI(X2)
+  TMC_SPI_DEFINE(X2, X);
+#endif
+#if AXIS_HAS_SPI(Y)
+  TMC_SPI_DEFINE(Y, Y);
+#endif
+#if AXIS_HAS_SPI(Y2)
+  TMC_SPI_DEFINE(Y2, Y);
+#endif
+#if AXIS_HAS_SPI(Z)
+  TMC_SPI_DEFINE(Z, Z);
+#endif
+#if AXIS_HAS_SPI(Z2)
+  TMC_SPI_DEFINE(Z2, Z);
+#endif
+#if AXIS_HAS_SPI(Z3)
+  TMC_SPI_DEFINE(Z3, Z);
+#endif
+#if AXIS_HAS_SPI(Z4)
+  TMC_SPI_DEFINE(Z4, Z);
+#endif
+#if AXIS_HAS_SPI(E0)
+  TMC_SPI_DEFINE_E(0);
+#endif
+#if AXIS_HAS_SPI(E1)
+  TMC_SPI_DEFINE_E(1);
+#endif
+#if AXIS_HAS_SPI(E2)
+  TMC_SPI_DEFINE_E(2);
+#endif
+#if AXIS_HAS_SPI(E3)
+  TMC_SPI_DEFINE_E(3);
+#endif
+#if AXIS_HAS_SPI(E4)
+  TMC_SPI_DEFINE_E(4);
+#endif
+#if AXIS_HAS_SPI(E5)
+  TMC_SPI_DEFINE_E(5);
+#endif
+#if AXIS_HAS_SPI(E6)
+  TMC_SPI_DEFINE_E(6);
+#endif
+#if AXIS_HAS_SPI(E7)
+  TMC_SPI_DEFINE_E(7);
+#endif
+
+#ifndef TMC_BAUD_RATE
+  // Reduce baud rate for boards not already overriding TMC_BAUD_RATE for software serial.
+  // Testing has shown that 115200 is not 100% reliable on AVR platforms, occasionally
+  // failing to read status properly. 32-bit platforms typically define an even lower
+  // TMC_BAUD_RATE, due to differences in how SoftwareSerial libraries work on different
+  // platforms.
+  #define TMC_BAUD_RATE TERN(HAS_TMC_SW_SERIAL, 57600, 115200)
+#endif
+
+#if HAS_DRIVER(TMC2130)
+  template<char AXIS_LETTER, char DRIVER_ID, AxisEnum AXIS_ID>
+  void tmc_init(TMCMarlin<TMC2130Stepper, AXIS_LETTER, DRIVER_ID, AXIS_ID> &st, const uint16_t mA, const uint16_t microsteps, const uint32_t hyb_thrs, const bool stealth, const chopper_timing_t &chop_init, const bool interpolate) {
+    st.begin();
+
+    CHOPCONF_t chopconf{0};
+    chopconf.tbl = 0b01;
+    chopconf.toff = chop_init.toff;
+    chopconf.intpol = interpolate;
+    chopconf.hend = chop_init.hend + 3;
+    chopconf.hstrt = chop_init.hstrt - 1;
+    TERN_(SQUARE_WAVE_STEPPING, chopconf.dedge = true);
+    st.CHOPCONF(chopconf.sr);
+
+    st.rms_current(mA, HOLD_MULTIPLIER);
+    st.microsteps(microsteps);
+    st.iholddelay(10);
+    st.TPOWERDOWN(128); // ~2s until driver lowers to hold current
+
+    st.en_pwm_mode(stealth);
+    st.stored.stealthChop_enabled = stealth;
+
+    PWMCONF_t pwmconf{0};
+    pwmconf.pwm_freq = 0b01; // f_pwm = 2/683 f_clk
+    pwmconf.pwm_autoscale = true;
+    pwmconf.pwm_grad = 5;
+    pwmconf.pwm_ampl = 180;
+    st.PWMCONF(pwmconf.sr);
+
+    TERN(HYBRID_THRESHOLD, st.set_pwm_thrs(hyb_thrs), UNUSED(hyb_thrs));
+
+    st.GSTAT(); // Clear GSTAT
+  }
+#endif // TMC2130
+
+#if HAS_DRIVER(TMC2160)
+  template<char AXIS_LETTER, char DRIVER_ID, AxisEnum AXIS_ID>
+  void tmc_init(TMCMarlin<TMC2160Stepper, AXIS_LETTER, DRIVER_ID, AXIS_ID> &st, const uint16_t mA, const uint16_t microsteps, const uint32_t hyb_thrs, const bool stealth, const chopper_timing_t &chop_init, const bool interpolate) {
+    st.begin();
+
+    CHOPCONF_t chopconf{0};
+    chopconf.tbl = 0b01;
+    chopconf.toff = chop_init.toff;
+    chopconf.intpol = interpolate;
+    chopconf.hend = chop_init.hend + 3;
+    chopconf.hstrt = chop_init.hstrt - 1;
+    TERN_(SQUARE_WAVE_STEPPING, chopconf.dedge = true);
+    st.CHOPCONF(chopconf.sr);
+
+    st.rms_current(mA, HOLD_MULTIPLIER);
+    st.microsteps(microsteps);
+    st.iholddelay(10);
+    st.TPOWERDOWN(128); // ~2s until driver lowers to hold current
+
+    st.en_pwm_mode(stealth);
+    st.stored.stealthChop_enabled = stealth;
+
+    TMC2160_n::PWMCONF_t pwmconf{0};
+    pwmconf.pwm_lim = 12;
+    pwmconf.pwm_reg = 8;
+    pwmconf.pwm_autograd = true;
+    pwmconf.pwm_autoscale = true;
+    pwmconf.pwm_freq = 0b01;
+    pwmconf.pwm_grad = 14;
+    pwmconf.pwm_ofs = 36;
+    st.PWMCONF(pwmconf.sr);
+
+    TERN(HYBRID_THRESHOLD, st.set_pwm_thrs(hyb_thrs), UNUSED(hyb_thrs));
+
+    st.GSTAT(); // Clear GSTAT
+  }
+#endif // TMC2160
+
+//
+// TMC2208/2209 Driver objects and inits
+//
+#if HAS_TMC220x
+  #if AXIS_HAS_UART(X)
+    #ifdef X_HARDWARE_SERIAL
+      TMC_UART_DEFINE(HW, X, X);
+      #define X_HAS_HW_SERIAL 1
+    #else
+      TMC_UART_DEFINE(SW, X, X);
+      #define X_HAS_SW_SERIAL 1
+    #endif
+  #endif
+  #if AXIS_HAS_UART(X2)
+    #ifdef X2_HARDWARE_SERIAL
+      TMC_UART_DEFINE(HW, X2, X);
+      #define X2_HAS_HW_SERIAL 1
+    #else
+      TMC_UART_DEFINE(SW, X2, X);
+      #define X2_HAS_SW_SERIAL 1
+    #endif
+  #endif
+  #if AXIS_HAS_UART(Y)
+    #ifdef Y_HARDWARE_SERIAL
+      TMC_UART_DEFINE(HW, Y, Y);
+      #define Y_HAS_HW_SERIAL 1
+    #else
+      TMC_UART_DEFINE(SW, Y, Y);
+      #define Y_HAS_SW_SERIAL 1
+    #endif
+  #endif
+  #if AXIS_HAS_UART(Y2)
+    #ifdef Y2_HARDWARE_SERIAL
+      TMC_UART_DEFINE(HW, Y2, Y);
+      #define Y2_HAS_HW_SERIAL 1
+    #else
+      TMC_UART_DEFINE(SW, Y2, Y);
+      #define Y2_HAS_SW_SERIAL 1
+    #endif
+  #endif
+  #if AXIS_HAS_UART(Z)
+    #ifdef Z_HARDWARE_SERIAL
+      TMC_UART_DEFINE(HW, Z, Z);
+      #define Z_HAS_HW_SERIAL 1
+    #else
+      TMC_UART_DEFINE(SW, Z, Z);
+      #define Z_HAS_SW_SERIAL 1
+    #endif
+  #endif
+  #if AXIS_HAS_UART(Z2)
+    #ifdef Z2_HARDWARE_SERIAL
+      TMC_UART_DEFINE(HW, Z2, Z);
+      #define Z2_HAS_HW_SERIAL 1
+    #else
+      TMC_UART_DEFINE(SW, Z2, Z);
+      #define Z2_HAS_SW_SERIAL 1
+    #endif
+  #endif
+  #if AXIS_HAS_UART(Z3)
+    #ifdef Z3_HARDWARE_SERIAL
+      TMC_UART_DEFINE(HW, Z3, Z);
+      #define Z3_HAS_HW_SERIAL 1
+    #else
+      TMC_UART_DEFINE(SW, Z3, Z);
+      #define Z3_HAS_SW_SERIAL 1
+    #endif
+  #endif
+  #if AXIS_HAS_UART(Z4)
+    #ifdef Z4_HARDWARE_SERIAL
+      TMC_UART_DEFINE(HW, Z4, Z);
+      #define Z4_HAS_HW_SERIAL 1
+    #else
+      TMC_UART_DEFINE(SW, Z4, Z);
+      #define Z4_HAS_SW_SERIAL 1
+    #endif
+  #endif
+  #if AXIS_HAS_UART(E0)
+    #ifdef E0_HARDWARE_SERIAL
+      TMC_UART_DEFINE_E(HW, 0);
+      #define E0_HAS_HW_SERIAL 1
+    #else
+      TMC_UART_DEFINE_E(SW, 0);
+      #define E0_HAS_SW_SERIAL 1
+    #endif
+  #endif
+  #if AXIS_HAS_UART(E1)
+    #ifdef E1_HARDWARE_SERIAL
+      TMC_UART_DEFINE_E(HW, 1);
+      #define E1_HAS_HW_SERIAL 1
+    #else
+      TMC_UART_DEFINE_E(SW, 1);
+      #define E1_HAS_SW_SERIAL 1
+    #endif
+  #endif
+  #if AXIS_HAS_UART(E2)
+    #ifdef E2_HARDWARE_SERIAL
+      TMC_UART_DEFINE_E(HW, 2);
+      #define E2_HAS_HW_SERIAL 1
+    #else
+      TMC_UART_DEFINE_E(SW, 2);
+      #define E2_HAS_SW_SERIAL 1
+    #endif
+  #endif
+  #if AXIS_HAS_UART(E3)
+    #ifdef E3_HARDWARE_SERIAL
+      TMC_UART_DEFINE_E(HW, 3);
+      #define E3_HAS_HW_SERIAL 1
+    #else
+      TMC_UART_DEFINE_E(SW, 3);
+      #define E3_HAS_SW_SERIAL 1
+    #endif
+  #endif
+  #if AXIS_HAS_UART(E4)
+    #ifdef E4_HARDWARE_SERIAL
+      TMC_UART_DEFINE_E(HW, 4);
+      #define E4_HAS_HW_SERIAL 1
+    #else
+      TMC_UART_DEFINE_E(SW, 4);
+      #define E4_HAS_SW_SERIAL 1
+    #endif
+  #endif
+  #if AXIS_HAS_UART(E5)
+    #ifdef E5_HARDWARE_SERIAL
+      TMC_UART_DEFINE_E(HW, 5);
+      #define E5_HAS_HW_SERIAL 1
+    #else
+      TMC_UART_DEFINE_E(SW, 5);
+      #define E5_HAS_SW_SERIAL 1
+    #endif
+  #endif
+  #if AXIS_HAS_UART(E6)
+    #ifdef E6_HARDWARE_SERIAL
+      TMC_UART_DEFINE_E(HW, 6);
+      #define E6_HAS_HW_SERIAL 1
+    #else
+      TMC_UART_DEFINE_E(SW, 6);
+      #define E6_HAS_SW_SERIAL 1
+    #endif
+  #endif
+  #if AXIS_HAS_UART(E7)
+    #ifdef E7_HARDWARE_SERIAL
+      TMC_UART_DEFINE_E(HW, 7);
+      #define E7_HAS_HW_SERIAL 1
+    #else
+      TMC_UART_DEFINE_E(SW, 7);
+      #define E7_HAS_SW_SERIAL 1
+    #endif
+  #endif
+
+  enum TMCAxis : uint8_t { X, Y, Z, X2, Y2, Z2, Z3, Z4, E0, E1, E2, E3, E4, E5, E6, E7, TOTAL };
+
+  void tmc_serial_begin() {
+    #if HAS_TMC_HW_SERIAL
+      struct {
+        const void *ptr[TMCAxis::TOTAL];
+        bool began(const TMCAxis a, const void * const p) {
+          LOOP_L_N(i, a) if (p == ptr[i]) return true;
+          ptr[a] = p; return false;
+        };
+      } sp_helper;
+
+      #define HW_SERIAL_BEGIN(A) do{ if (!sp_helper.began(TMCAxis::A, &A##_HARDWARE_SERIAL)) \
+                                          A##_HARDWARE_SERIAL.begin(TMC_BAUD_RATE); }while(0)
+    #endif
+
+    #if AXIS_HAS_UART(X)
+      #ifdef X_HARDWARE_SERIAL
+        HW_SERIAL_BEGIN(X);
+      #else
+        stepperX.beginSerial(TMC_BAUD_RATE);
+      #endif
+    #endif
+    #if AXIS_HAS_UART(X2)
+      #ifdef X2_HARDWARE_SERIAL
+        HW_SERIAL_BEGIN(X2);
+      #else
+        stepperX2.beginSerial(TMC_BAUD_RATE);
+      #endif
+    #endif
+    #if AXIS_HAS_UART(Y)
+      #ifdef Y_HARDWARE_SERIAL
+        HW_SERIAL_BEGIN(Y);
+      #else
+        stepperY.beginSerial(TMC_BAUD_RATE);
+      #endif
+    #endif
+    #if AXIS_HAS_UART(Y2)
+      #ifdef Y2_HARDWARE_SERIAL
+        HW_SERIAL_BEGIN(Y2);
+      #else
+        stepperY2.beginSerial(TMC_BAUD_RATE);
+      #endif
+    #endif
+    #if AXIS_HAS_UART(Z)
+      #ifdef Z_HARDWARE_SERIAL
+        HW_SERIAL_BEGIN(Z);
+      #else
+        stepperZ.beginSerial(TMC_BAUD_RATE);
+      #endif
+    #endif
+    #if AXIS_HAS_UART(Z2)
+      #ifdef Z2_HARDWARE_SERIAL
+        HW_SERIAL_BEGIN(Z2);
+      #else
+        stepperZ2.beginSerial(TMC_BAUD_RATE);
+      #endif
+    #endif
+    #if AXIS_HAS_UART(Z3)
+      #ifdef Z3_HARDWARE_SERIAL
+        HW_SERIAL_BEGIN(Z3);
+      #else
+        stepperZ3.beginSerial(TMC_BAUD_RATE);
+      #endif
+    #endif
+    #if AXIS_HAS_UART(Z4)
+      #ifdef Z4_HARDWARE_SERIAL
+        HW_SERIAL_BEGIN(Z4);
+      #else
+        stepperZ4.beginSerial(TMC_BAUD_RATE);
+      #endif
+    #endif
+    #if AXIS_HAS_UART(E0)
+      #ifdef E0_HARDWARE_SERIAL
+        HW_SERIAL_BEGIN(E0);
+      #else
+        stepperE0.beginSerial(TMC_BAUD_RATE);
+      #endif
+    #endif
+    #if AXIS_HAS_UART(E1)
+      #ifdef E1_HARDWARE_SERIAL
+        HW_SERIAL_BEGIN(E1);
+      #else
+        stepperE1.beginSerial(TMC_BAUD_RATE);
+      #endif
+    #endif
+    #if AXIS_HAS_UART(E2)
+      #ifdef E2_HARDWARE_SERIAL
+        HW_SERIAL_BEGIN(E2);
+      #else
+        stepperE2.beginSerial(TMC_BAUD_RATE);
+      #endif
+    #endif
+    #if AXIS_HAS_UART(E3)
+      #ifdef E3_HARDWARE_SERIAL
+        HW_SERIAL_BEGIN(E3);
+      #else
+        stepperE3.beginSerial(TMC_BAUD_RATE);
+      #endif
+    #endif
+    #if AXIS_HAS_UART(E4)
+      #ifdef E4_HARDWARE_SERIAL
+        HW_SERIAL_BEGIN(E4);
+      #else
+        stepperE4.beginSerial(TMC_BAUD_RATE);
+      #endif
+    #endif
+    #if AXIS_HAS_UART(E5)
+      #ifdef E5_HARDWARE_SERIAL
+        HW_SERIAL_BEGIN(E5);
+      #else
+        stepperE5.beginSerial(TMC_BAUD_RATE);
+      #endif
+    #endif
+    #if AXIS_HAS_UART(E6)
+      #ifdef E6_HARDWARE_SERIAL
+        HW_SERIAL_BEGIN(E6);
+      #else
+        stepperE6.beginSerial(TMC_BAUD_RATE);
+      #endif
+    #endif
+    #if AXIS_HAS_UART(E7)
+      #ifdef E7_HARDWARE_SERIAL
+        HW_SERIAL_BEGIN(E7);
+      #else
+        stepperE7.beginSerial(TMC_BAUD_RATE);
+      #endif
+    #endif
+  }
+#endif
+
+#if HAS_DRIVER(TMC2208)
+  template<char AXIS_LETTER, char DRIVER_ID, AxisEnum AXIS_ID>
+  void tmc_init(TMCMarlin<TMC2208Stepper, AXIS_LETTER, DRIVER_ID, AXIS_ID> &st, const uint16_t mA, const uint16_t microsteps, const uint32_t hyb_thrs, const bool stealth, const chopper_timing_t &chop_init, const bool interpolate) {
+    TMC2208_n::GCONF_t gconf{0};
+    gconf.pdn_disable = true; // Use UART
+    gconf.mstep_reg_select = true; // Select microsteps with UART
+    gconf.i_scale_analog = false;
+    gconf.en_spreadcycle = !stealth;
+    st.GCONF(gconf.sr);
+    st.stored.stealthChop_enabled = stealth;
+
+    TMC2208_n::CHOPCONF_t chopconf{0};
+    chopconf.tbl = 0b01; // blank_time = 24
+    chopconf.toff = chop_init.toff;
+    chopconf.intpol = interpolate;
+    chopconf.hend = chop_init.hend + 3;
+    chopconf.hstrt = chop_init.hstrt - 1;
+    TERN_(SQUARE_WAVE_STEPPING, chopconf.dedge = true);
+    st.CHOPCONF(chopconf.sr);
+
+    st.rms_current(mA, HOLD_MULTIPLIER);
+    st.microsteps(microsteps);
+    st.iholddelay(10);
+    st.TPOWERDOWN(128); // ~2s until driver lowers to hold current
+
+    TMC2208_n::PWMCONF_t pwmconf{0};
+    pwmconf.pwm_lim = 12;
+    pwmconf.pwm_reg = 8;
+    pwmconf.pwm_autograd = true;
+    pwmconf.pwm_autoscale = true;
+    pwmconf.pwm_freq = 0b01;
+    pwmconf.pwm_grad = 14;
+    pwmconf.pwm_ofs = 36;
+    st.PWMCONF(pwmconf.sr);
+
+    TERN(HYBRID_THRESHOLD, st.set_pwm_thrs(hyb_thrs), UNUSED(hyb_thrs));
+
+    st.GSTAT(0b111); // Clear
+    delay(200);
+  }
+#endif // TMC2208
+
+#if HAS_DRIVER(TMC2209)
+  template<char AXIS_LETTER, char DRIVER_ID, AxisEnum AXIS_ID>
+  void tmc_init(TMCMarlin<TMC2209Stepper, AXIS_LETTER, DRIVER_ID, AXIS_ID> &st, const uint16_t mA, const uint16_t microsteps, const uint32_t hyb_thrs, const bool stealth, const chopper_timing_t &chop_init, const bool interpolate) {
+    TMC2208_n::GCONF_t gconf{0};
+    gconf.pdn_disable = true; // Use UART
+    gconf.mstep_reg_select = true; // Select microsteps with UART
+    gconf.i_scale_analog = false;
+    gconf.en_spreadcycle = !stealth;
+    st.GCONF(gconf.sr);
+    st.stored.stealthChop_enabled = stealth;
+
+    TMC2208_n::CHOPCONF_t chopconf{0};
+    chopconf.tbl = 0b01; // blank_time = 24
+    chopconf.toff = chop_init.toff;
+    chopconf.intpol = interpolate;
+    chopconf.hend = chop_init.hend + 3;
+    chopconf.hstrt = chop_init.hstrt - 1;
+    TERN_(SQUARE_WAVE_STEPPING, chopconf.dedge = true);
+    st.CHOPCONF(chopconf.sr);
+
+    st.rms_current(mA, HOLD_MULTIPLIER);
+    st.microsteps(microsteps);
+    st.iholddelay(10);
+    st.TPOWERDOWN(128); // ~2s until driver lowers to hold current
+
+    TMC2208_n::PWMCONF_t pwmconf{0};
+    pwmconf.pwm_lim = 12;
+    pwmconf.pwm_reg = 8;
+    pwmconf.pwm_autograd = true;
+    pwmconf.pwm_autoscale = true;
+    pwmconf.pwm_freq = 0b01;
+    pwmconf.pwm_grad = 14;
+    pwmconf.pwm_ofs = 36;
+    st.PWMCONF(pwmconf.sr);
+
+    TERN(HYBRID_THRESHOLD, st.set_pwm_thrs(hyb_thrs), UNUSED(hyb_thrs));
+
+    st.GSTAT(0b111); // Clear
+    delay(200);
+  }
+#endif // TMC2209
+
+#if HAS_DRIVER(TMC2660)
+  template<char AXIS_LETTER, char DRIVER_ID, AxisEnum AXIS_ID>
+  void tmc_init(TMCMarlin<TMC2660Stepper, AXIS_LETTER, DRIVER_ID, AXIS_ID> &st, const uint16_t mA, const uint16_t microsteps, const uint32_t, const bool, const chopper_timing_t &chop_init, const bool interpolate) {
+    st.begin();
+
+    TMC2660_n::CHOPCONF_t chopconf{0};
+    chopconf.tbl = 0b01;
+    chopconf.toff = chop_init.toff;
+    chopconf.hend = chop_init.hend + 3;
+    chopconf.hstrt = chop_init.hstrt - 1;
+    st.CHOPCONF(chopconf.sr);
+
+    st.sdoff(0);
+    st.rms_current(mA);
+    st.microsteps(microsteps);
+    TERN_(SQUARE_WAVE_STEPPING, st.dedge(true));
+    st.intpol(interpolate);
+    st.diss2g(true); // Disable short to ground protection. Too many false readings?
+    TERN_(TMC_DEBUG, st.rdsel(0b01));
+  }
+#endif // TMC2660
+
+#if HAS_DRIVER(TMC5130)
+  template<char AXIS_LETTER, char DRIVER_ID, AxisEnum AXIS_ID>
+  void tmc_init(TMCMarlin<TMC5130Stepper, AXIS_LETTER, DRIVER_ID, AXIS_ID> &st, const uint16_t mA, const uint16_t microsteps, const uint32_t hyb_thrs, const bool stealth, const chopper_timing_t &chop_init, const bool interpolate) {
+    st.begin();
+
+    CHOPCONF_t chopconf{0};
+    chopconf.tbl = 0b01;
+    chopconf.toff = chop_init.toff;
+    chopconf.intpol = interpolate;
+    chopconf.hend = chop_init.hend + 3;
+    chopconf.hstrt = chop_init.hstrt - 1;
+    TERN_(SQUARE_WAVE_STEPPING, chopconf.dedge = true);
+    st.CHOPCONF(chopconf.sr);
+
+    st.rms_current(mA, HOLD_MULTIPLIER);
+    st.microsteps(microsteps);
+    st.iholddelay(10);
+    st.TPOWERDOWN(128); // ~2s until driver lowers to hold current
+
+    st.en_pwm_mode(stealth);
+    st.stored.stealthChop_enabled = stealth;
+
+    PWMCONF_t pwmconf{0};
+    pwmconf.pwm_freq = 0b01; // f_pwm = 2/683 f_clk
+    pwmconf.pwm_autoscale = true;
+    pwmconf.pwm_grad = 5;
+    pwmconf.pwm_ampl = 180;
+    st.PWMCONF(pwmconf.sr);
+
+    TERN(HYBRID_THRESHOLD, st.set_pwm_thrs(hyb_thrs), UNUSED(hyb_thrs));
+
+    st.GSTAT(); // Clear GSTAT
+  }
+#endif // TMC5130
+
+#if HAS_DRIVER(TMC5160)
+  template<char AXIS_LETTER, char DRIVER_ID, AxisEnum AXIS_ID>
+  void tmc_init(TMCMarlin<TMC5160Stepper, AXIS_LETTER, DRIVER_ID, AXIS_ID> &st, const uint16_t mA, const uint16_t microsteps, const uint32_t hyb_thrs, const bool stealth, const chopper_timing_t &chop_init, const bool interpolate) {
+    st.begin();
+
+    CHOPCONF_t chopconf{0};
+    chopconf.tbl = 0b01;
+    chopconf.toff = chop_init.toff;
+    chopconf.intpol = interpolate;
+    chopconf.hend = chop_init.hend + 3;
+    chopconf.hstrt = chop_init.hstrt - 1;
+    TERN_(SQUARE_WAVE_STEPPING, chopconf.dedge = true);
+    st.CHOPCONF(chopconf.sr);
+
+    st.rms_current(mA, HOLD_MULTIPLIER);
+    st.microsteps(microsteps);
+    st.iholddelay(10);
+    st.TPOWERDOWN(128); // ~2s until driver lowers to hold current
+
+    st.en_pwm_mode(stealth);
+    st.stored.stealthChop_enabled = stealth;
+
+    TMC2160_n::PWMCONF_t pwmconf{0};
+    pwmconf.pwm_lim = 12;
+    pwmconf.pwm_reg = 8;
+    pwmconf.pwm_autograd = true;
+    pwmconf.pwm_autoscale = true;
+    pwmconf.pwm_freq = 0b01;
+    pwmconf.pwm_grad = 14;
+    pwmconf.pwm_ofs = 36;
+    st.PWMCONF(pwmconf.sr);
+
+    #if ENABLED(HYBRID_THRESHOLD)
+      st.set_pwm_thrs(hyb_thrs);
+    #else
+      UNUSED(hyb_thrs);
+    #endif
+    st.GSTAT(); // Clear GSTAT
+  }
+#endif // TMC5160
+
+void restore_trinamic_drivers() {
+  #if AXIS_IS_TMC(X)
+    stepperX.push();
+  #endif
+  #if AXIS_IS_TMC(X2)
+    stepperX2.push();
+  #endif
+  #if AXIS_IS_TMC(Y)
+    stepperY.push();
+  #endif
+  #if AXIS_IS_TMC(Y2)
+    stepperY2.push();
+  #endif
+  #if AXIS_IS_TMC(Z)
+    stepperZ.push();
+  #endif
+  #if AXIS_IS_TMC(Z2)
+    stepperZ2.push();
+  #endif
+  #if AXIS_IS_TMC(Z3)
+    stepperZ3.push();
+  #endif
+  #if AXIS_IS_TMC(Z4)
+    stepperZ4.push();
+  #endif
+  #if AXIS_IS_TMC(E0)
+    stepperE0.push();
+  #endif
+  #if AXIS_IS_TMC(E1)
+    stepperE1.push();
+  #endif
+  #if AXIS_IS_TMC(E2)
+    stepperE2.push();
+  #endif
+  #if AXIS_IS_TMC(E3)
+    stepperE3.push();
+  #endif
+  #if AXIS_IS_TMC(E4)
+    stepperE4.push();
+  #endif
+  #if AXIS_IS_TMC(E5)
+    stepperE5.push();
+  #endif
+  #if AXIS_IS_TMC(E6)
+    stepperE6.push();
+  #endif
+  #if AXIS_IS_TMC(E7)
+    stepperE7.push();
+  #endif
+}
+
+void reset_trinamic_drivers() {
+  static constexpr bool stealthchop_by_axis[] = { ENABLED(STEALTHCHOP_XY), ENABLED(STEALTHCHOP_Z), ENABLED(STEALTHCHOP_E) };
+
+  #if AXIS_IS_TMC(X)
+    TMC_INIT(X, STEALTH_AXIS_XY);
+  #endif
+  #if AXIS_IS_TMC(X2)
+    TMC_INIT(X2, STEALTH_AXIS_XY);
+  #endif
+  #if AXIS_IS_TMC(Y)
+    TMC_INIT(Y, STEALTH_AXIS_XY);
+  #endif
+  #if AXIS_IS_TMC(Y2)
+    TMC_INIT(Y2, STEALTH_AXIS_XY);
+  #endif
+  #if AXIS_IS_TMC(Z)
+    TMC_INIT(Z, STEALTH_AXIS_Z);
+  #endif
+  #if AXIS_IS_TMC(Z2)
+    TMC_INIT(Z2, STEALTH_AXIS_Z);
+  #endif
+  #if AXIS_IS_TMC(Z3)
+    TMC_INIT(Z3, STEALTH_AXIS_Z);
+  #endif
+  #if AXIS_IS_TMC(Z4)
+    TMC_INIT(Z4, STEALTH_AXIS_Z);
+  #endif
+  #if AXIS_IS_TMC(E0)
+    TMC_INIT(E0, STEALTH_AXIS_E);
+  #endif
+  #if AXIS_IS_TMC(E1)
+    TMC_INIT(E1, STEALTH_AXIS_E);
+  #endif
+  #if AXIS_IS_TMC(E2)
+    TMC_INIT(E2, STEALTH_AXIS_E);
+  #endif
+  #if AXIS_IS_TMC(E3)
+    TMC_INIT(E3, STEALTH_AXIS_E);
+  #endif
+  #if AXIS_IS_TMC(E4)
+    TMC_INIT(E4, STEALTH_AXIS_E);
+  #endif
+  #if AXIS_IS_TMC(E5)
+    TMC_INIT(E5, STEALTH_AXIS_E);
+  #endif
+  #if AXIS_IS_TMC(E6)
+    TMC_INIT(E6, STEALTH_AXIS_E);
+  #endif
+  #if AXIS_IS_TMC(E7)
+    TMC_INIT(E7, STEALTH_AXIS_E);
+  #endif
+
+  #if USE_SENSORLESS
+    #if X_SENSORLESS
+      stepperX.homing_threshold(X_STALL_SENSITIVITY);
+      #if AXIS_HAS_STALLGUARD(X2)
+        stepperX2.homing_threshold(CAT(TERN(X2_SENSORLESS, X2, X), _STALL_SENSITIVITY));
+      #endif
+    #endif
+    #if Y_SENSORLESS
+      stepperY.homing_threshold(Y_STALL_SENSITIVITY);
+      #if AXIS_HAS_STALLGUARD(Y2)
+        stepperY2.homing_threshold(CAT(TERN(Y2_SENSORLESS, Y2, Y), _STALL_SENSITIVITY));
+      #endif
+    #endif
+    #if Z_SENSORLESS
+      stepperZ.homing_threshold(Z_STALL_SENSITIVITY);
+      #if AXIS_HAS_STALLGUARD(Z2)
+        stepperZ2.homing_threshold(CAT(TERN(Z2_SENSORLESS, Z2, Z), _STALL_SENSITIVITY));
+      #endif
+      #if AXIS_HAS_STALLGUARD(Z3)
+        stepperZ3.homing_threshold(CAT(TERN(Z3_SENSORLESS, Z3, Z), _STALL_SENSITIVITY));
+      #endif
+      #if AXIS_HAS_STALLGUARD(Z4)
+        stepperZ4.homing_threshold(CAT(TERN(Z4_SENSORLESS, Z4, Z), _STALL_SENSITIVITY));
+      #endif
+    #endif
+  #endif
+
+  #ifdef TMC_ADV
+    TMC_ADV()
+  #endif
+
+  stepper.set_directions();
+}
+
+// TMC Slave Address Conflict Detection
+//
+// Conflict detection is performed in the following way. Similar methods are used for
+// hardware and software serial, but the implementations are indepenent.
+//
+// 1. Populate a data structure with UART parameters and addresses for all possible axis.
+//      If an axis is not in use, populate it with recognizable placeholder data.
+// 2. For each axis in use, static_assert using a constexpr function, which counts the
+//      number of matching/conflicting axis. If the value is not exactly 1, fail.
+
+#if ANY_AXIS_HAS(HW_SERIAL)
+  // Hardware serial names are compared as strings, since actually resolving them cannot occur in a constexpr.
+  // Using a fixed-length character array for the port name allows this to be constexpr compatible.
+  struct SanityHwSerialDetails { const char port[20]; uint32_t address; };
+  #define TMC_HW_DETAIL_ARGS(A) TERN(A##_HAS_HW_SERIAL, STRINGIFY(A##_HARDWARE_SERIAL), ""), TERN0(A##_HAS_HW_SERIAL, A##_SLAVE_ADDRESS)
+  #define TMC_HW_DETAIL(A) {TMC_HW_DETAIL_ARGS(A)}
+  constexpr SanityHwSerialDetails sanity_tmc_hw_details[] = {
+    TMC_HW_DETAIL(X), TMC_HW_DETAIL(X2),
+    TMC_HW_DETAIL(Y), TMC_HW_DETAIL(Y2),
+    TMC_HW_DETAIL(Z), TMC_HW_DETAIL(Z2), TMC_HW_DETAIL(Z3), TMC_HW_DETAIL(Z4),
+    TMC_HW_DETAIL(E0), TMC_HW_DETAIL(E1), TMC_HW_DETAIL(E2), TMC_HW_DETAIL(E3), TMC_HW_DETAIL(E4), TMC_HW_DETAIL(E5), TMC_HW_DETAIL(E6), TMC_HW_DETAIL(E7)
+  };
+
+  // constexpr compatible string comparison
+  constexpr bool str_eq_ce(const char * a, const char * b) {
+    return *a == *b && (*a == '\0' || str_eq_ce(a+1,b+1));
+  }
+
+  constexpr bool sc_hw_done(size_t start, size_t end) { return start == end; }
+  constexpr bool sc_hw_skip(const char* port_name) { return !(*port_name); }
+  constexpr bool sc_hw_match(const char* port_name, uint32_t address, size_t start, size_t end) {
+    return !sc_hw_done(start, end) && !sc_hw_skip(port_name) && (address == sanity_tmc_hw_details[start].address && str_eq_ce(port_name, sanity_tmc_hw_details[start].port));
+  }
+  constexpr int count_tmc_hw_serial_matches(const char* port_name, uint32_t address, size_t start, size_t end) {
+    return sc_hw_done(start, end) ? 0 : ((sc_hw_skip(port_name) ? 0 : (sc_hw_match(port_name, address, start, end) ? 1 : 0)) + count_tmc_hw_serial_matches(port_name, address, start + 1, end));
+  }
+
+  #define TMC_HWSERIAL_CONFLICT_MSG(A) STRINGIFY(A) "_SLAVE_ADDRESS conflicts with another driver using the same " STRINGIFY(A) "_HARDWARE_SERIAL"
+  #define SA_NO_TMC_HW_C(A) static_assert(1 >= count_tmc_hw_serial_matches(TMC_HW_DETAIL_ARGS(A), 0, COUNT(sanity_tmc_hw_details)), TMC_HWSERIAL_CONFLICT_MSG(A));
+  SA_NO_TMC_HW_C(X);SA_NO_TMC_HW_C(X2);
+  SA_NO_TMC_HW_C(Y);SA_NO_TMC_HW_C(Y2);
+  SA_NO_TMC_HW_C(Z);SA_NO_TMC_HW_C(Z2);SA_NO_TMC_HW_C(Z3);SA_NO_TMC_HW_C(Z4);
+  SA_NO_TMC_HW_C(E0);SA_NO_TMC_HW_C(E1);SA_NO_TMC_HW_C(E2);SA_NO_TMC_HW_C(E3);SA_NO_TMC_HW_C(E4);SA_NO_TMC_HW_C(E5);SA_NO_TMC_HW_C(E6);SA_NO_TMC_HW_C(E7);
+#endif
+
+#if ANY_AXIS_HAS(SW_SERIAL)
+  struct SanitySwSerialDetails { int32_t txpin; int32_t rxpin; uint32_t address; };
+  #define TMC_SW_DETAIL_ARGS(A) TERN(A##_HAS_SW_SERIAL, A##_SERIAL_TX_PIN, -1), TERN(A##_HAS_SW_SERIAL, A##_SERIAL_RX_PIN, -1), TERN0(A##_HAS_SW_SERIAL, A##_SLAVE_ADDRESS)
+  #define TMC_SW_DETAIL(A) TMC_SW_DETAIL_ARGS(A)
+  constexpr SanitySwSerialDetails sanity_tmc_sw_details[] = {
+    TMC_SW_DETAIL(X), TMC_SW_DETAIL(X2),
+    TMC_SW_DETAIL(Y), TMC_SW_DETAIL(Y2),
+    TMC_SW_DETAIL(Z), TMC_SW_DETAIL(Z2), TMC_SW_DETAIL(Z3), TMC_SW_DETAIL(Z4),
+    TMC_SW_DETAIL(E0), TMC_SW_DETAIL(E1), TMC_SW_DETAIL(E2), TMC_SW_DETAIL(E3), TMC_SW_DETAIL(E4), TMC_SW_DETAIL(E5), TMC_SW_DETAIL(E6), TMC_SW_DETAIL(E7)
+  };
+
+  constexpr bool sc_sw_done(size_t start, size_t end) { return start == end; }
+  constexpr bool sc_sw_skip(int32_t txpin) { return txpin < 0; }
+  constexpr bool sc_sw_match(int32_t txpin, int32_t rxpin, uint32_t address, size_t start, size_t end) {
+    return !sc_sw_done(start, end) && !sc_sw_skip(txpin) && (txpin == sanity_tmc_sw_details[start].txpin || rxpin == sanity_tmc_sw_details[start].rxpin) && (address == sanity_tmc_sw_details[start].address);
+  }
+  constexpr int count_tmc_sw_serial_matches(int32_t txpin, int32_t rxpin, uint32_t address, size_t start, size_t end) {
+    return sc_sw_done(start, end) ? 0 : ((sc_sw_skip(txpin) ? 0 : (sc_sw_match(txpin, rxpin, address, start, end) ? 1 : 0)) + count_tmc_sw_serial_matches(txpin, rxpin, address, start + 1, end));
+  }
+
+  #define TMC_SWSERIAL_CONFLICT_MSG(A) STRINGIFY(A) "_SLAVE_ADDRESS conflicts with another driver using the same " STRINGIFY(A) "_SERIAL_RX_PIN or " STRINGIFY(A) "_SERIAL_TX_PIN"
+  #define SA_NO_TMC_SW_C(A) static_assert(1 >= count_tmc_sw_serial_matches(TMC_SW_DETAIL_ARGS(A), 0, COUNT(sanity_tmc_sw_details)), TMC_SWSERIAL_CONFLICT_MSG(A));
+  SA_NO_TMC_SW_C(X);SA_NO_TMC_SW_C(X2);
+  SA_NO_TMC_SW_C(Y);SA_NO_TMC_SW_C(Y2);
+  SA_NO_TMC_SW_C(Z);SA_NO_TMC_SW_C(Z2);SA_NO_TMC_SW_C(Z3);SA_NO_TMC_SW_C(Z4);
+  SA_NO_TMC_SW_C(E0);SA_NO_TMC_SW_C(E1);SA_NO_TMC_SW_C(E2);SA_NO_TMC_SW_C(E3);SA_NO_TMC_SW_C(E4);SA_NO_TMC_SW_C(E5);SA_NO_TMC_SW_C(E6);SA_NO_TMC_SW_C(E7);
+#endif
+
+#endif // HAS_TRINAMIC_CONFIG
diff --git a/Marlin/src/module/stepper/trinamic.h b/Marlin/src/module/stepper/trinamic.h
new file mode 100644
index 0000000..9f7445e
--- /dev/null
+++ b/Marlin/src/module/stepper/trinamic.h
@@ -0,0 +1,362 @@
+/**
+ * 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
+
+/**
+ * stepper/trinamic.h
+ * Stepper driver indirection for Trinamic
+ */
+
+#include <TMCStepper.h>
+#if TMCSTEPPER_VERSION < 0x000500
+  #error "Update TMCStepper library to 0.5.0 or newer."
+#endif
+
+#include "../../inc/MarlinConfig.h"
+#include "../../feature/tmc_util.h"
+
+#define CLASS_TMC2130 TMC2130Stepper
+#define CLASS_TMC2160 TMC2160Stepper
+#define CLASS_TMC2208 TMC2208Stepper
+#define CLASS_TMC2209 TMC2209Stepper
+#define CLASS_TMC2660 TMC2660Stepper
+#define CLASS_TMC5130 TMC5130Stepper
+#define CLASS_TMC5160 TMC5160Stepper
+
+#define TMC_X_LABEL 'X', '0'
+#define TMC_Y_LABEL 'Y', '0'
+#define TMC_Z_LABEL 'Z', '0'
+
+#define TMC_X2_LABEL 'X', '2'
+#define TMC_Y2_LABEL 'Y', '2'
+#define TMC_Z2_LABEL 'Z', '2'
+#define TMC_Z3_LABEL 'Z', '3'
+#define TMC_Z4_LABEL 'Z', '4'
+
+#define TMC_E0_LABEL 'E', '0'
+#define TMC_E1_LABEL 'E', '1'
+#define TMC_E2_LABEL 'E', '2'
+#define TMC_E3_LABEL 'E', '3'
+#define TMC_E4_LABEL 'E', '4'
+#define TMC_E5_LABEL 'E', '5'
+#define TMC_E6_LABEL 'E', '6'
+#define TMC_E7_LABEL 'E', '7'
+
+#define __TMC_CLASS(TYPE, L, I, A) TMCMarlin<CLASS_##TYPE, L, I, A>
+#define _TMC_CLASS(TYPE, LandI, A) __TMC_CLASS(TYPE, LandI, A)
+#define TMC_CLASS(ST, A) _TMC_CLASS(ST##_DRIVER_TYPE, TMC_##ST##_LABEL, A##_AXIS)
+#if ENABLED(DISTINCT_E_FACTORS)
+  #define TMC_CLASS_E(N) TMC_CLASS(E##N, E##N)
+#else
+  #define TMC_CLASS_E(N) TMC_CLASS(E##N, E)
+#endif
+
+typedef struct {
+  uint8_t toff;
+  int8_t hend;
+  uint8_t hstrt;
+} chopper_timing_t;
+
+#ifndef CHOPPER_TIMING_X
+  #define CHOPPER_TIMING_X CHOPPER_TIMING
+#endif
+#ifndef CHOPPER_TIMING_Y
+  #define CHOPPER_TIMING_Y CHOPPER_TIMING
+#endif
+#ifndef CHOPPER_TIMING_Z
+  #define CHOPPER_TIMING_Z CHOPPER_TIMING
+#endif
+#ifndef CHOPPER_TIMING_E
+  #define CHOPPER_TIMING_E CHOPPER_TIMING
+#endif
+
+#if HAS_TMC220x
+  void tmc_serial_begin();
+#endif
+
+void restore_trinamic_drivers();
+void reset_trinamic_drivers();
+
+#define AXIS_HAS_SQUARE_WAVE(A) (AXIS_IS_TMC(A) && ENABLED(SQUARE_WAVE_STEPPING))
+
+// X Stepper
+#if AXIS_IS_TMC(X)
+  extern TMC_CLASS(X, X) stepperX;
+  static constexpr chopper_timing_t chopper_timing_X = CHOPPER_TIMING_X;
+  #if ENABLED(SOFTWARE_DRIVER_ENABLE)
+    #define X_ENABLE_INIT() NOOP
+    #define X_ENABLE_WRITE(STATE) stepperX.toff((STATE)==X_ENABLE_ON ? chopper_timing_X.toff : 0)
+    #define X_ENABLE_READ() stepperX.isEnabled()
+  #endif
+  #if AXIS_HAS_SQUARE_WAVE(X)
+    #define X_STEP_WRITE(STATE) do{ if(STATE) TOGGLE(X_STEP_PIN); }while(0)
+  #endif
+#endif
+
+// Y Stepper
+#if AXIS_IS_TMC(Y)
+  extern TMC_CLASS(Y, Y) stepperY;
+  static constexpr chopper_timing_t chopper_timing_Y = CHOPPER_TIMING_Y;
+  #if ENABLED(SOFTWARE_DRIVER_ENABLE)
+    #define Y_ENABLE_INIT() NOOP
+    #define Y_ENABLE_WRITE(STATE) stepperY.toff((STATE)==Y_ENABLE_ON ? chopper_timing_Y.toff : 0)
+    #define Y_ENABLE_READ() stepperY.isEnabled()
+  #endif
+  #if AXIS_HAS_SQUARE_WAVE(Y)
+    #define Y_STEP_WRITE(STATE) do{ if (STATE) TOGGLE(Y_STEP_PIN); }while(0)
+  #endif
+#endif
+
+// Z Stepper
+#if AXIS_IS_TMC(Z)
+  extern TMC_CLASS(Z, Z) stepperZ;
+  static constexpr chopper_timing_t chopper_timing_Z = CHOPPER_TIMING_Z;
+  #if ENABLED(SOFTWARE_DRIVER_ENABLE)
+    #define Z_ENABLE_INIT() NOOP
+    #define Z_ENABLE_WRITE(STATE) stepperZ.toff((STATE)==Z_ENABLE_ON ? chopper_timing_Z.toff : 0)
+    #define Z_ENABLE_READ() stepperZ.isEnabled()
+  #endif
+  #if AXIS_HAS_SQUARE_WAVE(Z)
+    #define Z_STEP_WRITE(STATE) do{ if(STATE) TOGGLE(Z_STEP_PIN); }while(0)
+  #endif
+#endif
+
+// X2 Stepper
+#if HAS_X2_ENABLE && AXIS_IS_TMC(X2)
+  extern TMC_CLASS(X2, X) stepperX2;
+  #ifndef CHOPPER_TIMING_X2
+    #define CHOPPER_TIMING_X2 CHOPPER_TIMING_X
+  #endif
+  static constexpr chopper_timing_t chopper_timing_X2 = CHOPPER_TIMING_X2;
+  #if ENABLED(SOFTWARE_DRIVER_ENABLE)
+    #define X2_ENABLE_INIT() NOOP
+    #define X2_ENABLE_WRITE(STATE) stepperX2.toff((STATE)==X_ENABLE_ON ? chopper_timing_X2.toff : 0)
+    #define X2_ENABLE_READ() stepperX2.isEnabled()
+  #endif
+  #if AXIS_HAS_SQUARE_WAVE(X2)
+    #define X2_STEP_WRITE(STATE) do{ if(STATE) TOGGLE(X2_STEP_PIN); }while(0)
+  #endif
+#endif
+
+// Y2 Stepper
+#if HAS_Y2_ENABLE && AXIS_IS_TMC(Y2)
+  extern TMC_CLASS(Y2, Y) stepperY2;
+  #ifndef CHOPPER_TIMING_Y2
+    #define CHOPPER_TIMING_Y2 CHOPPER_TIMING_Y
+  #endif
+  static constexpr chopper_timing_t chopper_timing_Y2 = CHOPPER_TIMING_Y2;
+  #if ENABLED(SOFTWARE_DRIVER_ENABLE)
+    #define Y2_ENABLE_INIT() NOOP
+    #define Y2_ENABLE_WRITE(STATE) stepperY2.toff((STATE)==Y_ENABLE_ON ? chopper_timing_Y2.toff : 0)
+    #define Y2_ENABLE_READ() stepperY2.isEnabled()
+  #endif
+  #if AXIS_HAS_SQUARE_WAVE(Y2)
+    #define Y2_STEP_WRITE(STATE) do{ if(STATE) TOGGLE(Y2_STEP_PIN); }while(0)
+  #endif
+#endif
+
+// Z2 Stepper
+#if HAS_Z2_ENABLE && AXIS_IS_TMC(Z2)
+  extern TMC_CLASS(Z2, Z) stepperZ2;
+  #ifndef CHOPPER_TIMING_Z2
+    #define CHOPPER_TIMING_Z2 CHOPPER_TIMING_Z
+  #endif
+  static constexpr chopper_timing_t chopper_timing_Z2 = CHOPPER_TIMING_Z2;
+  #if ENABLED(SOFTWARE_DRIVER_ENABLE) && AXIS_IS_TMC(Z2)
+    #define Z2_ENABLE_INIT() NOOP
+    #define Z2_ENABLE_WRITE(STATE) stepperZ2.toff((STATE)==Z_ENABLE_ON ? chopper_timing_Z2.toff : 0)
+    #define Z2_ENABLE_READ() stepperZ2.isEnabled()
+  #endif
+  #if AXIS_HAS_SQUARE_WAVE(Z2)
+    #define Z2_STEP_WRITE(STATE) do{ if(STATE) TOGGLE(Z2_STEP_PIN); }while(0)
+  #endif
+#endif
+
+// Z3 Stepper
+#if HAS_Z3_ENABLE && AXIS_IS_TMC(Z3)
+  extern TMC_CLASS(Z3, Z) stepperZ3;
+  #ifndef CHOPPER_TIMING_Z3
+    #define CHOPPER_TIMING_Z3 CHOPPER_TIMING_Z
+  #endif
+  static constexpr chopper_timing_t chopper_timing_Z3 = CHOPPER_TIMING_Z3;
+  #if ENABLED(SOFTWARE_DRIVER_ENABLE)
+    #define Z3_ENABLE_INIT() NOOP
+    #define Z3_ENABLE_WRITE(STATE) stepperZ3.toff((STATE)==Z_ENABLE_ON ? chopper_timing_Z3.toff : 0)
+    #define Z3_ENABLE_READ() stepperZ3.isEnabled()
+  #endif
+  #if AXIS_HAS_SQUARE_WAVE(Z3)
+    #define Z3_STEP_WRITE(STATE) do{ if(STATE) TOGGLE(Z3_STEP_PIN); }while(0)
+  #endif
+#endif
+
+// Z4 Stepper
+#if HAS_Z4_ENABLE && AXIS_IS_TMC(Z4)
+  extern TMC_CLASS(Z4, Z) stepperZ4;
+  #ifndef CHOPPER_TIMING_Z4
+    #define CHOPPER_TIMING_Z4 CHOPPER_TIMING_Z
+  #endif
+  static constexpr chopper_timing_t chopper_timing_Z4 = CHOPPER_TIMING_Z4;
+  #if ENABLED(SOFTWARE_DRIVER_ENABLE)
+    #define Z4_ENABLE_INIT() NOOP
+    #define Z4_ENABLE_WRITE(STATE) stepperZ4.toff((STATE)==Z_ENABLE_ON ? chopper_timing_Z4.toff : 0)
+    #define Z4_ENABLE_READ() stepperZ4.isEnabled()
+  #endif
+  #if AXIS_HAS_SQUARE_WAVE(Z4)
+    #define Z4_STEP_WRITE(STATE) do{ if(STATE) TOGGLE(Z4_STEP_PIN); }while(0)
+  #endif
+#endif
+
+// E0 Stepper
+#if AXIS_IS_TMC(E0)
+  extern TMC_CLASS_E(0) stepperE0;
+  #ifndef CHOPPER_TIMING_E0
+    #define CHOPPER_TIMING_E0 CHOPPER_TIMING_E
+  #endif
+  static constexpr chopper_timing_t chopper_timing_E0 = CHOPPER_TIMING_E0;
+  #if ENABLED(SOFTWARE_DRIVER_ENABLE) && AXIS_IS_TMC(E0)
+    #define E0_ENABLE_INIT() NOOP
+    #define E0_ENABLE_WRITE(STATE) stepperE0.toff((STATE)==E_ENABLE_ON ? chopper_timing_E0.toff : 0)
+    #define E0_ENABLE_READ() stepperE0.isEnabled()
+  #endif
+  #if AXIS_HAS_SQUARE_WAVE(E0)
+    #define E0_STEP_WRITE(STATE) do{ if(STATE) TOGGLE(E0_STEP_PIN); }while(0)
+  #endif
+#endif
+
+// E1 Stepper
+#if AXIS_IS_TMC(E1)
+  extern TMC_CLASS_E(1) stepperE1;
+  #ifndef CHOPPER_TIMING_E1
+    #define CHOPPER_TIMING_E1 CHOPPER_TIMING_E
+  #endif
+  static constexpr chopper_timing_t chopper_timing_E1 = CHOPPER_TIMING_E1;
+  #if ENABLED(SOFTWARE_DRIVER_ENABLE) && AXIS_IS_TMC(E1)
+    #define E1_ENABLE_INIT() NOOP
+    #define E1_ENABLE_WRITE(STATE) stepperE1.toff((STATE)==E_ENABLE_ON ? chopper_timing_E1.toff : 0)
+    #define E1_ENABLE_READ() stepperE1.isEnabled()
+  #endif
+  #if AXIS_HAS_SQUARE_WAVE(E1)
+    #define E1_STEP_WRITE(STATE) do{ if(STATE) TOGGLE(E1_STEP_PIN); }while(0)
+  #endif
+#endif
+
+// E2 Stepper
+#if AXIS_IS_TMC(E2)
+  extern TMC_CLASS_E(2) stepperE2;
+  #ifndef CHOPPER_TIMING_E2
+    #define CHOPPER_TIMING_E2 CHOPPER_TIMING_E
+  #endif
+  static constexpr chopper_timing_t chopper_timing_E2 = CHOPPER_TIMING_E2;
+  #if ENABLED(SOFTWARE_DRIVER_ENABLE) && AXIS_IS_TMC(E2)
+    #define E2_ENABLE_INIT() NOOP
+    #define E2_ENABLE_WRITE(STATE) stepperE2.toff((STATE)==E_ENABLE_ON ? chopper_timing_E2.toff : 0)
+    #define E2_ENABLE_READ() stepperE2.isEnabled()
+  #endif
+  #if AXIS_HAS_SQUARE_WAVE(E2)
+    #define E2_STEP_WRITE(STATE) do{ if(STATE) TOGGLE(E2_STEP_PIN); }while(0)
+  #endif
+#endif
+
+// E3 Stepper
+#if AXIS_IS_TMC(E3)
+  extern TMC_CLASS_E(3) stepperE3;
+  #ifndef CHOPPER_TIMING_E3
+    #define CHOPPER_TIMING_E3 CHOPPER_TIMING_E
+  #endif
+  static constexpr chopper_timing_t chopper_timing_E3 = CHOPPER_TIMING_E3;
+  #if ENABLED(SOFTWARE_DRIVER_ENABLE) && AXIS_IS_TMC(E3)
+    #define E3_ENABLE_INIT() NOOP
+    #define E3_ENABLE_WRITE(STATE) stepperE3.toff((STATE)==E_ENABLE_ON ? chopper_timing_E3.toff : 0)
+    #define E3_ENABLE_READ() stepperE3.isEnabled()
+  #endif
+  #if AXIS_HAS_SQUARE_WAVE(E3)
+    #define E3_STEP_WRITE(STATE) do{ if(STATE) TOGGLE(E3_STEP_PIN); }while(0)
+  #endif
+#endif
+
+// E4 Stepper
+#if AXIS_IS_TMC(E4)
+  extern TMC_CLASS_E(4) stepperE4;
+  #ifndef CHOPPER_TIMING_E4
+    #define CHOPPER_TIMING_E4 CHOPPER_TIMING_E
+  #endif
+  static constexpr chopper_timing_t chopper_timing_E4 = CHOPPER_TIMING_E4;
+  #if ENABLED(SOFTWARE_DRIVER_ENABLE) && AXIS_IS_TMC(E4)
+    #define E4_ENABLE_INIT() NOOP
+    #define E4_ENABLE_WRITE(STATE) stepperE4.toff((STATE)==E_ENABLE_ON ? chopper_timing_E4.toff : 0)
+    #define E4_ENABLE_READ() stepperE4.isEnabled()
+  #endif
+  #if AXIS_HAS_SQUARE_WAVE(E4)
+    #define E4_STEP_WRITE(STATE) do{ if(STATE) TOGGLE(E4_STEP_PIN); }while(0)
+  #endif
+#endif
+
+// E5 Stepper
+#if AXIS_IS_TMC(E5)
+  extern TMC_CLASS_E(5) stepperE5;
+  #ifndef CHOPPER_TIMING_E5
+    #define CHOPPER_TIMING_E5 CHOPPER_TIMING_E
+  #endif
+  static constexpr chopper_timing_t chopper_timing_E5 = CHOPPER_TIMING_E5;
+  #if ENABLED(SOFTWARE_DRIVER_ENABLE) && AXIS_IS_TMC(E5)
+    #define E5_ENABLE_INIT() NOOP
+    #define E5_ENABLE_WRITE(STATE) stepperE5.toff((STATE)==E_ENABLE_ON ? chopper_timing_E5.toff : 0)
+    #define E5_ENABLE_READ() stepperE5.isEnabled()
+  #endif
+  #if AXIS_HAS_SQUARE_WAVE(E5)
+    #define E5_STEP_WRITE(STATE) do{ if(STATE) TOGGLE(E5_STEP_PIN); }while(0)
+  #endif
+#endif
+
+// E6 Stepper
+#if AXIS_IS_TMC(E6)
+  extern TMC_CLASS_E(6) stepperE6;
+  #ifndef CHOPPER_TIMING_E6
+    #define CHOPPER_TIMING_E6 CHOPPER_TIMING_E
+  #endif
+  static constexpr chopper_timing_t chopper_timing_E6 = CHOPPER_TIMING_E6;
+  #if ENABLED(SOFTWARE_DRIVER_ENABLE) && AXIS_IS_TMC(E6)
+    #define E6_ENABLE_INIT() NOOP
+    #define E6_ENABLE_WRITE(STATE) stepperE6.toff((STATE)==E_ENABLE_ON ? chopper_timing_E6.toff : 0)
+    #define E6_ENABLE_READ() stepperE6.isEnabled()
+  #endif
+  #if AXIS_HAS_SQUARE_WAVE(E6)
+    #define E6_STEP_WRITE(STATE) do{ if(STATE) TOGGLE(E6_STEP_PIN); }while(0)
+  #endif
+#endif
+
+// E7 Stepper
+#if AXIS_IS_TMC(E7)
+  extern TMC_CLASS_E(7) stepperE7;
+  #ifndef CHOPPER_TIMING_E7
+    #define CHOPPER_TIMING_E7 CHOPPER_TIMING_E
+  #endif
+  static constexpr chopper_timing_t chopper_timing_E7 = CHOPPER_TIMING_E7;
+  #if ENABLED(SOFTWARE_DRIVER_ENABLE) && AXIS_IS_TMC(E7)
+    #define E7_ENABLE_INIT() NOOP
+    #define E7_ENABLE_WRITE(STATE) stepperE7.toff((STATE)==E_ENABLE_ON ? chopper_timing_E7.toff : 0)
+    #define E7_ENABLE_READ() stepperE7.isEnabled()
+  #endif
+  #if AXIS_HAS_SQUARE_WAVE(E7)
+    #define E7_STEP_WRITE(STATE) do{ if(STATE) TOGGLE(E7_STEP_PIN); }while(0)
+  #endif
+#endif
diff --git a/Marlin/src/module/temperature.cpp b/Marlin/src/module/temperature.cpp
new file mode 100644
index 0000000..b5820e1
--- /dev/null
+++ b/Marlin/src/module/temperature.cpp
@@ -0,0 +1,3578 @@
+/**
+ * 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/>.
+ *
+ */
+
+/**
+ * temperature.cpp - temperature control
+ */
+
+// Useful when debugging thermocouples
+//#define IGNORE_THERMOCOUPLE_ERRORS
+
+#include "../MarlinCore.h"
+#include "../HAL/shared/Delay.h"
+#include "../lcd/marlinui.h"
+
+#include "temperature.h"
+#include "endstops.h"
+#include "planner.h"
+
+#if ENABLED(EMERGENCY_PARSER)
+  #include "motion.h"
+#endif
+
+#if ENABLED(DWIN_CREALITY_LCD)
+  #include "../lcd/dwin/e3v2/dwin.h"
+#endif
+
+#if ENABLED(EXTENSIBLE_UI)
+  #include "../lcd/extui/ui_api.h"
+#endif
+
+#if MAX6675_0_IS_MAX31865 || MAX6675_1_IS_MAX31865
+  #include <Adafruit_MAX31865.h>
+  #if MAX6675_0_IS_MAX31865 && !defined(MAX31865_CS_PIN) && PIN_EXISTS(MAX6675_SS)
+    #define MAX31865_CS_PIN   MAX6675_SS_PIN
+  #endif
+  #if MAX6675_1_IS_MAX31865 && !defined(MAX31865_CS2_PIN) && PIN_EXISTS(MAX6675_SS2)
+    #define MAX31865_CS2_PIN  MAX6675_SS2_PIN
+  #endif
+  #ifndef MAX31865_MOSI_PIN
+    #define MAX31865_MOSI_PIN SD_MOSI_PIN
+  #endif
+  #ifndef MAX31865_MISO_PIN
+    #define MAX31865_MISO_PIN MAX6675_DO_PIN
+  #endif
+  #ifndef MAX31865_SCK_PIN
+    #define MAX31865_SCK_PIN  MAX6675_SCK_PIN
+  #endif
+  #if MAX6675_0_IS_MAX31865 && PIN_EXISTS(MAX31865_CS)
+    #define HAS_MAX31865 1
+    Adafruit_MAX31865 max31865_0 = Adafruit_MAX31865(MAX31865_CS_PIN
+      #if MAX31865_CS_PIN != MAX6675_SS_PIN
+        , MAX31865_MOSI_PIN, MAX31865_MISO_PIN, MAX31865_SCK_PIN // For software SPI also set MOSI/MISO/SCK
+      #endif
+    );
+  #endif
+  #if MAX6675_1_IS_MAX31865 && PIN_EXISTS(MAX31865_CS2)
+    #define HAS_MAX31865 1
+    Adafruit_MAX31865 max31865_1 = Adafruit_MAX31865(MAX31865_CS2_PIN
+      #if MAX31865_CS2_PIN != MAX6675_SS2_PIN
+        , MAX31865_MOSI_PIN, MAX31865_MISO_PIN, MAX31865_SCK_PIN // For software SPI also set MOSI/MISO/SCK
+      #endif
+    );
+  #endif
+#endif
+
+#if EITHER(HEATER_0_USES_MAX6675, HEATER_1_USES_MAX6675) && PINS_EXIST(MAX6675_SCK, MAX6675_DO)
+  #define MAX6675_SEPARATE_SPI 1
+#endif
+
+#if MAX6675_SEPARATE_SPI
+  #include "../libs/private_spi.h"
+#endif
+
+#if ENABLED(PID_EXTRUSION_SCALING)
+  #include "stepper.h"
+#endif
+
+#if ENABLED(BABYSTEPPING) && DISABLED(INTEGRATED_BABYSTEPPING)
+  #include "../feature/babystep.h"
+#endif
+
+#include "printcounter.h"
+
+#if ENABLED(FILAMENT_WIDTH_SENSOR)
+  #include "../feature/filwidth.h"
+#endif
+
+#if HAS_POWER_MONITOR
+  #include "../feature/power_monitor.h"
+#endif
+
+#if ENABLED(EMERGENCY_PARSER)
+  #include "../feature/e_parser.h"
+#endif
+
+#if ENABLED(PRINTER_EVENT_LEDS)
+  #include "../feature/leds/printer_event_leds.h"
+#endif
+
+#if ENABLED(JOYSTICK)
+  #include "../feature/joystick.h"
+#endif
+
+#if ENABLED(SINGLENOZZLE)
+  #include "tool_change.h"
+#endif
+
+#if USE_BEEPER
+  #include "../libs/buzzer.h"
+#endif
+
+#if HAS_SERVOS
+  #include "./servo.h"
+#endif
+
+#if ANY(HEATER_0_USES_THERMISTOR, HEATER_1_USES_THERMISTOR, HEATER_2_USES_THERMISTOR, HEATER_3_USES_THERMISTOR, \
+        HEATER_4_USES_THERMISTOR, HEATER_5_USES_THERMISTOR, HEATER_6_USES_THERMISTOR, HEATER_7_USES_THERMISTOR )
+  #define HAS_HOTEND_THERMISTOR 1
+#endif
+
+#if HAS_HOTEND_THERMISTOR
+  #if ENABLED(TEMP_SENSOR_1_AS_REDUNDANT)
+    static const temp_entry_t* heater_ttbl_map[2] = { HEATER_0_TEMPTABLE, HEATER_1_TEMPTABLE };
+    static constexpr uint8_t heater_ttbllen_map[2] = { HEATER_0_TEMPTABLE_LEN, HEATER_1_TEMPTABLE_LEN };
+  #else
+    #define NEXT_TEMPTABLE(N) ,HEATER_##N##_TEMPTABLE
+    #define NEXT_TEMPTABLE_LEN(N) ,HEATER_##N##_TEMPTABLE_LEN
+    static const temp_entry_t* heater_ttbl_map[HOTENDS] = ARRAY_BY_HOTENDS(HEATER_0_TEMPTABLE REPEAT_S(1, HOTENDS, NEXT_TEMPTABLE));
+    static constexpr uint8_t heater_ttbllen_map[HOTENDS] = ARRAY_BY_HOTENDS(HEATER_0_TEMPTABLE_LEN REPEAT_S(1, HOTENDS, NEXT_TEMPTABLE_LEN));
+  #endif
+#endif
+
+Temperature thermalManager;
+
+const char str_t_thermal_runaway[] PROGMEM = STR_T_THERMAL_RUNAWAY,
+           str_t_heating_failed[] PROGMEM = STR_T_HEATING_FAILED;
+
+/**
+ * Macros to include the heater id in temp errors. The compiler's dead-code
+ * elimination should (hopefully) optimize out the unused strings.
+ */
+
+#if HAS_HEATED_BED
+  #define _BED_PSTR(h) (h) == H_BED ? GET_TEXT(MSG_BED) :
+#else
+  #define _BED_PSTR(h)
+#endif
+#if HAS_HEATED_CHAMBER
+  #define _CHAMBER_PSTR(h) (h) == H_CHAMBER ? GET_TEXT(MSG_CHAMBER) :
+#else
+  #define _CHAMBER_PSTR(h)
+#endif
+#define _E_PSTR(h,N) ((HOTENDS) > N && (h) == N) ? PSTR(LCD_STR_E##N) :
+#define HEATER_PSTR(h) _BED_PSTR(h) _CHAMBER_PSTR(h) _E_PSTR(h,1) _E_PSTR(h,2) _E_PSTR(h,3) _E_PSTR(h,4) _E_PSTR(h,5) PSTR(LCD_STR_E0)
+
+// public:
+
+#if ENABLED(NO_FAN_SLOWING_IN_PID_TUNING)
+  bool Temperature::adaptive_fan_slowing = true;
+#endif
+
+#if HAS_HOTEND
+  hotend_info_t Temperature::temp_hotend[HOTEND_TEMPS]; // = { 0 }
+  const uint16_t Temperature::heater_maxtemp[HOTENDS] = ARRAY_BY_HOTENDS(HEATER_0_MAXTEMP, HEATER_1_MAXTEMP, HEATER_2_MAXTEMP, HEATER_3_MAXTEMP, HEATER_4_MAXTEMP, HEATER_5_MAXTEMP, HEATER_6_MAXTEMP, HEATER_7_MAXTEMP);
+#endif
+
+#if ENABLED(AUTO_POWER_E_FANS)
+  uint8_t Temperature::autofan_speed[HOTENDS]; // = { 0 }
+#endif
+
+#if ENABLED(AUTO_POWER_CHAMBER_FAN)
+  uint8_t Temperature::chamberfan_speed; // = 0
+#endif
+
+#if HAS_FAN
+
+  uint8_t Temperature::fan_speed[FAN_COUNT]; // = { 0 }
+
+  #if ENABLED(EXTRA_FAN_SPEED)
+    uint8_t Temperature::old_fan_speed[FAN_COUNT], Temperature::new_fan_speed[FAN_COUNT];
+
+    void Temperature::set_temp_fan_speed(const uint8_t fan, const uint16_t tmp_temp) {
+      switch (tmp_temp) {
+        case 1:
+          set_fan_speed(fan, old_fan_speed[fan]);
+          break;
+        case 2:
+          old_fan_speed[fan] = fan_speed[fan];
+          set_fan_speed(fan, new_fan_speed[fan]);
+          break;
+        default:
+          new_fan_speed[fan] = _MIN(tmp_temp, 255U);
+          break;
+      }
+    }
+
+  #endif
+
+  #if EITHER(PROBING_FANS_OFF, ADVANCED_PAUSE_FANS_PAUSE)
+    bool Temperature::fans_paused; // = false;
+    uint8_t Temperature::saved_fan_speed[FAN_COUNT]; // = { 0 }
+  #endif
+
+  #if ENABLED(ADAPTIVE_FAN_SLOWING)
+    uint8_t Temperature::fan_speed_scaler[FAN_COUNT] = ARRAY_N(FAN_COUNT, 128, 128, 128, 128, 128, 128, 128, 128);
+  #endif
+
+  /**
+   * Set the print fan speed for a target extruder
+   */
+  void Temperature::set_fan_speed(uint8_t target, uint16_t speed) {
+
+    NOMORE(speed, 255U);
+
+    #if ENABLED(SINGLENOZZLE_STANDBY_FAN)
+      if (target != active_extruder) {
+        if (target < EXTRUDERS) singlenozzle_fan_speed[target] = speed;
+        return;
+      }
+    #endif
+
+    TERN_(SINGLENOZZLE, target = 0); // Always use fan index 0 with SINGLENOZZLE
+
+    if (target >= FAN_COUNT) return;
+
+    fan_speed[target] = speed;
+
+    TERN_(REPORT_FAN_CHANGE, report_fan_speed(target));
+  }
+
+  #if ENABLED(REPORT_FAN_CHANGE)
+    /**
+     * Report print fan speed for a target extruder
+     */
+    void Temperature::report_fan_speed(const uint8_t target) {
+      if (target >= FAN_COUNT) return;
+      PORT_REDIRECT(SERIAL_ALL);
+      SERIAL_ECHOLNPAIR("M106 P", target, " S", fan_speed[target]);
+    }
+  #endif
+
+  #if EITHER(PROBING_FANS_OFF, ADVANCED_PAUSE_FANS_PAUSE)
+
+    void Temperature::set_fans_paused(const bool p) {
+      if (p != fans_paused) {
+        fans_paused = p;
+        if (p)
+          FANS_LOOP(i) { saved_fan_speed[i] = fan_speed[i]; fan_speed[i] = 0; }
+        else
+          FANS_LOOP(i) fan_speed[i] = saved_fan_speed[i];
+      }
+    }
+
+  #endif
+
+#endif // HAS_FAN
+
+#if WATCH_HOTENDS
+  hotend_watch_t Temperature::watch_hotend[HOTENDS]; // = { { 0 } }
+#endif
+#if HEATER_IDLE_HANDLER
+  Temperature::heater_idle_t Temperature::heater_idle[NR_HEATER_IDLE]; // = { { 0 } }
+#endif
+
+#if HAS_HEATED_BED
+  bed_info_t Temperature::temp_bed; // = { 0 }
+  // Init min and max temp with extreme values to prevent false errors during startup
+  #ifdef BED_MINTEMP
+    int16_t Temperature::mintemp_raw_BED = HEATER_BED_RAW_LO_TEMP;
+  #endif
+  #ifdef BED_MAXTEMP
+    int16_t Temperature::maxtemp_raw_BED = HEATER_BED_RAW_HI_TEMP;
+  #endif
+  TERN_(WATCH_BED, bed_watch_t Temperature::watch_bed); // = { 0 }
+  IF_DISABLED(PIDTEMPBED, millis_t Temperature::next_bed_check_ms);
+#endif // HAS_HEATED_BED
+
+#if HAS_TEMP_CHAMBER
+  chamber_info_t Temperature::temp_chamber; // = { 0 }
+  #if HAS_HEATED_CHAMBER
+    int16_t fan_chamber_pwm;
+    bool flag_chamber_off;
+    bool flag_chamber_excess_heat = false;
+    millis_t next_cool_check_ms_2 = 0;
+    float old_temp = 9999;
+    #ifdef CHAMBER_MINTEMP
+      int16_t Temperature::mintemp_raw_CHAMBER = HEATER_CHAMBER_RAW_LO_TEMP;
+    #endif
+    #ifdef CHAMBER_MAXTEMP
+      int16_t Temperature::maxtemp_raw_CHAMBER = HEATER_CHAMBER_RAW_HI_TEMP;
+    #endif
+    #if WATCH_CHAMBER
+      chamber_watch_t Temperature::watch_chamber{0};
+    #endif
+    millis_t Temperature::next_chamber_check_ms;
+  #endif // HAS_HEATED_CHAMBER
+#endif // HAS_TEMP_CHAMBER
+
+#if HAS_TEMP_PROBE
+  probe_info_t Temperature::temp_probe; // = { 0 }
+#endif
+
+// Initialized by settings.load()
+#if ENABLED(PIDTEMP)
+  //hotend_pid_t Temperature::pid[HOTENDS];
+#endif
+
+#if ENABLED(PREVENT_COLD_EXTRUSION)
+  bool Temperature::allow_cold_extrude = false;
+  int16_t Temperature::extrude_min_temp = EXTRUDE_MINTEMP;
+#endif
+
+// private:
+
+#if EARLY_WATCHDOG
+  bool Temperature::inited = false;
+#endif
+
+#if ENABLED(TEMP_SENSOR_1_AS_REDUNDANT)
+  uint16_t Temperature::redundant_temperature_raw = 0;
+  float Temperature::redundant_temperature = 0.0;
+#endif
+
+volatile bool Temperature::raw_temps_ready = false;
+
+#if ENABLED(PID_EXTRUSION_SCALING)
+  int32_t Temperature::last_e_position, Temperature::lpq[LPQ_MAX_LEN];
+  lpq_ptr_t Temperature::lpq_ptr = 0;
+#endif
+
+#define TEMPDIR(N) ((HEATER_##N##_RAW_LO_TEMP) < (HEATER_##N##_RAW_HI_TEMP) ? 1 : -1)
+
+#if HAS_HOTEND
+  // Init mintemp and maxtemp with extreme values to prevent false errors during startup
+  constexpr temp_range_t sensor_heater_0 { HEATER_0_RAW_LO_TEMP, HEATER_0_RAW_HI_TEMP, 0, 16383 },
+                         sensor_heater_1 { HEATER_1_RAW_LO_TEMP, HEATER_1_RAW_HI_TEMP, 0, 16383 },
+                         sensor_heater_2 { HEATER_2_RAW_LO_TEMP, HEATER_2_RAW_HI_TEMP, 0, 16383 },
+                         sensor_heater_3 { HEATER_3_RAW_LO_TEMP, HEATER_3_RAW_HI_TEMP, 0, 16383 },
+                         sensor_heater_4 { HEATER_4_RAW_LO_TEMP, HEATER_4_RAW_HI_TEMP, 0, 16383 },
+                         sensor_heater_5 { HEATER_5_RAW_LO_TEMP, HEATER_5_RAW_HI_TEMP, 0, 16383 },
+                         sensor_heater_6 { HEATER_6_RAW_LO_TEMP, HEATER_6_RAW_HI_TEMP, 0, 16383 },
+                         sensor_heater_7 { HEATER_7_RAW_LO_TEMP, HEATER_7_RAW_HI_TEMP, 0, 16383 };
+
+  temp_range_t Temperature::temp_range[HOTENDS] = ARRAY_BY_HOTENDS(sensor_heater_0, sensor_heater_1, sensor_heater_2, sensor_heater_3, sensor_heater_4, sensor_heater_5, sensor_heater_6, sensor_heater_7);
+#endif
+
+#ifdef MAX_CONSECUTIVE_LOW_TEMPERATURE_ERROR_ALLOWED
+  uint8_t Temperature::consecutive_low_temperature_error[HOTENDS] = { 0 };
+#endif
+
+#ifdef MILLISECONDS_PREHEAT_TIME
+  millis_t Temperature::preheat_end_time[HOTENDS] = { 0 };
+#endif
+
+#if HAS_AUTO_FAN
+  millis_t Temperature::next_auto_fan_check_ms = 0;
+#endif
+
+#if ENABLED(FAN_SOFT_PWM)
+  uint8_t Temperature::soft_pwm_amount_fan[FAN_COUNT],
+          Temperature::soft_pwm_count_fan[FAN_COUNT];
+#endif
+
+#if ENABLED(SINGLENOZZLE_STANDBY_TEMP)
+  uint16_t Temperature::singlenozzle_temp[EXTRUDERS];
+  #if HAS_FAN
+    uint8_t Temperature::singlenozzle_fan_speed[EXTRUDERS];
+  #endif
+#endif
+
+#if ENABLED(PROBING_HEATERS_OFF)
+  bool Temperature::paused;
+#endif
+
+// public:
+
+#if HAS_ADC_BUTTONS
+  uint32_t Temperature::current_ADCKey_raw = HAL_ADC_RANGE;
+  uint16_t Temperature::ADCKey_count = 0;
+#endif
+
+#if ENABLED(PID_EXTRUSION_SCALING)
+  int16_t Temperature::lpq_len; // Initialized in settings.cpp
+#endif
+
+#if HAS_PID_HEATING
+
+  inline void say_default_() { SERIAL_ECHOPGM("#define DEFAULT_"); }
+
+  /**
+   * PID Autotuning (M303)
+   *
+   * Alternately heat and cool the nozzle, observing its behavior to
+   * determine the best PID values to achieve a stable temperature.
+   * Needs sufficient heater power to make some overshoot at target
+   * temperature to succeed.
+   */
+  void Temperature::PID_autotune(const float &target, const heater_id_t heater_id, const int8_t ncycles, const bool set_result/*=false*/) {
+    float current_temp = 0.0;
+    int cycles = 0;
+    bool heating = true;
+
+    millis_t next_temp_ms = millis(), t1 = next_temp_ms, t2 = next_temp_ms;
+    long t_high = 0, t_low = 0;
+
+    long bias, d;
+    PID_t tune_pid = { 0, 0, 0 };
+    float maxT = 0, minT = 10000;
+
+    const bool isbed = (heater_id == H_BED);
+
+    #if HAS_PID_FOR_BOTH
+      #define GHV(B,H) (isbed ? (B) : (H))
+      #define SHV(B,H) do{ if (isbed) temp_bed.soft_pwm_amount = B; else temp_hotend[heater_id].soft_pwm_amount = H; }while(0)
+      #define ONHEATINGSTART() (isbed ? printerEventLEDs.onBedHeatingStart() : printerEventLEDs.onHotendHeatingStart())
+      #define ONHEATING(S,C,T) (isbed ? printerEventLEDs.onBedHeating(S,C,T) : printerEventLEDs.onHotendHeating(S,C,T))
+    #elif ENABLED(PIDTEMPBED)
+      #define GHV(B,H) B
+      #define SHV(B,H) (temp_bed.soft_pwm_amount = B)
+      #define ONHEATINGSTART() printerEventLEDs.onBedHeatingStart()
+      #define ONHEATING(S,C,T) printerEventLEDs.onBedHeating(S,C,T)
+    #else
+      #define GHV(B,H) H
+      #define SHV(B,H) (temp_hotend[heater_id].soft_pwm_amount = H)
+      #define ONHEATINGSTART() printerEventLEDs.onHotendHeatingStart()
+      #define ONHEATING(S,C,T) printerEventLEDs.onHotendHeating(S,C,T)
+    #endif
+    #define WATCH_PID BOTH(WATCH_BED, PIDTEMPBED) || BOTH(WATCH_HOTENDS, PIDTEMP)
+
+    #if WATCH_PID
+      #if ALL(THERMAL_PROTECTION_HOTENDS, PIDTEMP, THERMAL_PROTECTION_BED, PIDTEMPBED)
+        #define GTV(B,H) (isbed ? (B) : (H))
+      #elif BOTH(THERMAL_PROTECTION_HOTENDS, PIDTEMP)
+        #define GTV(B,H) (H)
+      #else
+        #define GTV(B,H) (B)
+      #endif
+      const uint16_t watch_temp_period = GTV(WATCH_BED_TEMP_PERIOD, WATCH_TEMP_PERIOD);
+      const uint8_t watch_temp_increase = GTV(WATCH_BED_TEMP_INCREASE, WATCH_TEMP_INCREASE);
+      const float watch_temp_target = target - float(watch_temp_increase + GTV(TEMP_BED_HYSTERESIS, TEMP_HYSTERESIS) + 1);
+      millis_t temp_change_ms = next_temp_ms + SEC_TO_MS(watch_temp_period);
+      float next_watch_temp = 0.0;
+      bool heated = false;
+    #endif
+
+    TERN_(HAS_AUTO_FAN, next_auto_fan_check_ms = next_temp_ms + 2500UL);
+
+    if (target > GHV(BED_MAX_TARGET, temp_range[heater_id].maxtemp - HOTEND_OVERSHOOT)) {
+      SERIAL_ECHOLNPGM(STR_PID_TEMP_TOO_HIGH);
+      TERN_(EXTENSIBLE_UI, ExtUI::onPidTuning(ExtUI::result_t::PID_TEMP_TOO_HIGH));
+      return;
+    }
+
+    SERIAL_ECHOLNPGM(STR_PID_AUTOTUNE_START);
+
+    disable_all_heaters();
+    TERN_(AUTO_POWER_CONTROL, powerManager.power_on());
+
+    SHV(bias = d = (MAX_BED_POWER) >> 1, bias = d = (PID_MAX) >> 1);
+
+    #if ENABLED(PRINTER_EVENT_LEDS)
+      const float start_temp = GHV(temp_bed.celsius, temp_hotend[heater_id].celsius);
+      LEDColor color = ONHEATINGSTART();
+    #endif
+
+    TERN_(NO_FAN_SLOWING_IN_PID_TUNING, adaptive_fan_slowing = false);
+
+    // PID Tuning loop
+    wait_for_heatup = true; // Can be interrupted with M108
+    while (wait_for_heatup) {
+
+      const millis_t ms = millis();
+
+      if (raw_temps_ready) { // temp sample ready
+        updateTemperaturesFromRawValues();
+
+        // Get the current temperature and constrain it
+        current_temp = GHV(temp_bed.celsius, temp_hotend[heater_id].celsius);
+        NOLESS(maxT, current_temp);
+        NOMORE(minT, current_temp);
+
+        #if ENABLED(PRINTER_EVENT_LEDS)
+          ONHEATING(start_temp, current_temp, target);
+        #endif
+
+        #if HAS_AUTO_FAN
+          if (ELAPSED(ms, next_auto_fan_check_ms)) {
+            checkExtruderAutoFans();
+            next_auto_fan_check_ms = ms + 2500UL;
+          }
+        #endif
+
+        if (heating && current_temp > target) {
+          if (ELAPSED(ms, t2 + 5000UL)) {
+            heating = false;
+            SHV((bias - d) >> 1, (bias - d) >> 1);
+            t1 = ms;
+            t_high = t1 - t2;
+            maxT = target;
+          }
+        }
+
+        if (!heating && current_temp < target) {
+          if (ELAPSED(ms, t1 + 5000UL)) {
+            heating = true;
+            t2 = ms;
+            t_low = t2 - t1;
+            if (cycles > 0) {
+              const long max_pow = GHV(MAX_BED_POWER, PID_MAX);
+              bias += (d * (t_high - t_low)) / (t_low + t_high);
+              LIMIT(bias, 20, max_pow - 20);
+              d = (bias > max_pow >> 1) ? max_pow - 1 - bias : bias;
+
+              SERIAL_ECHOPAIR(STR_BIAS, bias, STR_D_COLON, d, STR_T_MIN, minT, STR_T_MAX, maxT);
+              if (cycles > 2) {
+                const float Ku = (4.0f * d) / (float(M_PI) * (maxT - minT) * 0.5f),
+                            Tu = float(t_low + t_high) * 0.001f,
+                            pf = isbed ? 0.2f : 0.6f,
+                            df = isbed ? 1.0f / 3.0f : 1.0f / 8.0f;
+
+                SERIAL_ECHOPAIR(STR_KU, Ku, STR_TU, Tu);
+                if (isbed) { // Do not remove this otherwise PID autotune won't work right for the bed!
+                  tune_pid.Kp = Ku * 0.2f;
+                  tune_pid.Ki = 2 * tune_pid.Kp / Tu;
+                  tune_pid.Kd = tune_pid.Kp * Tu / 3;
+                  SERIAL_ECHOLNPGM("\n" " No overshoot"); // Works far better for the bed. Classic and some have bad ringing.
+                  SERIAL_ECHOLNPAIR(STR_KP, tune_pid.Kp, STR_KI, tune_pid.Ki, STR_KD, tune_pid.Kd);
+                }
+                else {
+                  tune_pid.Kp = Ku * pf;
+                  tune_pid.Kd = tune_pid.Kp * Tu * df;
+                  tune_pid.Ki = 2 * tune_pid.Kp / Tu;
+                  SERIAL_ECHOLNPGM("\n" STR_CLASSIC_PID);
+                  SERIAL_ECHOLNPAIR(STR_KP, tune_pid.Kp, STR_KI, tune_pid.Ki, STR_KD, tune_pid.Kd);
+                }
+
+                /**
+                tune_pid.Kp = 0.33 * Ku;
+                tune_pid.Ki = tune_pid.Kp / Tu;
+                tune_pid.Kd = tune_pid.Kp * Tu / 3;
+                SERIAL_ECHOLNPGM(" Some overshoot");
+                SERIAL_ECHOLNPAIR(" Kp: ", tune_pid.Kp, " Ki: ", tune_pid.Ki, " Kd: ", tune_pid.Kd, " No overshoot");
+                tune_pid.Kp = 0.2 * Ku;
+                tune_pid.Ki = 2 * tune_pid.Kp / Tu;
+                tune_pid.Kd = tune_pid.Kp * Tu / 3;
+                SERIAL_ECHOPAIR(" Kp: ", tune_pid.Kp, " Ki: ", tune_pid.Ki, " Kd: ", tune_pid.Kd);
+                */
+              }
+            }
+            SHV((bias + d) >> 1, (bias + d) >> 1);
+            cycles++;
+            minT = target;
+          }
+        }
+      }
+
+      // Did the temperature overshoot very far?
+      #ifndef MAX_OVERSHOOT_PID_AUTOTUNE
+        #define MAX_OVERSHOOT_PID_AUTOTUNE 30
+      #endif
+      if (current_temp > target + MAX_OVERSHOOT_PID_AUTOTUNE) {
+        SERIAL_ECHOLNPGM(STR_PID_TEMP_TOO_HIGH);
+        TERN_(EXTENSIBLE_UI, ExtUI::onPidTuning(ExtUI::result_t::PID_TEMP_TOO_HIGH));
+        break;
+      }
+
+      // Report heater states every 2 seconds
+      if (ELAPSED(ms, next_temp_ms)) {
+        #if HAS_TEMP_SENSOR
+          print_heater_states(isbed ? active_extruder : heater_id);
+          SERIAL_EOL();
+        #endif
+        next_temp_ms = ms + 2000UL;
+
+        // Make sure heating is actually working
+        #if WATCH_PID
+          if (BOTH(WATCH_BED, WATCH_HOTENDS) || isbed == DISABLED(WATCH_HOTENDS)) {
+            if (!heated) {                                            // If not yet reached target...
+              if (current_temp > next_watch_temp) {                   // Over the watch temp?
+                next_watch_temp = current_temp + watch_temp_increase; // - set the next temp to watch for
+                temp_change_ms = ms + SEC_TO_MS(watch_temp_period);     // - move the expiration timer up
+                if (current_temp > watch_temp_target) heated = true;  // - Flag if target temperature reached
+              }
+              else if (ELAPSED(ms, temp_change_ms))                   // Watch timer expired
+                _temp_error(heater_id, str_t_heating_failed, GET_TEXT(MSG_HEATING_FAILED_LCD));
+            }
+            else if (current_temp < target - (MAX_OVERSHOOT_PID_AUTOTUNE)) // Heated, then temperature fell too far?
+              _temp_error(heater_id, str_t_thermal_runaway, GET_TEXT(MSG_THERMAL_RUNAWAY));
+          }
+        #endif
+      } // every 2 seconds
+
+      // Timeout after MAX_CYCLE_TIME_PID_AUTOTUNE minutes since the last undershoot/overshoot cycle
+      #ifndef MAX_CYCLE_TIME_PID_AUTOTUNE
+        #define MAX_CYCLE_TIME_PID_AUTOTUNE 20L
+      #endif
+      if ((ms - _MIN(t1, t2)) > (MAX_CYCLE_TIME_PID_AUTOTUNE * 60L * 1000L)) {
+        TERN_(DWIN_CREALITY_LCD, DWIN_Popup_Temperature(0));
+        TERN_(EXTENSIBLE_UI, ExtUI::onPidTuning(ExtUI::result_t::PID_TUNING_TIMEOUT));
+        SERIAL_ECHOLNPGM(STR_PID_TIMEOUT);
+        break;
+      }
+
+      if (cycles > ncycles && cycles > 2) {
+        SERIAL_ECHOLNPGM(STR_PID_AUTOTUNE_FINISHED);
+
+        #if HAS_PID_FOR_BOTH
+          const char * const estring = GHV(PSTR("bed"), NUL_STR);
+          say_default_(); serialprintPGM(estring); SERIAL_ECHOLNPAIR("Kp ", tune_pid.Kp);
+          say_default_(); serialprintPGM(estring); SERIAL_ECHOLNPAIR("Ki ", tune_pid.Ki);
+          say_default_(); serialprintPGM(estring); SERIAL_ECHOLNPAIR("Kd ", tune_pid.Kd);
+        #elif ENABLED(PIDTEMP)
+          say_default_(); SERIAL_ECHOLNPAIR("Kp ", tune_pid.Kp);
+          say_default_(); SERIAL_ECHOLNPAIR("Ki ", tune_pid.Ki);
+          say_default_(); SERIAL_ECHOLNPAIR("Kd ", tune_pid.Kd);
+        #else
+          say_default_(); SERIAL_ECHOLNPAIR("bedKp ", tune_pid.Kp);
+          say_default_(); SERIAL_ECHOLNPAIR("bedKi ", tune_pid.Ki);
+          say_default_(); SERIAL_ECHOLNPAIR("bedKd ", tune_pid.Kd);
+        #endif
+
+        #define _SET_BED_PID() do { \
+          temp_bed.pid.Kp = tune_pid.Kp; \
+          temp_bed.pid.Ki = scalePID_i(tune_pid.Ki); \
+          temp_bed.pid.Kd = scalePID_d(tune_pid.Kd); \
+        }while(0)
+
+        #define _SET_EXTRUDER_PID() do { \
+          PID_PARAM(Kp, heater_id) = tune_pid.Kp; \
+          PID_PARAM(Ki, heater_id) = scalePID_i(tune_pid.Ki); \
+          PID_PARAM(Kd, heater_id) = scalePID_d(tune_pid.Kd); \
+          updatePID(); }while(0)
+
+        // Use the result? (As with "M303 U1")
+        if (set_result) {
+          #if HAS_PID_FOR_BOTH
+            if (isbed) _SET_BED_PID(); else _SET_EXTRUDER_PID();
+          #elif ENABLED(PIDTEMP)
+            _SET_EXTRUDER_PID();
+          #else
+            _SET_BED_PID();
+          #endif
+        }
+
+        TERN_(PRINTER_EVENT_LEDS, printerEventLEDs.onPidTuningDone(color));
+
+        TERN_(EXTENSIBLE_UI, ExtUI::onPidTuning(ExtUI::result_t::PID_DONE));
+
+        goto EXIT_M303;
+      }
+
+      // Run HAL idle tasks
+      TERN_(HAL_IDLETASK, HAL_idletask());
+
+      // Run UI update
+      TERN(DWIN_CREALITY_LCD, DWIN_Update(), ui.update());
+    }
+    wait_for_heatup = false;
+
+    disable_all_heaters();
+
+    TERN_(PRINTER_EVENT_LEDS, printerEventLEDs.onPidTuningDone(color));
+
+    TERN_(EXTENSIBLE_UI, ExtUI::onPidTuning(ExtUI::result_t::PID_DONE));
+
+    EXIT_M303:
+      TERN_(NO_FAN_SLOWING_IN_PID_TUNING, adaptive_fan_slowing = true);
+      return;
+  }
+
+#endif // HAS_PID_HEATING
+
+/**
+ * Class and Instance Methods
+ */
+
+int16_t Temperature::getHeaterPower(const heater_id_t heater_id) {
+  switch (heater_id) {
+    #if HAS_HEATED_BED
+      case H_BED: return temp_bed.soft_pwm_amount;
+    #endif
+    #if HAS_HEATED_CHAMBER
+      case H_CHAMBER: return temp_chamber.soft_pwm_amount;
+    #endif
+    default:
+      return TERN0(HAS_HOTEND, temp_hotend[heater_id].soft_pwm_amount);
+  }
+}
+
+#define _EFANOVERLAP(A,B) _FANOVERLAP(E##A,B)
+
+#if HAS_AUTO_FAN
+
+  #define CHAMBER_FAN_INDEX HOTENDS
+
+  void Temperature::checkExtruderAutoFans() {
+    #define _EFAN(B,A) _EFANOVERLAP(A,B) ? B :
+    static const uint8_t fanBit[] PROGMEM = {
+      0
+      #if HAS_MULTI_HOTEND
+        #define _NEXT_FAN(N) , REPEAT2(N,_EFAN,N) N
+        RREPEAT_S(1, HOTENDS, _NEXT_FAN)
+      #endif
+      #if HAS_AUTO_CHAMBER_FAN
+        #define _CFAN(B) _FANOVERLAP(CHAMBER,B) ? B :
+        , REPEAT(HOTENDS,_CFAN) (HOTENDS)
+      #endif
+    };
+
+    uint8_t fanState = 0;
+    HOTEND_LOOP()
+      if (temp_hotend[e].celsius >= EXTRUDER_AUTO_FAN_TEMPERATURE)
+        SBI(fanState, pgm_read_byte(&fanBit[e]));
+
+    #if HAS_AUTO_CHAMBER_FAN
+      if (temp_chamber.celsius >= CHAMBER_AUTO_FAN_TEMPERATURE)
+        SBI(fanState, pgm_read_byte(&fanBit[CHAMBER_FAN_INDEX]));
+    #endif
+
+    #define _UPDATE_AUTO_FAN(P,D,A) do{                  \
+      if (PWM_PIN(P##_AUTO_FAN_PIN) && A < 255)          \
+        analogWrite(pin_t(P##_AUTO_FAN_PIN), D ? A : 0); \
+      else                                               \
+        WRITE(P##_AUTO_FAN_PIN, D);                      \
+    }while(0)
+
+    uint8_t fanDone = 0;
+    LOOP_L_N(f, COUNT(fanBit)) {
+      const uint8_t realFan = pgm_read_byte(&fanBit[f]);
+      if (TEST(fanDone, realFan)) continue;
+      const bool fan_on = TEST(fanState, realFan);
+      switch (f) {
+        #if ENABLED(AUTO_POWER_CHAMBER_FAN)
+          case CHAMBER_FAN_INDEX:
+            chamberfan_speed = fan_on ? CHAMBER_AUTO_FAN_SPEED : 0;
+            break;
+        #endif
+        default:
+          #if ENABLED(AUTO_POWER_E_FANS)
+            autofan_speed[realFan] = fan_on ? EXTRUDER_AUTO_FAN_SPEED : 0;
+          #endif
+          break;
+      }
+
+      switch (f) {
+        #if HAS_AUTO_FAN_0
+          case 0: _UPDATE_AUTO_FAN(E0, fan_on, EXTRUDER_AUTO_FAN_SPEED); break;
+        #endif
+        #if HAS_AUTO_FAN_1
+          case 1: _UPDATE_AUTO_FAN(E1, fan_on, EXTRUDER_AUTO_FAN_SPEED); break;
+        #endif
+        #if HAS_AUTO_FAN_2
+          case 2: _UPDATE_AUTO_FAN(E2, fan_on, EXTRUDER_AUTO_FAN_SPEED); break;
+        #endif
+        #if HAS_AUTO_FAN_3
+          case 3: _UPDATE_AUTO_FAN(E3, fan_on, EXTRUDER_AUTO_FAN_SPEED); break;
+        #endif
+        #if HAS_AUTO_FAN_4
+          case 4: _UPDATE_AUTO_FAN(E4, fan_on, EXTRUDER_AUTO_FAN_SPEED); break;
+        #endif
+        #if HAS_AUTO_FAN_5
+          case 5: _UPDATE_AUTO_FAN(E5, fan_on, EXTRUDER_AUTO_FAN_SPEED); break;
+        #endif
+        #if HAS_AUTO_FAN_6
+          case 6: _UPDATE_AUTO_FAN(E6, fan_on, EXTRUDER_AUTO_FAN_SPEED); break;
+        #endif
+        #if HAS_AUTO_FAN_7
+          case 7: _UPDATE_AUTO_FAN(E7, fan_on, EXTRUDER_AUTO_FAN_SPEED); break;
+        #endif
+        #if HAS_AUTO_CHAMBER_FAN && !AUTO_CHAMBER_IS_E
+          case CHAMBER_FAN_INDEX: _UPDATE_AUTO_FAN(CHAMBER, fan_on, CHAMBER_AUTO_FAN_SPEED); break;
+        #endif
+      }
+      SBI(fanDone, realFan);
+    }
+  }
+
+#endif // HAS_AUTO_FAN
+
+//
+// Temperature Error Handlers
+//
+
+inline void loud_kill(PGM_P const lcd_msg, const heater_id_t heater_id) {
+  marlin_state = MF_KILLED;
+  #if USE_BEEPER
+    thermalManager.disable_all_heaters();
+    for (uint8_t i = 20; i--;) {
+      WRITE(BEEPER_PIN, HIGH);
+      delay(25);
+      watchdog_refresh();
+      WRITE(BEEPER_PIN, LOW);
+      delay(40);
+      watchdog_refresh();
+      delay(40);
+      watchdog_refresh();
+    }
+    WRITE(BEEPER_PIN, HIGH);
+  #endif
+  kill(lcd_msg, HEATER_PSTR(heater_id));
+}
+
+void Temperature::_temp_error(const heater_id_t heater_id, PGM_P const serial_msg, PGM_P const lcd_msg) {
+
+  static uint8_t killed = 0;
+
+  if (IsRunning() && TERN1(BOGUS_TEMPERATURE_GRACE_PERIOD, killed == 2)) {
+    SERIAL_ERROR_START();
+    serialprintPGM(serial_msg);
+    SERIAL_ECHOPGM(STR_STOPPED_HEATER);
+    if (heater_id >= 0)
+      SERIAL_ECHO((int)heater_id);
+    else if (TERN0(HAS_HEATED_CHAMBER, heater_id == H_CHAMBER))
+      SERIAL_ECHOPGM(STR_HEATER_CHAMBER);
+    else
+      SERIAL_ECHOPGM(STR_HEATER_BED);
+    SERIAL_EOL();
+  }
+
+  disable_all_heaters(); // always disable (even for bogus temp)
+  watchdog_refresh();
+
+  #if BOGUS_TEMPERATURE_GRACE_PERIOD
+    const millis_t ms = millis();
+    static millis_t expire_ms;
+    switch (killed) {
+      case 0:
+        expire_ms = ms + BOGUS_TEMPERATURE_GRACE_PERIOD;
+        ++killed;
+        break;
+      case 1:
+        if (ELAPSED(ms, expire_ms)) ++killed;
+        break;
+      case 2:
+        loud_kill(lcd_msg, heater_id);
+        ++killed;
+        break;
+    }
+  #elif defined(BOGUS_TEMPERATURE_GRACE_PERIOD)
+    UNUSED(killed);
+  #else
+    if (!killed) { killed = 1; loud_kill(lcd_msg, heater_id); }
+  #endif
+}
+
+void Temperature::max_temp_error(const heater_id_t heater_id) {
+  #if ENABLED(DWIN_CREALITY_LCD) && (HAS_HOTEND || HAS_HEATED_BED)
+    DWIN_Popup_Temperature(1);
+  #endif
+  _temp_error(heater_id, PSTR(STR_T_MAXTEMP), GET_TEXT(MSG_ERR_MAXTEMP));
+}
+
+void Temperature::min_temp_error(const heater_id_t heater_id) {
+  #if ENABLED(DWIN_CREALITY_LCD) && (HAS_HOTEND || HAS_HEATED_BED)
+    DWIN_Popup_Temperature(0);
+  #endif
+  _temp_error(heater_id, PSTR(STR_T_MINTEMP), GET_TEXT(MSG_ERR_MINTEMP));
+}
+
+#if HAS_HOTEND
+  #if ENABLED(PID_DEBUG)
+    extern bool pid_debug_flag;
+  #endif
+
+  float Temperature::get_pid_output_hotend(const uint8_t E_NAME) {
+    const uint8_t ee = HOTEND_INDEX;
+    #if ENABLED(PIDTEMP)
+      #if DISABLED(PID_OPENLOOP)
+        static hotend_pid_t work_pid[HOTENDS];
+        static float temp_iState[HOTENDS] = { 0 },
+                     temp_dState[HOTENDS] = { 0 };
+        static bool pid_reset[HOTENDS] = { false };
+        const float pid_error = temp_hotend[ee].target - temp_hotend[ee].celsius;
+
+        float pid_output;
+
+        if (temp_hotend[ee].target == 0
+          || pid_error < -(PID_FUNCTIONAL_RANGE)
+          || TERN0(HEATER_IDLE_HANDLER, heater_idle[ee].timed_out)
+        ) {
+          pid_output = 0;
+          pid_reset[ee] = true;
+        }
+        else if (pid_error > PID_FUNCTIONAL_RANGE) {
+          pid_output = BANG_MAX;
+          pid_reset[ee] = true;
+        }
+        else {
+          if (pid_reset[ee]) {
+            temp_iState[ee] = 0.0;
+            work_pid[ee].Kd = 0.0;
+            pid_reset[ee] = false;
+          }
+
+          work_pid[ee].Kd = work_pid[ee].Kd + PID_K2 * (PID_PARAM(Kd, ee) * (temp_dState[ee] - temp_hotend[ee].celsius) - work_pid[ee].Kd);
+          const float max_power_over_i_gain = float(PID_MAX) / PID_PARAM(Ki, ee) - float(MIN_POWER);
+          temp_iState[ee] = constrain(temp_iState[ee] + pid_error, 0, max_power_over_i_gain);
+          work_pid[ee].Kp = PID_PARAM(Kp, ee) * pid_error;
+          work_pid[ee].Ki = PID_PARAM(Ki, ee) * temp_iState[ee];
+
+          pid_output = work_pid[ee].Kp + work_pid[ee].Ki + work_pid[ee].Kd + float(MIN_POWER);
+
+          #if ENABLED(PID_EXTRUSION_SCALING)
+            #if HOTENDS == 1
+              constexpr bool this_hotend = true;
+            #else
+              const bool this_hotend = (ee == active_extruder);
+            #endif
+            work_pid[ee].Kc = 0;
+            if (this_hotend) {
+              const long e_position = stepper.position(E_AXIS);
+              if (e_position > last_e_position) {
+                lpq[lpq_ptr] = e_position - last_e_position;
+                last_e_position = e_position;
+              }
+              else
+                lpq[lpq_ptr] = 0;
+
+              if (++lpq_ptr >= lpq_len) lpq_ptr = 0;
+              work_pid[ee].Kc = (lpq[lpq_ptr] * planner.steps_to_mm[E_AXIS]) * PID_PARAM(Kc, ee);
+              pid_output += work_pid[ee].Kc;
+            }
+          #endif // PID_EXTRUSION_SCALING
+          #if ENABLED(PID_FAN_SCALING)
+            if (fan_speed[active_extruder] > PID_FAN_SCALING_MIN_SPEED) {
+              work_pid[ee].Kf = PID_PARAM(Kf, ee) + (PID_FAN_SCALING_LIN_FACTOR) * fan_speed[active_extruder];
+              pid_output += work_pid[ee].Kf;
+            }
+            //pid_output -= work_pid[ee].Ki;
+            //pid_output += work_pid[ee].Ki * work_pid[ee].Kf
+          #endif // PID_FAN_SCALING
+          LIMIT(pid_output, 0, PID_MAX);
+        }
+        temp_dState[ee] = temp_hotend[ee].celsius;
+
+      #else // PID_OPENLOOP
+
+        const float pid_output = constrain(temp_hotend[ee].target, 0, PID_MAX);
+
+      #endif // PID_OPENLOOP
+
+      #if ENABLED(PID_DEBUG)
+        if (ee == active_extruder && pid_debug_flag) {
+          SERIAL_ECHO_START();
+          SERIAL_ECHOPAIR(STR_PID_DEBUG, ee, STR_PID_DEBUG_INPUT, temp_hotend[ee].celsius, STR_PID_DEBUG_OUTPUT, pid_output);
+          #if DISABLED(PID_OPENLOOP)
+          {
+            SERIAL_ECHOPAIR(
+              STR_PID_DEBUG_PTERM, work_pid[ee].Kp,
+              STR_PID_DEBUG_ITERM, work_pid[ee].Ki,
+              STR_PID_DEBUG_DTERM, work_pid[ee].Kd
+              #if ENABLED(PID_EXTRUSION_SCALING)
+                , STR_PID_DEBUG_CTERM, work_pid[ee].Kc
+              #endif
+            );
+          }
+          #endif
+          SERIAL_EOL();
+        }
+      #endif // PID_DEBUG
+
+    #else // No PID enabled
+
+      const bool is_idling = TERN0(HEATER_IDLE_HANDLER, heater_idle[ee].timed_out);
+      const float pid_output = (!is_idling && temp_hotend[ee].celsius < temp_hotend[ee].target) ? BANG_MAX : 0;
+
+    #endif
+
+    return pid_output;
+  }
+
+#endif // HAS_HOTEND
+
+#if ENABLED(PIDTEMPBED)
+
+  float Temperature::get_pid_output_bed() {
+
+    #if DISABLED(PID_OPENLOOP)
+
+      static PID_t work_pid{0};
+      static float temp_iState = 0, temp_dState = 0;
+      static bool pid_reset = true;
+      float pid_output = 0;
+      const float max_power_over_i_gain = float(MAX_BED_POWER) / temp_bed.pid.Ki - float(MIN_BED_POWER),
+                  pid_error = temp_bed.target - temp_bed.celsius;
+
+      if (!temp_bed.target || pid_error < -(PID_FUNCTIONAL_RANGE)) {
+        pid_output = 0;
+        pid_reset = true;
+      }
+      else if (pid_error > PID_FUNCTIONAL_RANGE) {
+        pid_output = MAX_BED_POWER;
+        pid_reset = true;
+      }
+      else {
+        if (pid_reset) {
+          temp_iState = 0.0;
+          work_pid.Kd = 0.0;
+          pid_reset = false;
+        }
+
+        temp_iState = constrain(temp_iState + pid_error, 0, max_power_over_i_gain);
+
+        work_pid.Kp = temp_bed.pid.Kp * pid_error;
+        work_pid.Ki = temp_bed.pid.Ki * temp_iState;
+        work_pid.Kd = work_pid.Kd + PID_K2 * (temp_bed.pid.Kd * (temp_dState - temp_bed.celsius) - work_pid.Kd);
+
+        temp_dState = temp_bed.celsius;
+
+        pid_output = constrain(work_pid.Kp + work_pid.Ki + work_pid.Kd + float(MIN_BED_POWER), 0, MAX_BED_POWER);
+      }
+
+    #else // PID_OPENLOOP
+
+      const float pid_output = constrain(temp_bed.target, 0, MAX_BED_POWER);
+
+    #endif // PID_OPENLOOP
+
+    #if ENABLED(PID_BED_DEBUG)
+    {
+      SERIAL_ECHO_START();
+      SERIAL_ECHOLNPAIR(
+        " PID_BED_DEBUG : Input ", temp_bed.celsius, " Output ", pid_output,
+        #if DISABLED(PID_OPENLOOP)
+          STR_PID_DEBUG_PTERM, work_pid.Kp,
+          STR_PID_DEBUG_ITERM, work_pid.Ki,
+          STR_PID_DEBUG_DTERM, work_pid.Kd,
+        #endif
+      );
+    }
+    #endif
+
+    return pid_output;
+  }
+
+#endif // PIDTEMPBED
+
+/**
+ * Manage heating activities for extruder hot-ends and a heated bed
+ *  - Acquire updated temperature readings
+ *    - Also resets the watchdog timer
+ *  - Invoke thermal runaway protection
+ *  - Manage extruder auto-fan
+ *  - Apply filament width to the extrusion rate (may move)
+ *  - Update the heated bed PID output value
+ */
+void Temperature::manage_heater() {
+
+  #if EARLY_WATCHDOG
+    // If thermal manager is still not running, make sure to at least reset the watchdog!
+    if (!inited) return watchdog_refresh();
+  #endif
+
+  #if ENABLED(EMERGENCY_PARSER)
+    if (emergency_parser.killed_by_M112) kill(M112_KILL_STR, nullptr, true);
+
+    if (emergency_parser.quickstop_by_M410) {
+      emergency_parser.quickstop_by_M410 = false; // quickstop_stepper may call idle so clear this now!
+      quickstop_stepper();
+    }
+  #endif
+
+  if (!raw_temps_ready) return;
+
+  updateTemperaturesFromRawValues(); // also resets the watchdog
+
+  #if DISABLED(IGNORE_THERMOCOUPLE_ERRORS)
+    #if HEATER_0_USES_MAX6675
+      if (temp_hotend[0].celsius > _MIN(HEATER_0_MAXTEMP, HEATER_0_MAX6675_TMAX - 1.0)) max_temp_error(H_E0);
+      if (temp_hotend[0].celsius < _MAX(HEATER_0_MINTEMP, HEATER_0_MAX6675_TMIN + .01)) min_temp_error(H_E0);
+    #endif
+    #if HEATER_1_USES_MAX6675
+      if (temp_hotend[1].celsius > _MIN(HEATER_1_MAXTEMP, HEATER_1_MAX6675_TMAX - 1.0)) max_temp_error(H_E1);
+      if (temp_hotend[1].celsius < _MAX(HEATER_1_MINTEMP, HEATER_1_MAX6675_TMIN + .01)) min_temp_error(H_E1);
+    #endif
+  #endif
+
+  millis_t ms = millis();
+
+  #if HAS_HOTEND
+
+    HOTEND_LOOP() {
+      #if ENABLED(THERMAL_PROTECTION_HOTENDS)
+        if (degHotend(e) > temp_range[e].maxtemp) max_temp_error((heater_id_t)e);
+      #endif
+
+      TERN_(HEATER_IDLE_HANDLER, heater_idle[e].update(ms));
+
+      #if ENABLED(THERMAL_PROTECTION_HOTENDS)
+        // Check for thermal runaway
+        tr_state_machine[e].run(temp_hotend[e].celsius, temp_hotend[e].target, (heater_id_t)e, THERMAL_PROTECTION_PERIOD, THERMAL_PROTECTION_HYSTERESIS);
+      #endif
+
+      temp_hotend[e].soft_pwm_amount = (temp_hotend[e].celsius > temp_range[e].mintemp || is_preheating(e)) && temp_hotend[e].celsius < temp_range[e].maxtemp ? (int)get_pid_output_hotend(e) >> 1 : 0;
+
+      #if WATCH_HOTENDS
+        // Make sure temperature is increasing
+        if (watch_hotend[e].next_ms && ELAPSED(ms, watch_hotend[e].next_ms)) {  // Time to check this extruder?
+          if (degHotend(e) < watch_hotend[e].target) {                          // Failed to increase enough?
+            TERN_(DWIN_CREALITY_LCD, DWIN_Popup_Temperature(0));
+            _temp_error((heater_id_t)e, str_t_heating_failed, GET_TEXT(MSG_HEATING_FAILED_LCD));
+          }
+          else                                                                  // Start again if the target is still far off
+            start_watching_hotend(e);
+        }
+      #endif
+
+      #if ENABLED(TEMP_SENSOR_1_AS_REDUNDANT)
+        // Make sure measured temperatures are close together
+        if (ABS(temp_hotend[0].celsius - redundant_temperature) > MAX_REDUNDANT_TEMP_SENSOR_DIFF)
+          _temp_error(H_E0, PSTR(STR_REDUNDANCY), GET_TEXT(MSG_ERR_REDUNDANT_TEMP));
+      #endif
+
+    } // HOTEND_LOOP
+
+  #endif // HAS_HOTEND
+
+  #if HAS_AUTO_FAN
+    if (ELAPSED(ms, next_auto_fan_check_ms)) { // only need to check fan state very infrequently
+      checkExtruderAutoFans();
+      next_auto_fan_check_ms = ms + 2500UL;
+    }
+  #endif
+
+  #if ENABLED(FILAMENT_WIDTH_SENSOR)
+    /**
+     * Dynamically set the volumetric multiplier based
+     * on the delayed Filament Width measurement.
+     */
+    filwidth.update_volumetric();
+  #endif
+
+  #if HAS_HEATED_BED
+
+    #if ENABLED(THERMAL_PROTECTION_BED)
+      if (degBed() > BED_MAXTEMP) max_temp_error(H_BED);
+    #endif
+
+    #if WATCH_BED
+      // Make sure temperature is increasing
+      if (watch_bed.elapsed(ms)) {        // Time to check the bed?
+        if (degBed() < watch_bed.target) {                              // Failed to increase enough?
+          TERN_(DWIN_CREALITY_LCD, DWIN_Popup_Temperature(0));
+          _temp_error(H_BED, str_t_heating_failed, GET_TEXT(MSG_HEATING_FAILED_LCD));
+        }
+        else                                                            // Start again if the target is still far off
+          start_watching_bed();
+      }
+    #endif // WATCH_BED
+
+    #if BOTH(PROBING_HEATERS_OFF, BED_LIMIT_SWITCHING)
+      #define PAUSE_CHANGE_REQD 1
+    #endif
+
+    #if PAUSE_CHANGE_REQD
+      static bool last_pause_state;
+    #endif
+
+    do {
+
+      #if DISABLED(PIDTEMPBED)
+        if (PENDING(ms, next_bed_check_ms)
+          && TERN1(PAUSE_CHANGE_REQD, paused == last_pause_state)
+        ) break;
+        next_bed_check_ms = ms + BED_CHECK_INTERVAL;
+        TERN_(PAUSE_CHANGE_REQD, last_pause_state = paused);
+      #endif
+
+      TERN_(HEATER_IDLE_HANDLER, heater_idle[IDLE_INDEX_BED].update(ms));
+
+      #if HAS_THERMALLY_PROTECTED_BED
+        tr_state_machine[RUNAWAY_IND_BED].run(temp_bed.celsius, temp_bed.target, H_BED, THERMAL_PROTECTION_BED_PERIOD, THERMAL_PROTECTION_BED_HYSTERESIS);
+      #endif
+
+      #if HEATER_IDLE_HANDLER
+        if (heater_idle[IDLE_INDEX_BED].timed_out) {
+          temp_bed.soft_pwm_amount = 0;
+          #if DISABLED(PIDTEMPBED)
+            WRITE_HEATER_BED(LOW);
+          #endif
+        }
+        else
+      #endif
+      {
+        #if ENABLED(PIDTEMPBED)
+          temp_bed.soft_pwm_amount = WITHIN(temp_bed.celsius, BED_MINTEMP, BED_MAXTEMP) ? (int)get_pid_output_bed() >> 1 : 0;
+        #else
+          // Check if temperature is within the correct band
+          if (WITHIN(temp_bed.celsius, BED_MINTEMP, BED_MAXTEMP)) {
+            #if ENABLED(BED_LIMIT_SWITCHING)
+              if (temp_bed.celsius >= temp_bed.target + BED_HYSTERESIS)
+                temp_bed.soft_pwm_amount = 0;
+              else if (temp_bed.celsius <= temp_bed.target - (BED_HYSTERESIS))
+                temp_bed.soft_pwm_amount = MAX_BED_POWER >> 1;
+            #else // !PIDTEMPBED && !BED_LIMIT_SWITCHING
+              temp_bed.soft_pwm_amount = temp_bed.celsius < temp_bed.target ? MAX_BED_POWER >> 1 : 0;
+            #endif
+          }
+          else {
+            temp_bed.soft_pwm_amount = 0;
+            WRITE_HEATER_BED(LOW);
+          }
+        #endif
+      }
+
+    } while (false);
+
+  #endif // HAS_HEATED_BED
+
+  #if HAS_HEATED_CHAMBER
+
+    #ifndef CHAMBER_CHECK_INTERVAL
+      #define CHAMBER_CHECK_INTERVAL 1000UL
+    #endif
+
+    #if ENABLED(THERMAL_PROTECTION_CHAMBER)
+      if (degChamber() > CHAMBER_MAXTEMP) max_temp_error(H_CHAMBER);
+    #endif
+
+    #if WATCH_CHAMBER
+      // Make sure temperature is increasing
+      if (watch_chamber.elapsed(ms)) {              // Time to check the chamber?
+        if (degChamber() < watch_chamber.target)    // Failed to increase enough?
+          _temp_error(H_CHAMBER, str_t_heating_failed, GET_TEXT(MSG_HEATING_FAILED_LCD));
+        else
+          start_watching_chamber();                 // Start again if the target is still far off
+      }
+    #endif
+
+    #if EITHER(CHAMBER_FAN, CHAMBER_VENT)
+      if (temp_chamber.target > CHAMBER_MINTEMP) {
+        flag_chamber_off = false;
+
+        #if ENABLED(CHAMBER_FAN)
+          #if CHAMBER_FAN_MODE == 0
+            fan_chamber_pwm = CHAMBER_FAN_BASE;
+          #elif CHAMBER_FAN_MODE == 1
+            fan_chamber_pwm = (temp_chamber.celsius > temp_chamber.target) ? (CHAMBER_FAN_BASE) + (CHAMBER_FAN_FACTOR) * (temp_chamber.celsius - temp_chamber.target) : 0;
+          #elif CHAMBER_FAN_MODE == 2
+            fan_chamber_pwm = (CHAMBER_FAN_BASE) + (CHAMBER_FAN_FACTOR) * ABS(temp_chamber.celsius - temp_chamber.target);
+            if (temp_chamber.soft_pwm_amount)
+              fan_chamber_pwm += (CHAMBER_FAN_FACTOR) * 2;
+          #endif
+          NOMORE(fan_chamber_pwm, 225);
+          set_fan_speed(2, fan_chamber_pwm); // TODO: instead of fan 2, set to chamber fan
+        #endif
+
+        #if ENABLED(CHAMBER_VENT)
+          #ifndef MIN_COOLING_SLOPE_TIME_CHAMBER_VENT
+            #define MIN_COOLING_SLOPE_TIME_CHAMBER_VENT 20
+          #endif
+          #ifndef MIN_COOLING_SLOPE_DEG_CHAMBER_VENT
+            #define MIN_COOLING_SLOPE_DEG_CHAMBER_VENT 1.5
+          #endif
+          if (!flag_chamber_excess_heat && temp_chamber.celsius - temp_chamber.target >= HIGH_EXCESS_HEAT_LIMIT) {
+            // Open vent after MIN_COOLING_SLOPE_TIME_CHAMBER_VENT seconds if the
+            // temperature didn't drop at least MIN_COOLING_SLOPE_DEG_CHAMBER_VENT
+            if (next_cool_check_ms_2 == 0 || ELAPSED(ms, next_cool_check_ms_2)) {
+              if (old_temp - temp_chamber.celsius < float(MIN_COOLING_SLOPE_DEG_CHAMBER_VENT)) flag_chamber_excess_heat = true; //the bed is heating the chamber too much
+              next_cool_check_ms_2 = ms + SEC_TO_MS(MIN_COOLING_SLOPE_TIME_CHAMBER_VENT);
+              old_temp = temp_chamber.celsius;
+            }
+          }
+          else {
+            next_cool_check_ms_2 = 0;
+            old_temp = 9999;
+          }
+          if (flag_chamber_excess_heat && (temp_chamber.celsius - temp_chamber.target <= -LOW_EXCESS_HEAT_LIMIT) ) {
+            flag_chamber_excess_heat = false;
+          }
+        #endif
+      }
+      else if (!flag_chamber_off) {
+        #if ENABLED(CHAMBER_FAN)
+          flag_chamber_off = true;
+          set_fan_speed(2, 0);
+        #endif
+        #if ENABLED(CHAMBER_VENT)
+          flag_chamber_excess_heat = false;
+          MOVE_SERVO(CHAMBER_VENT_SERVO_NR, 90);
+        #endif
+      }
+    #endif
+
+    if (ELAPSED(ms, next_chamber_check_ms)) {
+      next_chamber_check_ms = ms + CHAMBER_CHECK_INTERVAL;
+
+      if (WITHIN(temp_chamber.celsius, CHAMBER_MINTEMP, CHAMBER_MAXTEMP)) {
+        if (flag_chamber_excess_heat) {
+          temp_chamber.soft_pwm_amount = 0;
+          #if ENABLED(CHAMBER_VENT)
+            if (!flag_chamber_off) MOVE_SERVO(CHAMBER_VENT_SERVO_NR, temp_chamber.celsius <= temp_chamber.target ? 0 : 90);
+          #endif
+        }
+        else {
+          #if ENABLED(CHAMBER_LIMIT_SWITCHING)
+            if (temp_chamber.celsius >= temp_chamber.target + TEMP_CHAMBER_HYSTERESIS)
+              temp_chamber.soft_pwm_amount = 0;
+            else if (temp_chamber.celsius <= temp_chamber.target - (TEMP_CHAMBER_HYSTERESIS))
+              temp_chamber.soft_pwm_amount = (MAX_CHAMBER_POWER) >> 1;
+          #else
+            temp_chamber.soft_pwm_amount = temp_chamber.celsius < temp_chamber.target ? (MAX_CHAMBER_POWER) >> 1 : 0;
+          #endif
+          #if ENABLED(CHAMBER_VENT)
+            if (!flag_chamber_off) MOVE_SERVO(CHAMBER_VENT_SERVO_NR, 0);
+          #endif
+        }
+      }
+      else {
+        temp_chamber.soft_pwm_amount = 0;
+        WRITE_HEATER_CHAMBER(LOW);
+      }
+
+      #if ENABLED(THERMAL_PROTECTION_CHAMBER)
+        tr_state_machine[RUNAWAY_IND_CHAMBER].run(temp_chamber.celsius, temp_chamber.target, H_CHAMBER, THERMAL_PROTECTION_CHAMBER_PERIOD, THERMAL_PROTECTION_CHAMBER_HYSTERESIS);
+      #endif
+    }
+
+    // TODO: Implement true PID pwm
+    //temp_bed.soft_pwm_amount = WITHIN(temp_chamber.celsius, CHAMBER_MINTEMP, CHAMBER_MAXTEMP) ? (int)get_pid_output_chamber() >> 1 : 0;
+
+  #endif // HAS_HEATED_CHAMBER
+
+  UNUSED(ms);
+}
+
+#define TEMP_AD595(RAW)  ((RAW) * 5.0 * 100.0 / float(HAL_ADC_RANGE) / (OVERSAMPLENR) * (TEMP_SENSOR_AD595_GAIN) + TEMP_SENSOR_AD595_OFFSET)
+#define TEMP_AD8495(RAW) ((RAW) * 6.6 * 100.0 / float(HAL_ADC_RANGE) / (OVERSAMPLENR) * (TEMP_SENSOR_AD8495_GAIN) + TEMP_SENSOR_AD8495_OFFSET)
+
+/**
+ * Bisect search for the range of the 'raw' value, then interpolate
+ * proportionally between the under and over values.
+ */
+#define SCAN_THERMISTOR_TABLE(TBL,LEN) do{                            \
+  uint8_t l = 0, r = LEN, m;                                          \
+  for (;;) {                                                          \
+    m = (l + r) >> 1;                                                 \
+    if (!m) return int16_t(pgm_read_word(&TBL[0].celsius));           \
+    if (m == l || m == r) return int16_t(pgm_read_word(&TBL[LEN-1].celsius)); \
+    int16_t v00 = pgm_read_word(&TBL[m-1].value),                     \
+          v10 = pgm_read_word(&TBL[m-0].value);                       \
+         if (raw < v00) r = m;                                        \
+    else if (raw > v10) l = m;                                        \
+    else {                                                            \
+      const int16_t v01 = int16_t(pgm_read_word(&TBL[m-1].celsius)),  \
+                  v11 = int16_t(pgm_read_word(&TBL[m-0].celsius));    \
+      return v01 + (raw - v00) * float(v11 - v01) / float(v10 - v00); \
+    }                                                                 \
+  }                                                                   \
+}while(0)
+
+#if HAS_USER_THERMISTORS
+
+  user_thermistor_t Temperature::user_thermistor[USER_THERMISTORS]; // Initialized by settings.load()
+
+  void Temperature::reset_user_thermistors() {
+    user_thermistor_t default_user_thermistor[USER_THERMISTORS] = {
+      #if HEATER_0_USER_THERMISTOR
+        { true, 0, 0, HOTEND0_PULLUP_RESISTOR_OHMS, HOTEND0_RESISTANCE_25C_OHMS, 0, 0, HOTEND0_BETA, 0 },
+      #endif
+      #if HEATER_1_USER_THERMISTOR
+        { true, 0, 0, HOTEND1_PULLUP_RESISTOR_OHMS, HOTEND1_RESISTANCE_25C_OHMS, 0, 0, HOTEND1_BETA, 0 },
+      #endif
+      #if HEATER_2_USER_THERMISTOR
+        { true, 0, 0, HOTEND2_PULLUP_RESISTOR_OHMS, HOTEND2_RESISTANCE_25C_OHMS, 0, 0, HOTEND2_BETA, 0 },
+      #endif
+      #if HEATER_3_USER_THERMISTOR
+        { true, 0, 0, HOTEND3_PULLUP_RESISTOR_OHMS, HOTEND3_RESISTANCE_25C_OHMS, 0, 0, HOTEND3_BETA, 0 },
+      #endif
+      #if HEATER_4_USER_THERMISTOR
+        { true, 0, 0, HOTEND4_PULLUP_RESISTOR_OHMS, HOTEND4_RESISTANCE_25C_OHMS, 0, 0, HOTEND4_BETA, 0 },
+      #endif
+      #if HEATER_5_USER_THERMISTOR
+        { true, 0, 0, HOTEND5_PULLUP_RESISTOR_OHMS, HOTEND5_RESISTANCE_25C_OHMS, 0, 0, HOTEND5_BETA, 0 },
+      #endif
+      #if HEATER_6_USER_THERMISTOR
+        { true, 0, 0, HOTEND6_PULLUP_RESISTOR_OHMS, HOTEND6_RESISTANCE_25C_OHMS, 0, 0, HOTEND6_BETA, 0 },
+      #endif
+      #if HEATER_7_USER_THERMISTOR
+        { true, 0, 0, HOTEND7_PULLUP_RESISTOR_OHMS, HOTEND7_RESISTANCE_25C_OHMS, 0, 0, HOTEND7_BETA, 0 },
+      #endif
+      #if HEATER_BED_USER_THERMISTOR
+        { true, 0, 0, BED_PULLUP_RESISTOR_OHMS, BED_RESISTANCE_25C_OHMS, 0, 0, BED_BETA, 0 },
+      #endif
+      #if HEATER_CHAMBER_USER_THERMISTOR
+        { true, 0, 0, CHAMBER_PULLUP_RESISTOR_OHMS, CHAMBER_RESISTANCE_25C_OHMS, 0, 0, CHAMBER_BETA, 0 }
+      #endif
+    };
+    COPY(user_thermistor, default_user_thermistor);
+  }
+
+  void Temperature::log_user_thermistor(const uint8_t t_index, const bool eprom/*=false*/) {
+
+    if (eprom)
+      SERIAL_ECHOPGM("  M305 ");
+    else
+      SERIAL_ECHO_START();
+    SERIAL_CHAR('P', '0' + t_index);
+
+    const user_thermistor_t &t = user_thermistor[t_index];
+
+    SERIAL_ECHOPAIR_F(" R", t.series_res, 1);
+    SERIAL_ECHOPAIR_F_P(SP_T_STR, t.res_25, 1);
+    SERIAL_ECHOPAIR_F_P(SP_B_STR, t.beta, 1);
+    SERIAL_ECHOPAIR_F_P(SP_C_STR, t.sh_c_coeff, 9);
+    SERIAL_ECHOPGM(" ; ");
+    serialprintPGM(
+      TERN_(HEATER_0_USER_THERMISTOR, t_index == CTI_HOTEND_0 ? PSTR("HOTEND 0") :)
+      TERN_(HEATER_1_USER_THERMISTOR, t_index == CTI_HOTEND_1 ? PSTR("HOTEND 1") :)
+      TERN_(HEATER_2_USER_THERMISTOR, t_index == CTI_HOTEND_2 ? PSTR("HOTEND 2") :)
+      TERN_(HEATER_3_USER_THERMISTOR, t_index == CTI_HOTEND_3 ? PSTR("HOTEND 3") :)
+      TERN_(HEATER_4_USER_THERMISTOR, t_index == CTI_HOTEND_4 ? PSTR("HOTEND 4") :)
+      TERN_(HEATER_5_USER_THERMISTOR, t_index == CTI_HOTEND_5 ? PSTR("HOTEND 5") :)
+      TERN_(HEATER_6_USER_THERMISTOR, t_index == CTI_HOTEND_6 ? PSTR("HOTEND 6") :)
+      TERN_(HEATER_7_USER_THERMISTOR, t_index == CTI_HOTEND_7 ? PSTR("HOTEND 7") :)
+      TERN_(HEATER_BED_USER_THERMISTOR, t_index == CTI_BED ? PSTR("BED") :)
+      TERN_(HEATER_CHAMBER_USER_THERMISTOR, t_index == CTI_CHAMBER ? PSTR("CHAMBER") :)
+      nullptr
+    );
+    SERIAL_EOL();
+  }
+
+  float Temperature::user_thermistor_to_deg_c(const uint8_t t_index, const int raw) {
+    //#if (MOTHERBOARD == BOARD_RAMPS_14_EFB)
+    //  static uint32_t clocks_total = 0;
+    //  static uint32_t calls = 0;
+    //  uint32_t tcnt5 = TCNT5;
+    //#endif
+
+    if (!WITHIN(t_index, 0, COUNT(user_thermistor) - 1)) return 25;
+
+    user_thermistor_t &t = user_thermistor[t_index];
+    if (t.pre_calc) { // pre-calculate some variables
+      t.pre_calc     = false;
+      t.res_25_recip = 1.0f / t.res_25;
+      t.res_25_log   = logf(t.res_25);
+      t.beta_recip   = 1.0f / t.beta;
+      t.sh_alpha     = RECIPROCAL(THERMISTOR_RESISTANCE_NOMINAL_C - (THERMISTOR_ABS_ZERO_C))
+                        - (t.beta_recip * t.res_25_log) - (t.sh_c_coeff * cu(t.res_25_log));
+    }
+
+    // maximum adc value .. take into account the over sampling
+    const int adc_max = MAX_RAW_THERMISTOR_VALUE,
+              adc_raw = constrain(raw, 1, adc_max - 1); // constrain to prevent divide-by-zero
+
+    const float adc_inverse = (adc_max - adc_raw) - 0.5f,
+                resistance = t.series_res * (adc_raw + 0.5f) / adc_inverse,
+                log_resistance = logf(resistance);
+
+    float value = t.sh_alpha;
+    value += log_resistance * t.beta_recip;
+    if (t.sh_c_coeff != 0)
+      value += t.sh_c_coeff * cu(log_resistance);
+    value = 1.0f / value;
+
+    //#if (MOTHERBOARD == BOARD_RAMPS_14_EFB)
+    //  int32_t clocks = TCNT5 - tcnt5;
+    //  if (clocks >= 0) {
+    //    clocks_total += clocks;
+    //    calls++;
+    //  }
+    //#endif
+
+    // Return degrees C (up to 999, as the LCD only displays 3 digits)
+    return _MIN(value + THERMISTOR_ABS_ZERO_C, 999);
+  }
+#endif
+
+#if HAS_HOTEND
+  // Derived from RepRap FiveD extruder::getTemperature()
+  // For hot end temperature measurement.
+  float Temperature::analog_to_celsius_hotend(const int raw, const uint8_t e) {
+      if (e > HOTENDS - DISABLED(TEMP_SENSOR_1_AS_REDUNDANT)) {
+        SERIAL_ERROR_START();
+        SERIAL_ECHO((int)e);
+        SERIAL_ECHOLNPGM(STR_INVALID_EXTRUDER_NUM);
+        kill();
+        return 0;
+      }
+
+    switch (e) {
+      case 0:
+        #if HEATER_0_USER_THERMISTOR
+          return user_thermistor_to_deg_c(CTI_HOTEND_0, raw);
+        #elif HEATER_0_USES_MAX6675
+          return TERN(MAX6675_0_IS_MAX31865, max31865_0.temperature(MAX31865_SENSOR_OHMS_0, MAX31865_CALIBRATION_OHMS_0), raw * 0.25);
+        #elif HEATER_0_USES_AD595
+          return TEMP_AD595(raw);
+        #elif HEATER_0_USES_AD8495
+          return TEMP_AD8495(raw);
+        #else
+          break;
+        #endif
+      case 1:
+        #if HEATER_1_USER_THERMISTOR
+          return user_thermistor_to_deg_c(CTI_HOTEND_1, raw);
+        #elif HEATER_1_USES_MAX6675
+          return TERN(MAX6675_1_IS_MAX31865, max31865_1.temperature(MAX31865_SENSOR_OHMS_1, MAX31865_CALIBRATION_OHMS_1), raw * 0.25);
+        #elif HEATER_1_USES_AD595
+          return TEMP_AD595(raw);
+        #elif HEATER_1_USES_AD8495
+          return TEMP_AD8495(raw);
+        #else
+          break;
+        #endif
+      case 2:
+        #if HEATER_2_USER_THERMISTOR
+          return user_thermistor_to_deg_c(CTI_HOTEND_2, raw);
+        #elif HEATER_2_USES_AD595
+          return TEMP_AD595(raw);
+        #elif HEATER_2_USES_AD8495
+          return TEMP_AD8495(raw);
+        #else
+          break;
+        #endif
+      case 3:
+        #if HEATER_3_USER_THERMISTOR
+          return user_thermistor_to_deg_c(CTI_HOTEND_3, raw);
+        #elif HEATER_3_USES_AD595
+          return TEMP_AD595(raw);
+        #elif HEATER_3_USES_AD8495
+          return TEMP_AD8495(raw);
+        #else
+          break;
+        #endif
+      case 4:
+        #if HEATER_4_USER_THERMISTOR
+          return user_thermistor_to_deg_c(CTI_HOTEND_4, raw);
+        #elif HEATER_4_USES_AD595
+          return TEMP_AD595(raw);
+        #elif HEATER_4_USES_AD8495
+          return TEMP_AD8495(raw);
+        #else
+          break;
+        #endif
+      case 5:
+        #if HEATER_5_USER_THERMISTOR
+          return user_thermistor_to_deg_c(CTI_HOTEND_5, raw);
+        #elif HEATER_5_USES_AD595
+          return TEMP_AD595(raw);
+        #elif HEATER_5_USES_AD8495
+          return TEMP_AD8495(raw);
+        #else
+          break;
+        #endif
+      case 6:
+        #if HEATER_6_USER_THERMISTOR
+          return user_thermistor_to_deg_c(CTI_HOTEND_6, raw);
+        #elif HEATER_6_USES_AD595
+          return TEMP_AD595(raw);
+        #elif HEATER_6_USES_AD8495
+          return TEMP_AD8495(raw);
+        #else
+          break;
+        #endif
+      case 7:
+        #if HEATER_7_USER_THERMISTOR
+          return user_thermistor_to_deg_c(CTI_HOTEND_7, raw);
+        #elif HEATER_7_USES_AD595
+          return TEMP_AD595(raw);
+        #elif HEATER_7_USES_AD8495
+          return TEMP_AD8495(raw);
+        #else
+          break;
+        #endif
+      default: break;
+    }
+
+    #if HAS_HOTEND_THERMISTOR
+      // Thermistor with conversion table?
+      const temp_entry_t(*tt)[] = (temp_entry_t(*)[])(heater_ttbl_map[e]);
+      SCAN_THERMISTOR_TABLE((*tt), heater_ttbllen_map[e]);
+    #endif
+
+    return 0;
+  }
+#endif // HAS_HOTEND
+
+#if HAS_HEATED_BED
+  // Derived from RepRap FiveD extruder::getTemperature()
+  // For bed temperature measurement.
+  float Temperature::analog_to_celsius_bed(const int raw) {
+    #if HEATER_BED_USER_THERMISTOR
+      return user_thermistor_to_deg_c(CTI_BED, raw);
+    #elif HEATER_BED_USES_THERMISTOR
+      SCAN_THERMISTOR_TABLE(BED_TEMPTABLE, BED_TEMPTABLE_LEN);
+    #elif HEATER_BED_USES_AD595
+      return TEMP_AD595(raw);
+    #elif HEATER_BED_USES_AD8495
+      return TEMP_AD8495(raw);
+    #else
+      UNUSED(raw);
+      return 0;
+    #endif
+  }
+#endif // HAS_HEATED_BED
+
+#if HAS_TEMP_CHAMBER
+  // Derived from RepRap FiveD extruder::getTemperature()
+  // For chamber temperature measurement.
+  float Temperature::analog_to_celsius_chamber(const int raw) {
+    #if HEATER_CHAMBER_USER_THERMISTOR
+      return user_thermistor_to_deg_c(CTI_CHAMBER, raw);
+    #elif HEATER_CHAMBER_USES_THERMISTOR
+      SCAN_THERMISTOR_TABLE(CHAMBER_TEMPTABLE, CHAMBER_TEMPTABLE_LEN);
+    #elif HEATER_CHAMBER_USES_AD595
+      return TEMP_AD595(raw);
+    #elif HEATER_CHAMBER_USES_AD8495
+      return TEMP_AD8495(raw);
+    #else
+      UNUSED(raw);
+      return 0;
+    #endif
+  }
+#endif // HAS_TEMP_CHAMBER
+
+#if HAS_TEMP_PROBE
+  // Derived from RepRap FiveD extruder::getTemperature()
+  // For probe temperature measurement.
+  float Temperature::analog_to_celsius_probe(const int raw) {
+    #if HEATER_PROBE_USER_THERMISTOR
+      return user_thermistor_to_deg_c(CTI_PROBE, raw);
+    #elif HEATER_PROBE_USES_THERMISTOR
+      SCAN_THERMISTOR_TABLE(PROBE_TEMPTABLE, PROBE_TEMPTABLE_LEN);
+    #elif HEATER_PROBE_USES_AD595
+      return TEMP_AD595(raw);
+    #elif HEATER_PROBE_USES_AD8495
+      return TEMP_AD8495(raw);
+    #else
+      UNUSED(raw);
+      return 0;
+    #endif
+  }
+#endif // HAS_TEMP_PROBE
+
+/**
+ * Get the raw values into the actual temperatures.
+ * The raw values are created in interrupt context,
+ * and this function is called from normal context
+ * as it would block the stepper routine.
+ */
+void Temperature::updateTemperaturesFromRawValues() {
+  TERN_(HEATER_0_USES_MAX6675, temp_hotend[0].raw = READ_MAX6675(0));
+  TERN_(HEATER_1_USES_MAX6675, temp_hotend[1].raw = READ_MAX6675(1));
+  #if HAS_HOTEND
+    HOTEND_LOOP() temp_hotend[e].celsius = analog_to_celsius_hotend(temp_hotend[e].raw, e);
+  #endif
+  TERN_(HAS_HEATED_BED, temp_bed.celsius = analog_to_celsius_bed(temp_bed.raw));
+  TERN_(HAS_TEMP_CHAMBER, temp_chamber.celsius = analog_to_celsius_chamber(temp_chamber.raw));
+  TERN_(HAS_TEMP_PROBE, temp_probe.celsius = analog_to_celsius_probe(temp_probe.raw));
+  TERN_(TEMP_SENSOR_1_AS_REDUNDANT, redundant_temperature = analog_to_celsius_hotend(redundant_temperature_raw, 1));
+  TERN_(FILAMENT_WIDTH_SENSOR, filwidth.update_measured_mm());
+  TERN_(HAS_POWER_MONITOR, power_monitor.capture_values());
+
+  // Reset the watchdog on good temperature measurement
+  watchdog_refresh();
+
+  raw_temps_ready = false;
+}
+
+#if MAX6675_SEPARATE_SPI
+  template<uint8_t MisoPin, uint8_t MosiPin, uint8_t SckPin> SoftSPI<MisoPin, MosiPin, SckPin> SPIclass<MisoPin, MosiPin, SckPin>::softSPI;
+  SPIclass<MAX6675_DO_PIN, SD_MOSI_PIN, MAX6675_SCK_PIN> max6675_spi;
+#endif
+
+// Init fans according to whether they're native PWM or Software PWM
+#ifdef ALFAWISE_UX0
+  #define _INIT_SOFT_FAN(P) OUT_WRITE_OD(P, FAN_INVERTING ? LOW : HIGH)
+#else
+  #define _INIT_SOFT_FAN(P) OUT_WRITE(P, FAN_INVERTING ? LOW : HIGH)
+#endif
+#if ENABLED(FAN_SOFT_PWM)
+  #define _INIT_FAN_PIN(P) _INIT_SOFT_FAN(P)
+#else
+  #define _INIT_FAN_PIN(P) do{ if (PWM_PIN(P)) SET_PWM(P); else _INIT_SOFT_FAN(P); }while(0)
+#endif
+#if ENABLED(FAST_PWM_FAN)
+  #define SET_FAST_PWM_FREQ(P) set_pwm_frequency(P, FAST_PWM_FAN_FREQUENCY)
+#else
+  #define SET_FAST_PWM_FREQ(P) NOOP
+#endif
+#define INIT_FAN_PIN(P) do{ _INIT_FAN_PIN(P); SET_FAST_PWM_FREQ(P); }while(0)
+#if EXTRUDER_AUTO_FAN_SPEED != 255
+  #define INIT_E_AUTO_FAN_PIN(P) do{ if (P == FAN1_PIN || P == FAN2_PIN) { SET_PWM(P); SET_FAST_PWM_FREQ(P); } else SET_OUTPUT(P); }while(0)
+#else
+  #define INIT_E_AUTO_FAN_PIN(P) SET_OUTPUT(P)
+#endif
+#if CHAMBER_AUTO_FAN_SPEED != 255
+  #define INIT_CHAMBER_AUTO_FAN_PIN(P) do{ if (P == FAN1_PIN || P == FAN2_PIN) { SET_PWM(P); SET_FAST_PWM_FREQ(P); } else SET_OUTPUT(P); }while(0)
+#else
+  #define INIT_CHAMBER_AUTO_FAN_PIN(P) SET_OUTPUT(P)
+#endif
+
+/**
+ * Initialize the temperature manager
+ * The manager is implemented by periodic calls to manage_heater()
+ */
+void Temperature::init() {
+
+  TERN_(MAX6675_0_IS_MAX31865, max31865_0.begin(MAX31865_2WIRE)); // MAX31865_2WIRE, MAX31865_3WIRE, MAX31865_4WIRE
+  TERN_(MAX6675_1_IS_MAX31865, max31865_1.begin(MAX31865_2WIRE));
+
+  #if EARLY_WATCHDOG
+    // Flag that the thermalManager should be running
+    if (inited) return;
+    inited = true;
+  #endif
+
+  #if MB(RUMBA)
+    // Disable RUMBA JTAG in case the thermocouple extension is plugged on top of JTAG connector
+    #define _AD(N) (HEATER_##N##_USES_AD595 || HEATER_##N##_USES_AD8495)
+    #if _AD(0) || _AD(1) || _AD(2) || _AD(BED) || _AD(CHAMBER)
+      MCUCR = _BV(JTD);
+      MCUCR = _BV(JTD);
+    #endif
+  #endif
+
+  // Thermistor activation by MCU pin
+  #if PIN_EXISTS(TEMP_0_TR_ENABLE_PIN)
+    OUT_WRITE(TEMP_0_TR_ENABLE_PIN, ENABLED(HEATER_0_USES_MAX6675));
+  #endif
+  #if PIN_EXISTS(TEMP_1_TR_ENABLE_PIN)
+    OUT_WRITE(TEMP_1_TR_ENABLE_PIN, ENABLED(HEATER_1_USES_MAX6675));
+  #endif
+
+  #if BOTH(PIDTEMP, PID_EXTRUSION_SCALING)
+    last_e_position = 0;
+  #endif
+
+  #if HAS_HEATER_0
+    #ifdef ALFAWISE_UX0
+      OUT_WRITE_OD(HEATER_0_PIN, HEATER_0_INVERTING);
+    #else
+      OUT_WRITE(HEATER_0_PIN, HEATER_0_INVERTING);
+    #endif
+  #endif
+
+  #if HAS_HEATER_1
+    OUT_WRITE(HEATER_1_PIN, HEATER_1_INVERTING);
+  #endif
+  #if HAS_HEATER_2
+    OUT_WRITE(HEATER_2_PIN, HEATER_2_INVERTING);
+  #endif
+  #if HAS_HEATER_3
+    OUT_WRITE(HEATER_3_PIN, HEATER_3_INVERTING);
+  #endif
+  #if HAS_HEATER_4
+    OUT_WRITE(HEATER_4_PIN, HEATER_4_INVERTING);
+  #endif
+  #if HAS_HEATER_5
+    OUT_WRITE(HEATER_5_PIN, HEATER_5_INVERTING);
+  #endif
+  #if HAS_HEATER_6
+    OUT_WRITE(HEATER_6_PIN, HEATER_6_INVERTING);
+  #endif
+  #if HAS_HEATER_7
+    OUT_WRITE(HEATER_7_PIN, HEATER_7_INVERTING);
+  #endif
+
+  #if HAS_HEATED_BED
+    #ifdef ALFAWISE_UX0
+      OUT_WRITE_OD(HEATER_BED_PIN, HEATER_BED_INVERTING);
+    #else
+      OUT_WRITE(HEATER_BED_PIN, HEATER_BED_INVERTING);
+    #endif
+  #endif
+
+  #if HAS_HEATED_CHAMBER
+    OUT_WRITE(HEATER_CHAMBER_PIN, HEATER_CHAMBER_INVERTING);
+  #endif
+
+  #if HAS_FAN0
+    INIT_FAN_PIN(FAN_PIN);
+  #endif
+  #if HAS_FAN1
+    INIT_FAN_PIN(FAN1_PIN);
+  #endif
+  #if HAS_FAN2
+    INIT_FAN_PIN(FAN2_PIN);
+  #endif
+  #if HAS_FAN3
+    INIT_FAN_PIN(FAN3_PIN);
+  #endif
+  #if HAS_FAN4
+    INIT_FAN_PIN(FAN4_PIN);
+  #endif
+  #if HAS_FAN5
+    INIT_FAN_PIN(FAN5_PIN);
+  #endif
+  #if HAS_FAN6
+    INIT_FAN_PIN(FAN6_PIN);
+  #endif
+  #if HAS_FAN7
+    INIT_FAN_PIN(FAN7_PIN);
+  #endif
+  #if ENABLED(USE_CONTROLLER_FAN)
+    INIT_FAN_PIN(CONTROLLER_FAN_PIN);
+  #endif
+
+  TERN_(MAX6675_SEPARATE_SPI, max6675_spi.init());
+
+  HAL_adc_init();
+
+  #if HAS_TEMP_ADC_0
+    HAL_ANALOG_SELECT(TEMP_0_PIN);
+  #endif
+  #if HAS_TEMP_ADC_1
+    HAL_ANALOG_SELECT(TEMP_1_PIN);
+  #endif
+  #if HAS_TEMP_ADC_2
+    HAL_ANALOG_SELECT(TEMP_2_PIN);
+  #endif
+  #if HAS_TEMP_ADC_3
+    HAL_ANALOG_SELECT(TEMP_3_PIN);
+  #endif
+  #if HAS_TEMP_ADC_4
+    HAL_ANALOG_SELECT(TEMP_4_PIN);
+  #endif
+  #if HAS_TEMP_ADC_5
+    HAL_ANALOG_SELECT(TEMP_5_PIN);
+  #endif
+  #if HAS_TEMP_ADC_6
+    HAL_ANALOG_SELECT(TEMP_6_PIN);
+  #endif
+  #if HAS_TEMP_ADC_7
+    HAL_ANALOG_SELECT(TEMP_7_PIN);
+  #endif
+  #if HAS_JOY_ADC_X
+    HAL_ANALOG_SELECT(JOY_X_PIN);
+  #endif
+  #if HAS_JOY_ADC_Y
+    HAL_ANALOG_SELECT(JOY_Y_PIN);
+  #endif
+  #if HAS_JOY_ADC_Z
+    HAL_ANALOG_SELECT(JOY_Z_PIN);
+  #endif
+  #if HAS_JOY_ADC_EN
+    SET_INPUT_PULLUP(JOY_EN_PIN);
+  #endif
+  #if HAS_TEMP_ADC_BED
+    HAL_ANALOG_SELECT(TEMP_BED_PIN);
+  #endif
+  #if HAS_TEMP_ADC_CHAMBER
+    HAL_ANALOG_SELECT(TEMP_CHAMBER_PIN);
+  #endif
+  #if HAS_TEMP_ADC_PROBE
+    HAL_ANALOG_SELECT(TEMP_PROBE_PIN);
+  #endif
+  #if ENABLED(FILAMENT_WIDTH_SENSOR)
+    HAL_ANALOG_SELECT(FILWIDTH_PIN);
+  #endif
+  #if HAS_ADC_BUTTONS
+    HAL_ANALOG_SELECT(ADC_KEYPAD_PIN);
+  #endif
+  #if ENABLED(POWER_MONITOR_CURRENT)
+    HAL_ANALOG_SELECT(POWER_MONITOR_CURRENT_PIN);
+  #endif
+  #if ENABLED(POWER_MONITOR_VOLTAGE)
+    HAL_ANALOG_SELECT(POWER_MONITOR_VOLTAGE_PIN);
+  #endif
+
+  HAL_timer_start(TEMP_TIMER_NUM, TEMP_TIMER_FREQUENCY);
+  ENABLE_TEMPERATURE_INTERRUPT();
+
+  #if HAS_AUTO_FAN_0
+    INIT_E_AUTO_FAN_PIN(E0_AUTO_FAN_PIN);
+  #endif
+  #if HAS_AUTO_FAN_1 && !_EFANOVERLAP(1,0)
+    INIT_E_AUTO_FAN_PIN(E1_AUTO_FAN_PIN);
+  #endif
+  #if HAS_AUTO_FAN_2 && !(_EFANOVERLAP(2,0) || _EFANOVERLAP(2,1))
+    INIT_E_AUTO_FAN_PIN(E2_AUTO_FAN_PIN);
+  #endif
+  #if HAS_AUTO_FAN_3 && !(_EFANOVERLAP(3,0) || _EFANOVERLAP(3,1) || _EFANOVERLAP(3,2))
+    INIT_E_AUTO_FAN_PIN(E3_AUTO_FAN_PIN);
+  #endif
+  #if HAS_AUTO_FAN_4 && !(_EFANOVERLAP(4,0) || _EFANOVERLAP(4,1) || _EFANOVERLAP(4,2) || _EFANOVERLAP(4,3))
+    INIT_E_AUTO_FAN_PIN(E4_AUTO_FAN_PIN);
+  #endif
+  #if HAS_AUTO_FAN_5 && !(_EFANOVERLAP(5,0) || _EFANOVERLAP(5,1) || _EFANOVERLAP(5,2) || _EFANOVERLAP(5,3) || _EFANOVERLAP(5,4))
+    INIT_E_AUTO_FAN_PIN(E5_AUTO_FAN_PIN);
+  #endif
+  #if HAS_AUTO_FAN_6 && !(_EFANOVERLAP(6,0) || _EFANOVERLAP(6,1) || _EFANOVERLAP(6,2) || _EFANOVERLAP(6,3) || _EFANOVERLAP(6,4) || _EFANOVERLAP(6,5))
+    INIT_E_AUTO_FAN_PIN(E6_AUTO_FAN_PIN);
+  #endif
+  #if HAS_AUTO_FAN_7 && !(_EFANOVERLAP(7,0) || _EFANOVERLAP(7,1) || _EFANOVERLAP(7,2) || _EFANOVERLAP(7,3) || _EFANOVERLAP(7,4) || _EFANOVERLAP(7,5) || _EFANOVERLAP(7,6))
+    INIT_E_AUTO_FAN_PIN(E7_AUTO_FAN_PIN);
+  #endif
+  #if HAS_AUTO_CHAMBER_FAN && !AUTO_CHAMBER_IS_E
+    INIT_CHAMBER_AUTO_FAN_PIN(CHAMBER_AUTO_FAN_PIN);
+  #endif
+
+  // Wait for temperature measurement to settle
+  delay(250);
+
+  #if HAS_HOTEND
+
+    #define _TEMP_MIN_E(NR) do{ \
+      const int16_t tmin = _MAX(HEATER_ ##NR## _MINTEMP, TERN(HEATER_##NR##_USER_THERMISTOR, 0, (int16_t)pgm_read_word(&HEATER_ ##NR## _TEMPTABLE[HEATER_ ##NR## _SENSOR_MINTEMP_IND].celsius))); \
+      temp_range[NR].mintemp = tmin; \
+      while (analog_to_celsius_hotend(temp_range[NR].raw_min, NR) < tmin) \
+        temp_range[NR].raw_min += TEMPDIR(NR) * (OVERSAMPLENR); \
+    }while(0)
+    #define _TEMP_MAX_E(NR) do{ \
+      const int16_t tmax = _MIN(HEATER_ ##NR## _MAXTEMP, TERN(HEATER_##NR##_USER_THERMISTOR, 2000, (int16_t)pgm_read_word(&HEATER_ ##NR## _TEMPTABLE[HEATER_ ##NR## _SENSOR_MAXTEMP_IND].celsius) - 1)); \
+      temp_range[NR].maxtemp = tmax; \
+      while (analog_to_celsius_hotend(temp_range[NR].raw_max, NR) > tmax) \
+        temp_range[NR].raw_max -= TEMPDIR(NR) * (OVERSAMPLENR); \
+    }while(0)
+
+    #define _MINMAX_TEST(N,M) (HOTENDS > N && THERMISTOR_HEATER_##N && THERMISTOR_HEATER_##N != 998 && THERMISTOR_HEATER_##N != 999 && defined(HEATER_##N##_##M##TEMP))
+
+    #if _MINMAX_TEST(0, MIN)
+      _TEMP_MIN_E(0);
+    #endif
+    #if _MINMAX_TEST(0, MAX)
+      _TEMP_MAX_E(0);
+    #endif
+    #if _MINMAX_TEST(1, MIN)
+      _TEMP_MIN_E(1);
+    #endif
+    #if _MINMAX_TEST(1, MAX)
+      _TEMP_MAX_E(1);
+    #endif
+    #if _MINMAX_TEST(2, MIN)
+      _TEMP_MIN_E(2);
+    #endif
+    #if _MINMAX_TEST(2, MAX)
+      _TEMP_MAX_E(2);
+    #endif
+    #if _MINMAX_TEST(3, MIN)
+      _TEMP_MIN_E(3);
+    #endif
+    #if _MINMAX_TEST(3, MAX)
+      _TEMP_MAX_E(3);
+    #endif
+    #if _MINMAX_TEST(4, MIN)
+      _TEMP_MIN_E(4);
+    #endif
+    #if _MINMAX_TEST(4, MAX)
+      _TEMP_MAX_E(4);
+    #endif
+    #if _MINMAX_TEST(5, MIN)
+      _TEMP_MIN_E(5);
+    #endif
+    #if _MINMAX_TEST(5, MAX)
+      _TEMP_MAX_E(5);
+    #endif
+    #if _MINMAX_TEST(6, MIN)
+      _TEMP_MIN_E(6);
+    #endif
+    #if _MINMAX_TEST(6, MAX)
+      _TEMP_MAX_E(6);
+    #endif
+    #if _MINMAX_TEST(7, MIN)
+      _TEMP_MIN_E(7);
+    #endif
+    #if _MINMAX_TEST(7, MAX)
+      _TEMP_MAX_E(7);
+    #endif
+
+  #endif // HAS_HOTEND
+
+  #if HAS_HEATED_BED
+    #ifdef BED_MINTEMP
+      while (analog_to_celsius_bed(mintemp_raw_BED) < BED_MINTEMP) mintemp_raw_BED += TEMPDIR(BED) * (OVERSAMPLENR);
+    #endif
+    #ifdef BED_MAXTEMP
+      while (analog_to_celsius_bed(maxtemp_raw_BED) > BED_MAXTEMP) maxtemp_raw_BED -= TEMPDIR(BED) * (OVERSAMPLENR);
+    #endif
+  #endif // HAS_HEATED_BED
+
+  #if HAS_HEATED_CHAMBER
+    #ifdef CHAMBER_MINTEMP
+      while (analog_to_celsius_chamber(mintemp_raw_CHAMBER) < CHAMBER_MINTEMP) mintemp_raw_CHAMBER += TEMPDIR(CHAMBER) * (OVERSAMPLENR);
+    #endif
+    #ifdef CHAMBER_MAXTEMP
+      while (analog_to_celsius_chamber(maxtemp_raw_CHAMBER) > CHAMBER_MAXTEMP) maxtemp_raw_CHAMBER -= TEMPDIR(CHAMBER) * (OVERSAMPLENR);
+    #endif
+  #endif
+
+  TERN_(PROBING_HEATERS_OFF, paused = false);
+}
+
+#if WATCH_HOTENDS
+  /**
+   * Start Heating Sanity Check for hotends that are below
+   * their target temperature by a configurable margin.
+   * This is called when the temperature is set. (M104, M109)
+   */
+  void Temperature::start_watching_hotend(const uint8_t E_NAME) {
+    const uint8_t ee = HOTEND_INDEX;
+    watch_hotend[ee].restart(degHotend(ee), degTargetHotend(ee));
+  }
+#endif
+
+#if WATCH_BED
+  /**
+   * Start Heating Sanity Check for hotends that are below
+   * their target temperature by a configurable margin.
+   * This is called when the temperature is set. (M140, M190)
+   */
+  void Temperature::start_watching_bed() {
+    watch_bed.restart(degBed(), degTargetBed());
+  }
+#endif
+
+#if WATCH_CHAMBER
+  /**
+   * Start Heating Sanity Check for chamber that is below
+   * its target temperature by a configurable margin.
+   * This is called when the temperature is set. (M141, M191)
+   */
+  void Temperature::start_watching_chamber() {
+    watch_chamber.restart(degChamber(), degTargetChamber());
+  }
+#endif
+
+#if HAS_THERMAL_PROTECTION
+
+  Temperature::tr_state_machine_t Temperature::tr_state_machine[NR_HEATER_RUNAWAY]; // = { { TRInactive, 0 } };
+
+  /**
+   * @brief Thermal Runaway state machine for a single heater
+   * @param current          current measured temperature
+   * @param target           current target temperature
+   * @param heater_id        extruder index
+   * @param period_seconds   missed temperature allowed time
+   * @param hysteresis_degc  allowed distance from target
+   *
+   * TODO: Embed the last 3 parameters during init, if not less optimal
+   */
+  void Temperature::tr_state_machine_t::run(const float &current, const float &target, const heater_id_t heater_id, const uint16_t period_seconds, const uint16_t hysteresis_degc) {
+
+    #if HEATER_IDLE_HANDLER
+      // Convert the given heater_id_t to an idle array index
+      const IdleIndex idle_index = idle_index_for_id(heater_id);
+    #endif
+
+    /**
+      SERIAL_ECHO_START();
+      SERIAL_ECHOPGM("Thermal Runaway Running. Heater ID: ");
+      switch (heater_id) {
+        case H_BED:     SERIAL_ECHOPGM("bed"); break;
+        case H_CHAMBER: SERIAL_ECHOPGM("chamber"); break;
+        default:        SERIAL_ECHO((int)heater_id);
+      }
+      SERIAL_ECHOLNPAIR(
+        " ; sizeof(running_temp):", sizeof(running_temp),
+        " ;  State:", state, " ;  Timer:", timer, " ;  Temperature:", current, " ;  Target Temp:", target
+        #if HEATER_IDLE_HANDLER
+          , " ;  Idle Timeout:", heater_idle[idle_index].timed_out
+        #endif
+      );
+    //*/
+
+    #if HEATER_IDLE_HANDLER
+      // If the heater idle timeout expires, restart
+      if (heater_idle[idle_index].timed_out) {
+        state = TRInactive;
+        running_temp = 0;
+      }
+      else
+    #endif
+    {
+      // If the target temperature changes, restart
+      if (running_temp != target) {
+        running_temp = target;
+        state = target > 0 ? TRFirstHeating : TRInactive;
+      }
+    }
+
+    switch (state) {
+      // Inactive state waits for a target temperature to be set
+      case TRInactive: break;
+
+      // When first heating, wait for the temperature to be reached then go to Stable state
+      case TRFirstHeating:
+        if (current < running_temp) break;
+        state = TRStable;
+
+      // While the temperature is stable watch for a bad temperature
+      case TRStable:
+
+        #if ENABLED(ADAPTIVE_FAN_SLOWING)
+          if (adaptive_fan_slowing && heater_id >= 0) {
+            const int fan_index = _MIN(heater_id, FAN_COUNT - 1);
+            if (fan_speed[fan_index] == 0 || current >= running_temp - (hysteresis_degc * 0.25f))
+              fan_speed_scaler[fan_index] = 128;
+            else if (current >= running_temp - (hysteresis_degc * 0.3335f))
+              fan_speed_scaler[fan_index] = 96;
+            else if (current >= running_temp - (hysteresis_degc * 0.5f))
+              fan_speed_scaler[fan_index] = 64;
+            else if (current >= running_temp - (hysteresis_degc * 0.8f))
+              fan_speed_scaler[fan_index] = 32;
+            else
+              fan_speed_scaler[fan_index] = 0;
+          }
+        #endif
+
+        if (current >= running_temp - hysteresis_degc) {
+          timer = millis() + SEC_TO_MS(period_seconds);
+          break;
+        }
+        else if (PENDING(millis(), timer)) break;
+        state = TRRunaway;
+
+      case TRRunaway:
+        TERN_(DWIN_CREALITY_LCD, DWIN_Popup_Temperature(0));
+        _temp_error(heater_id, str_t_thermal_runaway, GET_TEXT(MSG_THERMAL_RUNAWAY));
+    }
+  }
+
+#endif // HAS_THERMAL_PROTECTION
+
+void Temperature::disable_all_heaters() {
+
+  TERN_(AUTOTEMP, planner.autotemp_enabled = false);
+
+  // Unpause and reset everything
+  TERN_(PROBING_HEATERS_OFF, pause(false));
+
+  #if HAS_HOTEND
+    HOTEND_LOOP() {
+      setTargetHotend(0, e);
+      temp_hotend[e].soft_pwm_amount = 0;
+    }
+  #endif
+
+  #if HAS_TEMP_HOTEND
+    #define DISABLE_HEATER(N) WRITE_HEATER_##N(LOW);
+    REPEAT(HOTENDS, DISABLE_HEATER);
+  #endif
+
+  #if HAS_HEATED_BED
+    setTargetBed(0);
+    temp_bed.soft_pwm_amount = 0;
+    WRITE_HEATER_BED(LOW);
+  #endif
+
+  #if HAS_HEATED_CHAMBER
+    setTargetChamber(0);
+    temp_chamber.soft_pwm_amount = 0;
+    WRITE_HEATER_CHAMBER(LOW);
+  #endif
+}
+
+#if ENABLED(PRINTJOB_TIMER_AUTOSTART)
+
+  bool Temperature::auto_job_over_threshold() {
+    #if HAS_HOTEND
+      HOTEND_LOOP() if (degTargetHotend(e) > (EXTRUDE_MINTEMP) / 2) return true;
+    #endif
+    return TERN0(HAS_HEATED_BED, degTargetBed() > BED_MINTEMP)
+        || TERN0(HAS_HEATED_CHAMBER, degTargetChamber() > CHAMBER_MINTEMP);
+  }
+
+  void Temperature::auto_job_check_timer(const bool can_start, const bool can_stop) {
+    if (auto_job_over_threshold()) {
+      if (can_start) startOrResumeJob();
+    }
+    else if (can_stop) {
+      print_job_timer.stop();
+      ui.reset_status();
+    }
+  }
+
+#endif
+
+#if ENABLED(PROBING_HEATERS_OFF)
+
+  void Temperature::pause(const bool p) {
+    if (p != paused) {
+      paused = p;
+      if (p) {
+        HOTEND_LOOP() heater_idle[e].expire();    // Timeout immediately
+        TERN_(HAS_HEATED_BED, heater_idle[IDLE_INDEX_BED].expire()); // Timeout immediately
+      }
+      else {
+        HOTEND_LOOP() reset_hotend_idle_timer(e);
+        TERN_(HAS_HEATED_BED, reset_bed_idle_timer());
+      }
+    }
+  }
+
+#endif // PROBING_HEATERS_OFF
+
+#if ENABLED(SINGLENOZZLE_STANDBY_TEMP)
+
+  void Temperature::singlenozzle_change(const uint8_t old_tool, const uint8_t new_tool) {
+    #if HAS_FAN
+      singlenozzle_fan_speed[old_tool] = fan_speed[0];
+      fan_speed[0] = singlenozzle_fan_speed[new_tool];
+    #endif
+    singlenozzle_temp[old_tool] = temp_hotend[0].target;
+    if (singlenozzle_temp[new_tool] && singlenozzle_temp[new_tool] != singlenozzle_temp[old_tool]) {
+      setTargetHotend(singlenozzle_temp[new_tool], 0);
+      TERN_(AUTOTEMP, planner.autotemp_update());
+      TERN_(HAS_DISPLAY, set_heating_message(0));
+      (void)wait_for_hotend(0, false);  // Wait for heating or cooling
+    }
+  }
+
+#endif
+
+#if HAS_MAX6675
+
+  #ifndef THERMOCOUPLE_MAX_ERRORS
+    #define THERMOCOUPLE_MAX_ERRORS 15
+  #endif
+
+  int Temperature::read_max6675(TERN_(HAS_MULTI_6675, const uint8_t hindex/*=0*/)) {
+    #define MAX6675_HEAT_INTERVAL 250UL
+
+    #if MAX6675_0_IS_MAX31855 || MAX6675_1_IS_MAX31855
+      static uint32_t max6675_temp = 2000;
+      #define MAX6675_ERROR_MASK    7
+      #define MAX6675_DISCARD_BITS 18
+      #define MAX6675_SPEED_BITS    3       // (_BV(SPR1)) // clock ÷ 64
+    #elif HAS_MAX31865
+      static uint16_t max6675_temp = 2000;  // From datasheet 16 bits D15-D0
+      #define MAX6675_ERROR_MASK    1       // D0 Bit not used
+      #define MAX6675_DISCARD_BITS  1       // Data is in D15-D1
+      #define MAX6675_SPEED_BITS    3       //  (_BV(SPR1)) // clock ÷ 64
+    #else
+      static uint16_t max6675_temp = 2000;
+      #define MAX6675_ERROR_MASK    4
+      #define MAX6675_DISCARD_BITS  3
+      #define MAX6675_SPEED_BITS    2       // (_BV(SPR0)) // clock ÷ 16
+    #endif
+
+    #if HAS_MULTI_6675
+      // Needed to return the correct temp when this is called between readings
+      static uint16_t max6675_temp_previous[COUNT_6675] = { 0 };
+      #define MAX6675_TEMP(I) max6675_temp_previous[I]
+      #define MAX6675_SEL(A,B) (hindex ? (B) : (A))
+      #define MAX6675_WRITE(V)     do{ switch (hindex) { case 1:      WRITE(MAX6675_SS2_PIN, V); break; default:      WRITE(MAX6675_SS_PIN, V); } }while(0)
+      #define MAX6675_SET_OUTPUT() do{ switch (hindex) { case 1: SET_OUTPUT(MAX6675_SS2_PIN);    break; default: SET_OUTPUT(MAX6675_SS_PIN);    } }while(0)
+    #else
+      constexpr uint8_t hindex = 0;
+      #define MAX6675_TEMP(I) max6675_temp
+      #if MAX6675_1_IS_MAX31865
+        #define MAX6675_SEL(A,B) B
+      #else
+        #define MAX6675_SEL(A,B) A
+      #endif
+      #if HEATER_0_USES_MAX6675
+        #define MAX6675_WRITE(V)          WRITE(MAX6675_SS_PIN, V)
+        #define MAX6675_SET_OUTPUT() SET_OUTPUT(MAX6675_SS_PIN)
+      #else
+        #define MAX6675_WRITE(V)          WRITE(MAX6675_SS2_PIN, V)
+        #define MAX6675_SET_OUTPUT() SET_OUTPUT(MAX6675_SS2_PIN)
+      #endif
+    #endif
+
+    static uint8_t max6675_errors[COUNT_6675] = { 0 };
+
+    // Return last-read value between readings
+    static millis_t next_max6675_ms[COUNT_6675] = { 0 };
+    millis_t ms = millis();
+    if (PENDING(ms, next_max6675_ms[hindex])) return int(MAX6675_TEMP(hindex));
+    next_max6675_ms[hindex] = ms + MAX6675_HEAT_INTERVAL;
+
+    #if HAS_MAX31865
+      Adafruit_MAX31865 &maxref = MAX6675_SEL(max31865_0, max31865_1);
+      const uint16_t max31865_ohms = (uint32_t(maxref.readRTD()) * MAX6675_SEL(MAX31865_CALIBRATION_OHMS_0, MAX31865_CALIBRATION_OHMS_1)) >> 16;
+    #endif
+
+    //
+    // TODO: spiBegin, spiRec and spiInit doesn't work when soft spi is used.
+    //
+    #if !MAX6675_SEPARATE_SPI
+      spiBegin();
+      spiInit(MAX6675_SPEED_BITS);
+    #endif
+
+    MAX6675_WRITE(LOW);  // enable TT_MAX6675
+    DELAY_NS(100);       // Ensure 100ns delay
+
+    // Read a big-endian temperature value
+    max6675_temp = 0;
+    for (uint8_t i = sizeof(max6675_temp); i--;) {
+      max6675_temp |= TERN(MAX6675_SEPARATE_SPI, max6675_spi.receive(), spiRec());
+      if (i > 0) max6675_temp <<= 8; // shift left if not the last byte
+    }
+
+    MAX6675_WRITE(HIGH); // disable TT_MAX6675
+
+    const uint8_t fault_31865 = TERN1(HAS_MAX31865, maxref.readFault());
+
+    if (DISABLED(IGNORE_THERMOCOUPLE_ERRORS) && (max6675_temp & MAX6675_ERROR_MASK) && fault_31865) {
+      max6675_errors[hindex]++;
+      if (max6675_errors[hindex] > THERMOCOUPLE_MAX_ERRORS) {
+        SERIAL_ERROR_START();
+        SERIAL_ECHOPGM("Temp measurement error! ");
+        #if MAX6675_ERROR_MASK == 7
+          SERIAL_ECHOPGM("MAX31855 ");
+          if (max6675_temp & 1)
+            SERIAL_ECHOLNPGM("Open Circuit");
+          else if (max6675_temp & 2)
+            SERIAL_ECHOLNPGM("Short to GND");
+          else if (max6675_temp & 4)
+            SERIAL_ECHOLNPGM("Short to VCC");
+        #elif HAS_MAX31865
+          if (fault_31865) {
+            maxref.clearFault();
+            SERIAL_ECHOPAIR("MAX31865 Fault :(", fault_31865, ")  >>");
+            if (fault_31865 & MAX31865_FAULT_HIGHTHRESH)
+              SERIAL_ECHOLNPGM("RTD High Threshold");
+            else if (fault_31865 & MAX31865_FAULT_LOWTHRESH)
+              SERIAL_ECHOLNPGM("RTD Low Threshold");
+            else if (fault_31865 & MAX31865_FAULT_REFINLOW)
+              SERIAL_ECHOLNPGM("REFIN- > 0.85 x Bias");
+            else if (fault_31865 & MAX31865_FAULT_REFINHIGH)
+              SERIAL_ECHOLNPGM("REFIN- < 0.85 x Bias - FORCE- open");
+            else if (fault_31865 & MAX31865_FAULT_RTDINLOW)
+              SERIAL_ECHOLNPGM("REFIN- < 0.85 x Bias - FORCE- open");
+            else if (fault_31865 & MAX31865_FAULT_OVUV)
+              SERIAL_ECHOLNPGM("Under/Over voltage");
+          }
+        #else
+          SERIAL_ECHOLNPGM("MAX6675");
+        #endif
+
+        // Thermocouple open
+        max6675_temp = 4 * MAX6675_SEL(HEATER_0_MAX6675_TMAX, HEATER_1_MAX6675_TMAX);
+      }
+      else
+        max6675_temp >>= MAX6675_DISCARD_BITS;
+    }
+    else {
+      max6675_temp >>= MAX6675_DISCARD_BITS;
+      max6675_errors[hindex] = 0;
+    }
+
+    #if MAX6675_0_IS_MAX31855 || MAX6675_1_IS_MAX31855
+      if (max6675_temp & 0x00002000) max6675_temp |= 0xFFFFC000; // Support negative temperature
+    #endif
+
+    // Return the RTD resistance for MAX31865 for display in SHOW_TEMP_ADC_VALUES
+    TERN_(HAS_MAX31865, max6675_temp = max31865_ohms);
+
+    MAX6675_TEMP(hindex) = max6675_temp;
+
+    return int(max6675_temp);
+  }
+
+#endif // HAS_MAX6675
+
+/**
+ * Update raw temperatures
+ */
+void Temperature::update_raw_temperatures() {
+
+  #if HAS_TEMP_ADC_0 && !HEATER_0_USES_MAX6675
+    temp_hotend[0].update();
+  #endif
+
+  #if HAS_TEMP_ADC_1
+    #if ENABLED(TEMP_SENSOR_1_AS_REDUNDANT)
+      redundant_temperature_raw = temp_hotend[1].acc;
+    #elif !HEATER_1_USES_MAX6675
+      temp_hotend[1].update();
+    #endif
+  #endif
+
+  TERN_(HAS_TEMP_ADC_2, temp_hotend[2].update());
+  TERN_(HAS_TEMP_ADC_3, temp_hotend[3].update());
+  TERN_(HAS_TEMP_ADC_4, temp_hotend[4].update());
+  TERN_(HAS_TEMP_ADC_5, temp_hotend[5].update());
+  TERN_(HAS_TEMP_ADC_6, temp_hotend[6].update());
+  TERN_(HAS_TEMP_ADC_7, temp_hotend[7].update());
+  TERN_(HAS_TEMP_ADC_BED, temp_bed.update());
+  TERN_(HAS_TEMP_ADC_CHAMBER, temp_chamber.update());
+  TERN_(HAS_TEMP_ADC_PROBE, temp_probe.update());
+
+  TERN_(HAS_JOY_ADC_X, joystick.x.update());
+  TERN_(HAS_JOY_ADC_Y, joystick.y.update());
+  TERN_(HAS_JOY_ADC_Z, joystick.z.update());
+
+  raw_temps_ready = true;
+}
+
+void Temperature::readings_ready() {
+
+  // Update the raw values if they've been read. Else we could be updating them during reading.
+  if (!raw_temps_ready) update_raw_temperatures();
+
+  // Filament Sensor - can be read any time since IIR filtering is used
+  TERN_(FILAMENT_WIDTH_SENSOR, filwidth.reading_ready());
+
+  #if HAS_HOTEND
+    HOTEND_LOOP() temp_hotend[e].reset();
+    TERN_(TEMP_SENSOR_1_AS_REDUNDANT, temp_hotend[1].reset());
+  #endif
+
+  TERN_(HAS_HEATED_BED, temp_bed.reset());
+  TERN_(HAS_TEMP_CHAMBER, temp_chamber.reset());
+  TERN_(HAS_TEMP_PROBE, temp_probe.reset());
+
+  TERN_(HAS_JOY_ADC_X, joystick.x.reset());
+  TERN_(HAS_JOY_ADC_Y, joystick.y.reset());
+  TERN_(HAS_JOY_ADC_Z, joystick.z.reset());
+
+  #if HAS_HOTEND
+
+    static constexpr int8_t temp_dir[] = {
+      TERN(HEATER_0_USES_MAX6675, 0, TEMPDIR(0))
+      #if HAS_MULTI_HOTEND
+        , TERN(HEATER_1_USES_MAX6675, 0, TEMPDIR(1))
+        #if HOTENDS > 2
+          #define _TEMPDIR(N) , TEMPDIR(N)
+          REPEAT_S(2, HOTENDS, _TEMPDIR)
+        #endif
+      #endif
+    };
+
+    LOOP_L_N(e, COUNT(temp_dir)) {
+      const int8_t tdir = temp_dir[e];
+      if (tdir) {
+        const int16_t rawtemp = temp_hotend[e].raw * tdir; // normal direction, +rawtemp, else -rawtemp
+        const bool heater_on = (temp_hotend[e].target > 0
+          || TERN0(PIDTEMP, temp_hotend[e].soft_pwm_amount) > 0
+        );
+        if (rawtemp > temp_range[e].raw_max * tdir) max_temp_error((heater_id_t)e);
+        if (heater_on && rawtemp < temp_range[e].raw_min * tdir && !is_preheating(e)) {
+          #ifdef MAX_CONSECUTIVE_LOW_TEMPERATURE_ERROR_ALLOWED
+            if (++consecutive_low_temperature_error[e] >= MAX_CONSECUTIVE_LOW_TEMPERATURE_ERROR_ALLOWED)
+          #endif
+              min_temp_error((heater_id_t)e);
+        }
+        #ifdef MAX_CONSECUTIVE_LOW_TEMPERATURE_ERROR_ALLOWED
+          else
+            consecutive_low_temperature_error[e] = 0;
+        #endif
+      }
+    }
+
+  #endif // HAS_HOTEND
+
+  #if ENABLED(THERMAL_PROTECTION_BED)
+    #if TEMPDIR(BED) < 0
+      #define BEDCMP(A,B) ((A)<(B))
+    #else
+      #define BEDCMP(A,B) ((A)>(B))
+    #endif
+    const bool bed_on = (temp_bed.target > 0) || TERN0(PIDTEMPBED, temp_bed.soft_pwm_amount > 0);
+    if (BEDCMP(temp_bed.raw, maxtemp_raw_BED)) max_temp_error(H_BED);
+    if (bed_on && BEDCMP(mintemp_raw_BED, temp_bed.raw)) min_temp_error(H_BED);
+  #endif
+
+  #if BOTH(HAS_HEATED_CHAMBER, THERMAL_PROTECTION_CHAMBER)
+    #if TEMPDIR(CHAMBER) < 0
+      #define CHAMBERCMP(A,B) ((A)<(B))
+    #else
+      #define CHAMBERCMP(A,B) ((A)>(B))
+    #endif
+    const bool chamber_on = (temp_chamber.target > 0);
+    if (CHAMBERCMP(temp_chamber.raw, maxtemp_raw_CHAMBER)) max_temp_error(H_CHAMBER);
+    if (chamber_on && CHAMBERCMP(mintemp_raw_CHAMBER, temp_chamber.raw)) min_temp_error(H_CHAMBER);
+  #endif
+}
+
+/**
+ * Timer 0 is shared with millies so don't change the prescaler.
+ *
+ * On AVR this ISR uses the compare method so it runs at the base
+ * frequency (16 MHz / 64 / 256 = 976.5625 Hz), but at the TCNT0 set
+ * in OCR0B above (128 or halfway between OVFs).
+ *
+ *  - Manage PWM to all the heaters and fan
+ *  - Prepare or Measure one of the raw ADC sensor values
+ *  - Check new temperature values for MIN/MAX errors (kill on error)
+ *  - Step the babysteps value for each axis towards 0
+ *  - For PINS_DEBUGGING, monitor and report endstop pins
+ *  - For ENDSTOP_INTERRUPTS_FEATURE check endstops if flagged
+ *  - Call planner.tick to count down its "ignore" time
+ */
+HAL_TEMP_TIMER_ISR() {
+  HAL_timer_isr_prologue(TEMP_TIMER_NUM);
+
+  Temperature::tick();
+
+  HAL_timer_isr_epilogue(TEMP_TIMER_NUM);
+}
+
+#if ENABLED(SLOW_PWM_HEATERS) && !defined(MIN_STATE_TIME)
+  #define MIN_STATE_TIME 16 // MIN_STATE_TIME * 65.5 = time in milliseconds
+#endif
+
+class SoftPWM {
+public:
+  uint8_t count;
+  inline bool add(const uint8_t mask, const uint8_t amount) {
+    count = (count & mask) + amount; return (count > mask);
+  }
+  #if ENABLED(SLOW_PWM_HEATERS)
+    bool state_heater;
+    uint8_t state_timer_heater;
+    inline void dec() { if (state_timer_heater > 0) state_timer_heater--; }
+    inline bool ready(const bool v) {
+      const bool rdy = !state_timer_heater;
+      if (rdy && state_heater != v) {
+        state_heater = v;
+        state_timer_heater = MIN_STATE_TIME;
+      }
+      return rdy;
+    }
+  #endif
+};
+
+/**
+ * Handle various ~1KHz tasks associated with temperature
+ *  - Heater PWM (~1KHz with scaler)
+ *  - LCD Button polling (~500Hz)
+ *  - Start / Read one ADC sensor
+ *  - Advance Babysteps
+ *  - Endstop polling
+ *  - Planner clean buffer
+ */
+void Temperature::tick() {
+
+  static int8_t temp_count = -1;
+  static ADCSensorState adc_sensor_state = StartupDelay;
+  static uint8_t pwm_count = _BV(SOFT_PWM_SCALE);
+
+  // avoid multiple loads of pwm_count
+  uint8_t pwm_count_tmp = pwm_count;
+
+  #if HAS_ADC_BUTTONS
+    static unsigned int raw_ADCKey_value = 0;
+    static bool ADCKey_pressed = false;
+  #endif
+
+  #if HAS_HOTEND
+    static SoftPWM soft_pwm_hotend[HOTENDS];
+  #endif
+
+  #if HAS_HEATED_BED
+    static SoftPWM soft_pwm_bed;
+  #endif
+
+  #if HAS_HEATED_CHAMBER
+    static SoftPWM soft_pwm_chamber;
+  #endif
+
+  #define WRITE_FAN(n, v) WRITE(FAN##n##_PIN, (v) ^ FAN_INVERTING)
+
+  #if DISABLED(SLOW_PWM_HEATERS)
+
+    #if ANY(HAS_HOTEND, HAS_HEATED_BED, HAS_HEATED_CHAMBER, FAN_SOFT_PWM)
+      constexpr uint8_t pwm_mask = TERN0(SOFT_PWM_DITHER, _BV(SOFT_PWM_SCALE) - 1);
+      #define _PWM_MOD(N,S,T) do{                           \
+        const bool on = S.add(pwm_mask, T.soft_pwm_amount); \
+        WRITE_HEATER_##N(on);                               \
+      }while(0)
+    #endif
+
+    /**
+     * Standard heater PWM modulation
+     */
+    if (pwm_count_tmp >= 127) {
+      pwm_count_tmp -= 127;
+
+      #if HAS_HOTEND
+        #define _PWM_MOD_E(N) _PWM_MOD(N,soft_pwm_hotend[N],temp_hotend[N]);
+        REPEAT(HOTENDS, _PWM_MOD_E);
+      #endif
+
+      #if HAS_HEATED_BED
+        _PWM_MOD(BED,soft_pwm_bed,temp_bed);
+      #endif
+
+      #if HAS_HEATED_CHAMBER
+        _PWM_MOD(CHAMBER,soft_pwm_chamber,temp_chamber);
+      #endif
+
+      #if ENABLED(FAN_SOFT_PWM)
+        #define _FAN_PWM(N) do{                                     \
+          uint8_t &spcf = soft_pwm_count_fan[N];                    \
+          spcf = (spcf & pwm_mask) + (soft_pwm_amount_fan[N] >> 1); \
+          WRITE_FAN(N, spcf > pwm_mask ? HIGH : LOW);               \
+        }while(0)
+        #if HAS_FAN0
+          _FAN_PWM(0);
+        #endif
+        #if HAS_FAN1
+          _FAN_PWM(1);
+        #endif
+        #if HAS_FAN2
+          _FAN_PWM(2);
+        #endif
+        #if HAS_FAN3
+          _FAN_PWM(3);
+        #endif
+        #if HAS_FAN4
+          _FAN_PWM(4);
+        #endif
+        #if HAS_FAN5
+          _FAN_PWM(5);
+        #endif
+        #if HAS_FAN6
+          _FAN_PWM(6);
+        #endif
+        #if HAS_FAN7
+          _FAN_PWM(7);
+        #endif
+      #endif
+    }
+    else {
+      #define _PWM_LOW(N,S) do{ if (S.count <= pwm_count_tmp) WRITE_HEATER_##N(LOW); }while(0)
+      #if HAS_HOTEND
+        #define _PWM_LOW_E(N) _PWM_LOW(N, soft_pwm_hotend[N]);
+        REPEAT(HOTENDS, _PWM_LOW_E);
+      #endif
+
+      #if HAS_HEATED_BED
+        _PWM_LOW(BED, soft_pwm_bed);
+      #endif
+
+      #if HAS_HEATED_CHAMBER
+        _PWM_LOW(CHAMBER, soft_pwm_chamber);
+      #endif
+
+      #if ENABLED(FAN_SOFT_PWM)
+        #if HAS_FAN0
+          if (soft_pwm_count_fan[0] <= pwm_count_tmp) WRITE_FAN(0, LOW);
+        #endif
+        #if HAS_FAN1
+          if (soft_pwm_count_fan[1] <= pwm_count_tmp) WRITE_FAN(1, LOW);
+        #endif
+        #if HAS_FAN2
+          if (soft_pwm_count_fan[2] <= pwm_count_tmp) WRITE_FAN(2, LOW);
+        #endif
+        #if HAS_FAN3
+          if (soft_pwm_count_fan[3] <= pwm_count_tmp) WRITE_FAN(3, LOW);
+        #endif
+        #if HAS_FAN4
+          if (soft_pwm_count_fan[4] <= pwm_count_tmp) WRITE_FAN(4, LOW);
+        #endif
+        #if HAS_FAN5
+          if (soft_pwm_count_fan[5] <= pwm_count_tmp) WRITE_FAN(5, LOW);
+        #endif
+        #if HAS_FAN6
+          if (soft_pwm_count_fan[6] <= pwm_count_tmp) WRITE_FAN(6, LOW);
+        #endif
+        #if HAS_FAN7
+          if (soft_pwm_count_fan[7] <= pwm_count_tmp) WRITE_FAN(7, LOW);
+        #endif
+      #endif
+    }
+
+    // SOFT_PWM_SCALE to frequency:
+    //
+    // 0: 16000000/64/256/128 =   7.6294 Hz
+    // 1:                / 64 =  15.2588 Hz
+    // 2:                / 32 =  30.5176 Hz
+    // 3:                / 16 =  61.0352 Hz
+    // 4:                /  8 = 122.0703 Hz
+    // 5:                /  4 = 244.1406 Hz
+    pwm_count = pwm_count_tmp + _BV(SOFT_PWM_SCALE);
+
+  #else // SLOW_PWM_HEATERS
+
+    /**
+     * SLOW PWM HEATERS
+     *
+     * For relay-driven heaters
+     */
+    #define _SLOW_SET(NR,PWM,V) do{ if (PWM.ready(V)) WRITE_HEATER_##NR(V); }while(0)
+    #define _SLOW_PWM(NR,PWM,SRC) do{ PWM.count = SRC.soft_pwm_amount; _SLOW_SET(NR,PWM,(PWM.count > 0)); }while(0)
+    #define _PWM_OFF(NR,PWM) do{ if (PWM.count < slow_pwm_count) _SLOW_SET(NR,PWM,0); }while(0)
+
+    static uint8_t slow_pwm_count = 0;
+
+    if (slow_pwm_count == 0) {
+
+      #if HAS_HOTEND
+        #define _SLOW_PWM_E(N) _SLOW_PWM(N, soft_pwm_hotend[N], temp_hotend[N]);
+        REPEAT(HOTENDS, _SLOW_PWM_E);
+      #endif
+
+      #if HAS_HEATED_BED
+        _SLOW_PWM(BED, soft_pwm_bed, temp_bed);
+      #endif
+
+      #if HAS_HEATED_CHAMBER
+        _SLOW_PWM(CHAMBER, soft_pwm_chamber, temp_chamber);
+      #endif
+
+    } // slow_pwm_count == 0
+
+    #if HAS_HOTEND
+      #define _PWM_OFF_E(N) _PWM_OFF(N, soft_pwm_hotend[N]);
+      REPEAT(HOTENDS, _PWM_OFF_E);
+    #endif
+
+    #if HAS_HEATED_BED
+      _PWM_OFF(BED, soft_pwm_bed);
+    #endif
+
+    #if HAS_HEATED_CHAMBER
+      _PWM_OFF(CHAMBER, soft_pwm_chamber);
+    #endif
+
+    #if ENABLED(FAN_SOFT_PWM)
+      if (pwm_count_tmp >= 127) {
+        pwm_count_tmp = 0;
+        #define _PWM_FAN(N) do{                                 \
+          soft_pwm_count_fan[N] = soft_pwm_amount_fan[N] >> 1;  \
+          WRITE_FAN(N, soft_pwm_count_fan[N] > 0 ? HIGH : LOW); \
+        }while(0)
+        #if HAS_FAN0
+          _PWM_FAN(0);
+        #endif
+        #if HAS_FAN1
+          _PWM_FAN(1);
+        #endif
+        #if HAS_FAN2
+          _PWM_FAN(2);
+        #endif
+        #if HAS_FAN3
+          _FAN_PWM(3);
+        #endif
+        #if HAS_FAN4
+          _FAN_PWM(4);
+        #endif
+        #if HAS_FAN5
+          _FAN_PWM(5);
+        #endif
+        #if HAS_FAN6
+          _FAN_PWM(6);
+        #endif
+        #if HAS_FAN7
+          _FAN_PWM(7);
+        #endif
+      }
+      #if HAS_FAN0
+        if (soft_pwm_count_fan[0] <= pwm_count_tmp) WRITE_FAN(0, LOW);
+      #endif
+      #if HAS_FAN1
+        if (soft_pwm_count_fan[1] <= pwm_count_tmp) WRITE_FAN(1, LOW);
+      #endif
+      #if HAS_FAN2
+        if (soft_pwm_count_fan[2] <= pwm_count_tmp) WRITE_FAN(2, LOW);
+      #endif
+      #if HAS_FAN3
+        if (soft_pwm_count_fan[3] <= pwm_count_tmp) WRITE_FAN(3, LOW);
+      #endif
+      #if HAS_FAN4
+        if (soft_pwm_count_fan[4] <= pwm_count_tmp) WRITE_FAN(4, LOW);
+      #endif
+      #if HAS_FAN5
+        if (soft_pwm_count_fan[5] <= pwm_count_tmp) WRITE_FAN(5, LOW);
+      #endif
+      #if HAS_FAN6
+        if (soft_pwm_count_fan[6] <= pwm_count_tmp) WRITE_FAN(6, LOW);
+      #endif
+      #if HAS_FAN7
+        if (soft_pwm_count_fan[7] <= pwm_count_tmp) WRITE_FAN(7, LOW);
+      #endif
+    #endif // FAN_SOFT_PWM
+
+    // SOFT_PWM_SCALE to frequency:
+    //
+    // 0: 16000000/64/256/128 =   7.6294 Hz
+    // 1:                / 64 =  15.2588 Hz
+    // 2:                / 32 =  30.5176 Hz
+    // 3:                / 16 =  61.0352 Hz
+    // 4:                /  8 = 122.0703 Hz
+    // 5:                /  4 = 244.1406 Hz
+    pwm_count = pwm_count_tmp + _BV(SOFT_PWM_SCALE);
+
+    // increment slow_pwm_count only every 64th pwm_count,
+    // i.e. yielding a PWM frequency of 16/128 Hz (8s).
+    if (((pwm_count >> SOFT_PWM_SCALE) & 0x3F) == 0) {
+      slow_pwm_count++;
+      slow_pwm_count &= 0x7F;
+
+      #if HAS_HOTEND
+        HOTEND_LOOP() soft_pwm_hotend[e].dec();
+      #endif
+      TERN_(HAS_HEATED_BED, soft_pwm_bed.dec());
+      TERN_(HAS_HEATED_CHAMBER, soft_pwm_chamber.dec());
+    }
+
+  #endif // SLOW_PWM_HEATERS
+
+  //
+  // Update lcd buttons 488 times per second
+  //
+  static bool do_buttons;
+  if ((do_buttons ^= true)) ui.update_buttons();
+
+  /**
+   * One sensor is sampled on every other call of the ISR.
+   * Each sensor is read 16 (OVERSAMPLENR) times, taking the average.
+   *
+   * On each Prepare pass, ADC is started for a sensor pin.
+   * On the next pass, the ADC value is read and accumulated.
+   *
+   * This gives each ADC 0.9765ms to charge up.
+   */
+  #define ACCUMULATE_ADC(obj) do{ \
+    if (!HAL_ADC_READY()) next_sensor_state = adc_sensor_state; \
+    else obj.sample(HAL_READ_ADC()); \
+  }while(0)
+
+  ADCSensorState next_sensor_state = adc_sensor_state < SensorsReady ? (ADCSensorState)(int(adc_sensor_state) + 1) : StartSampling;
+
+  switch (adc_sensor_state) {
+
+    case SensorsReady: {
+      // All sensors have been read. Stay in this state for a few
+      // ISRs to save on calls to temp update/checking code below.
+      constexpr int8_t extra_loops = MIN_ADC_ISR_LOOPS - (int8_t)SensorsReady;
+      static uint8_t delay_count = 0;
+      if (extra_loops > 0) {
+        if (delay_count == 0) delay_count = extra_loops;  // Init this delay
+        if (--delay_count)                                // While delaying...
+          next_sensor_state = SensorsReady;               // retain this state (else, next state will be 0)
+        break;
+      }
+      else {
+        adc_sensor_state = StartSampling;                 // Fall-through to start sampling
+        next_sensor_state = (ADCSensorState)(int(StartSampling) + 1);
+      }
+    }
+
+    case StartSampling:                                   // Start of sampling loops. Do updates/checks.
+      if (++temp_count >= OVERSAMPLENR) {                 // 10 * 16 * 1/(16000000/64/256)  = 164ms.
+        temp_count = 0;
+        readings_ready();
+      }
+      break;
+
+    #if HAS_TEMP_ADC_0
+      case PrepareTemp_0: HAL_START_ADC(TEMP_0_PIN); break;
+      case MeasureTemp_0: ACCUMULATE_ADC(temp_hotend[0]); break;
+    #endif
+
+    #if HAS_TEMP_ADC_BED
+      case PrepareTemp_BED: HAL_START_ADC(TEMP_BED_PIN); break;
+      case MeasureTemp_BED: ACCUMULATE_ADC(temp_bed); break;
+    #endif
+
+    #if HAS_TEMP_ADC_CHAMBER
+      case PrepareTemp_CHAMBER: HAL_START_ADC(TEMP_CHAMBER_PIN); break;
+      case MeasureTemp_CHAMBER: ACCUMULATE_ADC(temp_chamber); break;
+    #endif
+
+    #if HAS_TEMP_ADC_PROBE
+      case PrepareTemp_PROBE: HAL_START_ADC(TEMP_PROBE_PIN); break;
+      case MeasureTemp_PROBE: ACCUMULATE_ADC(temp_probe); break;
+    #endif
+
+    #if HAS_TEMP_ADC_1
+      case PrepareTemp_1: HAL_START_ADC(TEMP_1_PIN); break;
+      case MeasureTemp_1: ACCUMULATE_ADC(temp_hotend[1]); break;
+    #endif
+
+    #if HAS_TEMP_ADC_2
+      case PrepareTemp_2: HAL_START_ADC(TEMP_2_PIN); break;
+      case MeasureTemp_2: ACCUMULATE_ADC(temp_hotend[2]); break;
+    #endif
+
+    #if HAS_TEMP_ADC_3
+      case PrepareTemp_3: HAL_START_ADC(TEMP_3_PIN); break;
+      case MeasureTemp_3: ACCUMULATE_ADC(temp_hotend[3]); break;
+    #endif
+
+    #if HAS_TEMP_ADC_4
+      case PrepareTemp_4: HAL_START_ADC(TEMP_4_PIN); break;
+      case MeasureTemp_4: ACCUMULATE_ADC(temp_hotend[4]); break;
+    #endif
+
+    #if HAS_TEMP_ADC_5
+      case PrepareTemp_5: HAL_START_ADC(TEMP_5_PIN); break;
+      case MeasureTemp_5: ACCUMULATE_ADC(temp_hotend[5]); break;
+    #endif
+
+    #if HAS_TEMP_ADC_6
+      case PrepareTemp_6: HAL_START_ADC(TEMP_6_PIN); break;
+      case MeasureTemp_6: ACCUMULATE_ADC(temp_hotend[6]); break;
+    #endif
+
+    #if HAS_TEMP_ADC_7
+      case PrepareTemp_7: HAL_START_ADC(TEMP_7_PIN); break;
+      case MeasureTemp_7: ACCUMULATE_ADC(temp_hotend[7]); break;
+    #endif
+
+    #if ENABLED(FILAMENT_WIDTH_SENSOR)
+      case Prepare_FILWIDTH: HAL_START_ADC(FILWIDTH_PIN); break;
+      case Measure_FILWIDTH:
+        if (!HAL_ADC_READY()) next_sensor_state = adc_sensor_state; // Redo this state
+        else filwidth.accumulate(HAL_READ_ADC());
+      break;
+    #endif
+
+    #if ENABLED(POWER_MONITOR_CURRENT)
+      case Prepare_POWER_MONITOR_CURRENT:
+        HAL_START_ADC(POWER_MONITOR_CURRENT_PIN);
+        break;
+      case Measure_POWER_MONITOR_CURRENT:
+        if (!HAL_ADC_READY()) next_sensor_state = adc_sensor_state; // Redo this state
+        else power_monitor.add_current_sample(HAL_READ_ADC());
+        break;
+    #endif
+
+    #if ENABLED(POWER_MONITOR_VOLTAGE)
+      case Prepare_POWER_MONITOR_VOLTAGE:
+        HAL_START_ADC(POWER_MONITOR_VOLTAGE_PIN);
+        break;
+      case Measure_POWER_MONITOR_VOLTAGE:
+        if (!HAL_ADC_READY()) next_sensor_state = adc_sensor_state; // Redo this state
+        else power_monitor.add_voltage_sample(HAL_READ_ADC());
+        break;
+    #endif
+
+    #if HAS_JOY_ADC_X
+      case PrepareJoy_X: HAL_START_ADC(JOY_X_PIN); break;
+      case MeasureJoy_X: ACCUMULATE_ADC(joystick.x); break;
+    #endif
+
+    #if HAS_JOY_ADC_Y
+      case PrepareJoy_Y: HAL_START_ADC(JOY_Y_PIN); break;
+      case MeasureJoy_Y: ACCUMULATE_ADC(joystick.y); break;
+    #endif
+
+    #if HAS_JOY_ADC_Z
+      case PrepareJoy_Z: HAL_START_ADC(JOY_Z_PIN); break;
+      case MeasureJoy_Z: ACCUMULATE_ADC(joystick.z); break;
+    #endif
+
+    #if HAS_ADC_BUTTONS
+      #ifndef ADC_BUTTON_DEBOUNCE_DELAY
+        #define ADC_BUTTON_DEBOUNCE_DELAY 16
+      #endif
+      case Prepare_ADC_KEY: HAL_START_ADC(ADC_KEYPAD_PIN); break;
+      case Measure_ADC_KEY:
+        if (!HAL_ADC_READY())
+          next_sensor_state = adc_sensor_state; // redo this state
+        else if (ADCKey_count < ADC_BUTTON_DEBOUNCE_DELAY) {
+          raw_ADCKey_value = HAL_READ_ADC();
+          if (raw_ADCKey_value <= 900UL * HAL_ADC_RANGE / 1024UL) {
+            NOMORE(current_ADCKey_raw, raw_ADCKey_value);
+            ADCKey_count++;
+          }
+          else { //ADC Key release
+            if (ADCKey_count > 0) ADCKey_count++; else ADCKey_pressed = false;
+            if (ADCKey_pressed) {
+              ADCKey_count = 0;
+              current_ADCKey_raw = HAL_ADC_RANGE;
+            }
+          }
+        }
+        if (ADCKey_count == ADC_BUTTON_DEBOUNCE_DELAY) ADCKey_pressed = true;
+        break;
+    #endif // HAS_ADC_BUTTONS
+
+    case StartupDelay: break;
+
+  } // switch(adc_sensor_state)
+
+  // Go to the next state
+  adc_sensor_state = next_sensor_state;
+
+  //
+  // Additional ~1KHz Tasks
+  //
+
+  #if ENABLED(BABYSTEPPING) && DISABLED(INTEGRATED_BABYSTEPPING)
+    babystep.task();
+  #endif
+
+  // Poll endstops state, if required
+  endstops.poll();
+
+  // Periodically call the planner timer
+  planner.tick();
+}
+
+#if HAS_TEMP_SENSOR
+
+  #include "../gcode/gcode.h"
+
+  static void print_heater_state(const float &c, const float &t
+    #if ENABLED(SHOW_TEMP_ADC_VALUES)
+      , const float r
+    #endif
+    , const heater_id_t e=INDEX_NONE
+  ) {
+    char k;
+    switch (e) {
+      #if HAS_TEMP_CHAMBER
+        case H_CHAMBER: k = 'C'; break;
+      #endif
+      #if HAS_TEMP_PROBE
+        case H_PROBE: k = 'P'; break;
+      #endif
+      #if HAS_TEMP_HOTEND
+        default: k = 'T'; break;
+        #if HAS_HEATED_BED
+          case H_BED: k = 'B'; break;
+        #endif
+        #if ENABLED(TEMP_SENSOR_1_AS_REDUNDANT)
+          case H_REDUNDANT: k = 'R'; break;
+        #endif
+      #elif HAS_HEATED_BED
+        default: k = 'B'; break;
+      #endif
+    }
+    SERIAL_CHAR(' ', k);
+    #if HAS_MULTI_HOTEND
+      if (e >= 0) SERIAL_CHAR('0' + e);
+    #endif
+    #ifdef SERIAL_FLOAT_PRECISION
+      #define SFP _MIN(SERIAL_FLOAT_PRECISION, 2)
+    #else
+      #define SFP 2
+    #endif
+    SERIAL_CHAR(':');
+    SERIAL_PRINT(c, SFP);
+    SERIAL_ECHOPGM(" /");
+    SERIAL_PRINT(t, SFP);
+    #if ENABLED(SHOW_TEMP_ADC_VALUES)
+      SERIAL_ECHOPAIR(" (", r * RECIPROCAL(OVERSAMPLENR));
+      SERIAL_CHAR(')');
+    #endif
+    delay(2);
+  }
+
+  void Temperature::print_heater_states(const uint8_t target_extruder
+    #if ENABLED(TEMP_SENSOR_1_AS_REDUNDANT)
+      , const bool include_r/*=false*/
+    #endif
+  ) {
+    #if HAS_TEMP_HOTEND
+      print_heater_state(degHotend(target_extruder), degTargetHotend(target_extruder)
+        #if ENABLED(SHOW_TEMP_ADC_VALUES)
+          , rawHotendTemp(target_extruder)
+        #endif
+      );
+      #if ENABLED(TEMP_SENSOR_1_AS_REDUNDANT)
+        if (include_r) print_heater_state(redundant_temperature, degTargetHotend(target_extruder)
+          #if ENABLED(SHOW_TEMP_ADC_VALUES)
+            , redundant_temperature_raw
+          #endif
+          , H_REDUNDANT
+        );
+      #endif
+    #endif
+    #if HAS_HEATED_BED
+      print_heater_state(degBed(), degTargetBed()
+        #if ENABLED(SHOW_TEMP_ADC_VALUES)
+          , rawBedTemp()
+        #endif
+        , H_BED
+      );
+    #endif
+    #if HAS_TEMP_CHAMBER
+      print_heater_state(degChamber()
+        #if HAS_HEATED_CHAMBER
+          , degTargetChamber()
+        #else
+          , 0
+        #endif
+        #if ENABLED(SHOW_TEMP_ADC_VALUES)
+          , rawChamberTemp()
+        #endif
+        , H_CHAMBER
+      );
+    #endif // HAS_TEMP_CHAMBER
+    #if HAS_TEMP_PROBE
+      print_heater_state(degProbe(), 0
+        #if ENABLED(SHOW_TEMP_ADC_VALUES)
+          , rawProbeTemp()
+        #endif
+        , H_PROBE
+      );
+    #endif // HAS_TEMP_PROBE
+    #if HAS_MULTI_HOTEND
+      HOTEND_LOOP() print_heater_state(degHotend(e), degTargetHotend(e)
+        #if ENABLED(SHOW_TEMP_ADC_VALUES)
+          , rawHotendTemp(e)
+        #endif
+        , (heater_id_t)e
+      );
+    #endif
+    SERIAL_ECHOPAIR(" @:", getHeaterPower((heater_id_t)target_extruder));
+    #if HAS_HEATED_BED
+      SERIAL_ECHOPAIR(" B@:", getHeaterPower(H_BED));
+    #endif
+    #if HAS_HEATED_CHAMBER
+      SERIAL_ECHOPAIR(" C@:", getHeaterPower(H_CHAMBER));
+    #endif
+    #if HAS_MULTI_HOTEND
+      HOTEND_LOOP() {
+        SERIAL_ECHOPAIR(" @", e);
+        SERIAL_CHAR(':');
+        SERIAL_ECHO(getHeaterPower((heater_id_t)e));
+      }
+    #endif
+  }
+
+  #if ENABLED(AUTO_REPORT_TEMPERATURES)
+    AutoReporter<Temperature::AutoReportTemp> Temperature::auto_reporter;
+    void Temperature::AutoReportTemp::report() {
+      print_heater_states(active_extruder);
+      SERIAL_EOL();
+    }
+  #endif
+
+  #if HAS_HOTEND && HAS_DISPLAY
+    void Temperature::set_heating_message(const uint8_t e) {
+      const bool heating = isHeatingHotend(e);
+      ui.status_printf_P(0,
+        #if HAS_MULTI_HOTEND
+          PSTR("E%c " S_FMT), '1' + e
+        #else
+          PSTR("E " S_FMT)
+        #endif
+        , heating ? GET_TEXT(MSG_HEATING) : GET_TEXT(MSG_COOLING)
+      );
+    }
+  #endif
+
+  #if HAS_TEMP_HOTEND
+
+    #ifndef MIN_COOLING_SLOPE_DEG
+      #define MIN_COOLING_SLOPE_DEG 1.50
+    #endif
+    #ifndef MIN_COOLING_SLOPE_TIME
+      #define MIN_COOLING_SLOPE_TIME 60
+    #endif
+
+    bool Temperature::wait_for_hotend(const uint8_t target_extruder, const bool no_wait_for_cooling/*=true*/
+      #if G26_CLICK_CAN_CANCEL
+        , const bool click_to_cancel/*=false*/
+      #endif
+    ) {
+
+      #if ENABLED(AUTOTEMP)
+        REMEMBER(1, planner.autotemp_enabled, false);
+      #endif
+
+      #if TEMP_RESIDENCY_TIME > 0
+        millis_t residency_start_ms = 0;
+        bool first_loop = true;
+        // Loop until the temperature has stabilized
+        #define TEMP_CONDITIONS (!residency_start_ms || PENDING(now, residency_start_ms + SEC_TO_MS(TEMP_RESIDENCY_TIME)))
+      #else
+        // Loop until the temperature is very close target
+        #define TEMP_CONDITIONS (wants_to_cool ? isCoolingHotend(target_extruder) : isHeatingHotend(target_extruder))
+      #endif
+
+      #if DISABLED(BUSY_WHILE_HEATING) && ENABLED(HOST_KEEPALIVE_FEATURE)
+        KEEPALIVE_STATE(NOT_BUSY);
+      #endif
+
+      #if ENABLED(PRINTER_EVENT_LEDS)
+        const float start_temp = degHotend(target_extruder);
+        printerEventLEDs.onHotendHeatingStart();
+      #endif
+
+      bool wants_to_cool = false;
+      float target_temp = -1.0, old_temp = 9999.0;
+      millis_t now, next_temp_ms = 0, next_cool_check_ms = 0;
+      wait_for_heatup = true;
+      do {
+        // Target temperature might be changed during the loop
+        if (target_temp != degTargetHotend(target_extruder)) {
+          wants_to_cool = isCoolingHotend(target_extruder);
+          target_temp = degTargetHotend(target_extruder);
+
+          // Exit if S<lower>, continue if S<higher>, R<lower>, or R<higher>
+          if (no_wait_for_cooling && wants_to_cool) break;
+        }
+
+        now = millis();
+        if (ELAPSED(now, next_temp_ms)) { // Print temp & remaining time every 1s while waiting
+          next_temp_ms = now + 1000UL;
+          print_heater_states(target_extruder);
+          #if TEMP_RESIDENCY_TIME > 0
+            SERIAL_ECHOPGM(" W:");
+            if (residency_start_ms)
+              SERIAL_ECHO(long((SEC_TO_MS(TEMP_RESIDENCY_TIME) - (now - residency_start_ms)) / 1000UL));
+            else
+              SERIAL_CHAR('?');
+          #endif
+          SERIAL_EOL();
+        }
+
+        idle();
+        gcode.reset_stepper_timeout(); // Keep steppers powered
+
+        const float temp = degHotend(target_extruder);
+
+        #if ENABLED(PRINTER_EVENT_LEDS)
+          // Gradually change LED strip from violet to red as nozzle heats up
+          if (!wants_to_cool) printerEventLEDs.onHotendHeating(start_temp, temp, target_temp);
+        #endif
+
+        #if TEMP_RESIDENCY_TIME > 0
+
+          const float temp_diff = ABS(target_temp - temp);
+
+          if (!residency_start_ms) {
+            // Start the TEMP_RESIDENCY_TIME timer when we reach target temp for the first time.
+            if (temp_diff < TEMP_WINDOW)
+              residency_start_ms = now + (first_loop ? SEC_TO_MS(TEMP_RESIDENCY_TIME) / 3 : 0);
+          }
+          else if (temp_diff > TEMP_HYSTERESIS) {
+            // Restart the timer whenever the temperature falls outside the hysteresis.
+            residency_start_ms = now;
+          }
+
+          first_loop = false;
+
+        #endif
+
+        // Prevent a wait-forever situation if R is misused i.e. M109 R0
+        if (wants_to_cool) {
+          // break after MIN_COOLING_SLOPE_TIME seconds
+          // if the temperature did not drop at least MIN_COOLING_SLOPE_DEG
+          if (!next_cool_check_ms || ELAPSED(now, next_cool_check_ms)) {
+            if (old_temp - temp < float(MIN_COOLING_SLOPE_DEG)) break;
+            next_cool_check_ms = now + SEC_TO_MS(MIN_COOLING_SLOPE_TIME);
+            old_temp = temp;
+          }
+        }
+
+        #if G26_CLICK_CAN_CANCEL
+          if (click_to_cancel && ui.use_click()) {
+            wait_for_heatup = false;
+            ui.quick_feedback();
+          }
+        #endif
+
+      } while (wait_for_heatup && TEMP_CONDITIONS);
+
+      if (wait_for_heatup) {
+        wait_for_heatup = false;
+        #if ENABLED(DWIN_CREALITY_LCD)
+          HMI_flag.heat_flag = 0;
+          duration_t elapsed = print_job_timer.duration();  // print timer
+          dwin_heat_time = elapsed.value;
+        #else
+          ui.reset_status();
+        #endif
+        TERN_(PRINTER_EVENT_LEDS, printerEventLEDs.onHeatingDone());
+        return true;
+      }
+
+      return false;
+    }
+
+  #endif // HAS_TEMP_HOTEND
+
+  #if HAS_HEATED_BED
+
+    #ifndef MIN_COOLING_SLOPE_DEG_BED
+      #define MIN_COOLING_SLOPE_DEG_BED 1.00
+    #endif
+    #ifndef MIN_COOLING_SLOPE_TIME_BED
+      #define MIN_COOLING_SLOPE_TIME_BED 60
+    #endif
+
+    bool Temperature::wait_for_bed(const bool no_wait_for_cooling/*=true*/
+      #if G26_CLICK_CAN_CANCEL
+        , const bool click_to_cancel/*=false*/
+      #endif
+    ) {
+      #if TEMP_BED_RESIDENCY_TIME > 0
+        millis_t residency_start_ms = 0;
+        bool first_loop = true;
+        // Loop until the temperature has stabilized
+        #define TEMP_BED_CONDITIONS (!residency_start_ms || PENDING(now, residency_start_ms + SEC_TO_MS(TEMP_BED_RESIDENCY_TIME)))
+      #else
+        // Loop until the temperature is very close target
+        #define TEMP_BED_CONDITIONS (wants_to_cool ? isCoolingBed() : isHeatingBed())
+      #endif
+
+      #if DISABLED(BUSY_WHILE_HEATING) && ENABLED(HOST_KEEPALIVE_FEATURE)
+        KEEPALIVE_STATE(NOT_BUSY);
+      #endif
+
+      #if ENABLED(PRINTER_EVENT_LEDS)
+        const float start_temp = degBed();
+        printerEventLEDs.onBedHeatingStart();
+      #endif
+
+      bool wants_to_cool = false;
+      float target_temp = -1, old_temp = 9999;
+      millis_t now, next_temp_ms = 0, next_cool_check_ms = 0;
+      wait_for_heatup = true;
+      do {
+        // Target temperature might be changed during the loop
+        if (target_temp != degTargetBed()) {
+          wants_to_cool = isCoolingBed();
+          target_temp = degTargetBed();
+
+          // Exit if S<lower>, continue if S<higher>, R<lower>, or R<higher>
+          if (no_wait_for_cooling && wants_to_cool) break;
+        }
+
+        now = millis();
+        if (ELAPSED(now, next_temp_ms)) { //Print Temp Reading every 1 second while heating up.
+          next_temp_ms = now + 1000UL;
+          print_heater_states(active_extruder);
+          #if TEMP_BED_RESIDENCY_TIME > 0
+            SERIAL_ECHOPGM(" W:");
+            if (residency_start_ms)
+              SERIAL_ECHO(long((SEC_TO_MS(TEMP_BED_RESIDENCY_TIME) - (now - residency_start_ms)) / 1000UL));
+            else
+              SERIAL_CHAR('?');
+          #endif
+          SERIAL_EOL();
+        }
+
+        idle();
+        gcode.reset_stepper_timeout(); // Keep steppers powered
+
+        const float temp = degBed();
+
+        #if ENABLED(PRINTER_EVENT_LEDS)
+          // Gradually change LED strip from blue to violet as bed heats up
+          if (!wants_to_cool) printerEventLEDs.onBedHeating(start_temp, temp, target_temp);
+        #endif
+
+        #if TEMP_BED_RESIDENCY_TIME > 0
+
+          const float temp_diff = ABS(target_temp - temp);
+
+          if (!residency_start_ms) {
+            // Start the TEMP_BED_RESIDENCY_TIME timer when we reach target temp for the first time.
+            if (temp_diff < TEMP_BED_WINDOW)
+              residency_start_ms = now + (first_loop ? SEC_TO_MS(TEMP_BED_RESIDENCY_TIME) / 3 : 0);
+          }
+          else if (temp_diff > TEMP_BED_HYSTERESIS) {
+            // Restart the timer whenever the temperature falls outside the hysteresis.
+            residency_start_ms = now;
+          }
+
+        #endif // TEMP_BED_RESIDENCY_TIME > 0
+
+        // Prevent a wait-forever situation if R is misused i.e. M190 R0
+        if (wants_to_cool) {
+          // Break after MIN_COOLING_SLOPE_TIME_BED seconds
+          // if the temperature did not drop at least MIN_COOLING_SLOPE_DEG_BED
+          if (!next_cool_check_ms || ELAPSED(now, next_cool_check_ms)) {
+            if (old_temp - temp < float(MIN_COOLING_SLOPE_DEG_BED)) break;
+            next_cool_check_ms = now + SEC_TO_MS(MIN_COOLING_SLOPE_TIME_BED);
+            old_temp = temp;
+          }
+        }
+
+        #if G26_CLICK_CAN_CANCEL
+          if (click_to_cancel && ui.use_click()) {
+            wait_for_heatup = false;
+            ui.quick_feedback();
+          }
+        #endif
+
+        #if TEMP_BED_RESIDENCY_TIME > 0
+          first_loop = false;
+        #endif
+
+      } while (wait_for_heatup && TEMP_BED_CONDITIONS);
+
+      if (wait_for_heatup) {
+        wait_for_heatup = false;
+        ui.reset_status();
+        return true;
+      }
+
+      return false;
+    }
+
+    void Temperature::wait_for_bed_heating() {
+      if (isHeatingBed()) {
+        SERIAL_ECHOLNPGM("Wait for bed heating...");
+        LCD_MESSAGEPGM(MSG_BED_HEATING);
+        wait_for_bed();
+        ui.reset_status();
+      }
+    }
+
+  #endif // HAS_HEATED_BED
+
+  #if HAS_TEMP_PROBE
+
+    #ifndef MIN_DELTA_SLOPE_DEG_PROBE
+      #define MIN_DELTA_SLOPE_DEG_PROBE 1.0
+    #endif
+    #ifndef MIN_DELTA_SLOPE_TIME_PROBE
+      #define MIN_DELTA_SLOPE_TIME_PROBE 600
+    #endif
+
+    bool Temperature::wait_for_probe(const float target_temp, bool no_wait_for_cooling/*=true*/) {
+
+      const bool wants_to_cool = isProbeAboveTemp(target_temp);
+      const bool will_wait = !(wants_to_cool && no_wait_for_cooling);
+      if (will_wait)
+        SERIAL_ECHOLNPAIR("Waiting for probe to ", (wants_to_cool ? PSTR("cool down") : PSTR("heat up")), " to ", target_temp, " degrees.");
+
+      #if DISABLED(BUSY_WHILE_HEATING) && ENABLED(HOST_KEEPALIVE_FEATURE)
+        KEEPALIVE_STATE(NOT_BUSY);
+      #endif
+
+      float old_temp = 9999;
+      millis_t next_temp_ms = 0, next_delta_check_ms = 0;
+      wait_for_heatup = true;
+      while (will_wait && wait_for_heatup) {
+
+        // Print Temp Reading every 10 seconds while heating up.
+        millis_t now = millis();
+        if (!next_temp_ms || ELAPSED(now, next_temp_ms)) {
+          next_temp_ms = now + 10000UL;
+          print_heater_states(active_extruder);
+          SERIAL_EOL();
+        }
+
+        idle();
+        gcode.reset_stepper_timeout(); // Keep steppers powered
+
+        // Break after MIN_DELTA_SLOPE_TIME_PROBE seconds if the temperature
+        // did not drop at least MIN_DELTA_SLOPE_DEG_PROBE. This avoids waiting
+        // forever as the probe is not actively heated.
+        if (!next_delta_check_ms || ELAPSED(now, next_delta_check_ms)) {
+          const float temp = degProbe(),
+                      delta_temp = old_temp > temp ? old_temp - temp : temp - old_temp;
+          if (delta_temp < float(MIN_DELTA_SLOPE_DEG_PROBE)) {
+            SERIAL_ECHOLNPGM("Timed out waiting for probe temperature.");
+            break;
+          }
+          next_delta_check_ms = now + SEC_TO_MS(MIN_DELTA_SLOPE_TIME_PROBE);
+          old_temp = temp;
+        }
+
+        // Loop until the temperature is very close target
+        if (!(wants_to_cool ? isProbeAboveTemp(target_temp) : isProbeBelowTemp(target_temp))) {
+            SERIAL_ECHOLN(wants_to_cool ? PSTR("Cooldown") : PSTR("Heatup"));
+            SERIAL_ECHOLNPGM(" complete, target probe temperature reached.");
+            break;
+        }
+      }
+
+      if (wait_for_heatup) {
+        wait_for_heatup = false;
+        ui.reset_status();
+        return true;
+      }
+      else if (will_wait)
+        SERIAL_ECHOLNPGM("Canceled wait for probe temperature.");
+
+      return false;
+    }
+
+  #endif // HAS_TEMP_PROBE
+
+  #if HAS_HEATED_CHAMBER
+
+    #ifndef MIN_COOLING_SLOPE_DEG_CHAMBER
+      #define MIN_COOLING_SLOPE_DEG_CHAMBER 1.50
+    #endif
+    #ifndef MIN_COOLING_SLOPE_TIME_CHAMBER
+      #define MIN_COOLING_SLOPE_TIME_CHAMBER 120
+    #endif
+
+    bool Temperature::wait_for_chamber(const bool no_wait_for_cooling/*=true*/) {
+      #if TEMP_CHAMBER_RESIDENCY_TIME > 0
+        millis_t residency_start_ms = 0;
+        bool first_loop = true;
+        // Loop until the temperature has stabilized
+        #define TEMP_CHAMBER_CONDITIONS (!residency_start_ms || PENDING(now, residency_start_ms + SEC_TO_MS(TEMP_CHAMBER_RESIDENCY_TIME)))
+      #else
+        // Loop until the temperature is very close target
+        #define TEMP_CHAMBER_CONDITIONS (wants_to_cool ? isCoolingChamber() : isHeatingChamber())
+      #endif
+
+      #if DISABLED(BUSY_WHILE_HEATING) && ENABLED(HOST_KEEPALIVE_FEATURE)
+        KEEPALIVE_STATE(NOT_BUSY);
+      #endif
+
+      bool wants_to_cool = false;
+      float target_temp = -1, old_temp = 9999;
+      millis_t now, next_temp_ms = 0, next_cool_check_ms = 0;
+      wait_for_heatup = true;
+      do {
+        // Target temperature might be changed during the loop
+        if (target_temp != degTargetChamber()) {
+          wants_to_cool = isCoolingChamber();
+          target_temp = degTargetChamber();
+
+          // Exit if S<lower>, continue if S<higher>, R<lower>, or R<higher>
+          if (no_wait_for_cooling && wants_to_cool) break;
+        }
+
+        now = millis();
+        if (ELAPSED(now, next_temp_ms)) { // Print Temp Reading every 1 second while heating up.
+          next_temp_ms = now + 1000UL;
+          print_heater_states(active_extruder);
+          #if TEMP_CHAMBER_RESIDENCY_TIME > 0
+            SERIAL_ECHOPGM(" W:");
+            if (residency_start_ms)
+              SERIAL_ECHO(long((SEC_TO_MS(TEMP_CHAMBER_RESIDENCY_TIME) - (now - residency_start_ms)) / 1000UL));
+            else
+              SERIAL_CHAR('?');
+          #endif
+          SERIAL_EOL();
+        }
+
+        idle();
+        gcode.reset_stepper_timeout(); // Keep steppers powered
+
+        const float temp = degChamber();
+
+        #if TEMP_CHAMBER_RESIDENCY_TIME > 0
+
+          const float temp_diff = ABS(target_temp - temp);
+
+          if (!residency_start_ms) {
+            // Start the TEMP_CHAMBER_RESIDENCY_TIME timer when we reach target temp for the first time.
+            if (temp_diff < TEMP_CHAMBER_WINDOW)
+              residency_start_ms = now + (first_loop ? SEC_TO_MS(TEMP_CHAMBER_RESIDENCY_TIME) / 3 : 0);
+          }
+          else if (temp_diff > TEMP_CHAMBER_HYSTERESIS) {
+            // Restart the timer whenever the temperature falls outside the hysteresis.
+            residency_start_ms = now;
+          }
+
+          first_loop = false;
+        #endif // TEMP_CHAMBER_RESIDENCY_TIME > 0
+
+        // Prevent a wait-forever situation if R is misused i.e. M191 R0
+        if (wants_to_cool) {
+          // Break after MIN_COOLING_SLOPE_TIME_CHAMBER seconds
+          // if the temperature did not drop at least MIN_COOLING_SLOPE_DEG_CHAMBER
+          if (!next_cool_check_ms || ELAPSED(now, next_cool_check_ms)) {
+            if (old_temp - temp < float(MIN_COOLING_SLOPE_DEG_CHAMBER)) break;
+            next_cool_check_ms = now + SEC_TO_MS(MIN_COOLING_SLOPE_TIME_CHAMBER);
+            old_temp = temp;
+          }
+        }
+      } while (wait_for_heatup && TEMP_CHAMBER_CONDITIONS);
+
+      if (wait_for_heatup) {
+        wait_for_heatup = false;
+        ui.reset_status();
+        return true;
+      }
+
+      return false;
+    }
+
+  #endif // HAS_HEATED_CHAMBER
+
+#endif // HAS_TEMP_SENSOR
diff --git a/Marlin/src/module/temperature.h b/Marlin/src/module/temperature.h
new file mode 100644
index 0000000..002e1cb
--- /dev/null
+++ b/Marlin/src/module/temperature.h
@@ -0,0 +1,885 @@
+/**
+ * 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
+
+/**
+ * temperature.h - temperature controller
+ */
+
+#include "thermistor/thermistors.h"
+
+#include "../inc/MarlinConfig.h"
+
+#if ENABLED(AUTO_POWER_CONTROL)
+  #include "../feature/power.h"
+#endif
+
+#if ENABLED(AUTO_REPORT_TEMPERATURES)
+  #include "../libs/autoreport.h"
+#endif
+
+#ifndef SOFT_PWM_SCALE
+  #define SOFT_PWM_SCALE 0
+#endif
+
+#define HOTEND_INDEX TERN(HAS_MULTI_HOTEND, e, 0)
+#define E_NAME TERN_(HAS_MULTI_HOTEND, e)
+
+// Heater identifiers. Positive values are hotends. Negative values are other heaters.
+typedef enum : int8_t {
+  INDEX_NONE = -5,
+  H_PROBE, H_REDUNDANT, H_CHAMBER, H_BED,
+  H_E0, H_E1, H_E2, H_E3, H_E4, H_E5, H_E6, H_E7
+} heater_id_t;
+
+// PID storage
+typedef struct { float Kp, Ki, Kd;     } PID_t;
+typedef struct { float Kp, Ki, Kd, Kc; } PIDC_t;
+typedef struct { float Kp, Ki, Kd, Kf; } PIDF_t;
+typedef struct { float Kp, Ki, Kd, Kc, Kf; } PIDCF_t;
+
+typedef
+  #if BOTH(PID_EXTRUSION_SCALING, PID_FAN_SCALING)
+    PIDCF_t
+  #elif ENABLED(PID_EXTRUSION_SCALING)
+    PIDC_t
+  #elif ENABLED(PID_FAN_SCALING)
+    PIDF_t
+  #else
+    PID_t
+  #endif
+hotend_pid_t;
+
+#if ENABLED(PID_EXTRUSION_SCALING)
+  typedef IF<(LPQ_MAX_LEN > 255), uint16_t, uint8_t>::type lpq_ptr_t;
+#endif
+
+#define PID_PARAM(F,H) _PID_##F(TERN(PID_PARAMS_PER_HOTEND, H, 0 & H)) // Always use 'H' to suppress warning
+#define _PID_Kp(H) TERN(PIDTEMP, Temperature::temp_hotend[H].pid.Kp, NAN)
+#define _PID_Ki(H) TERN(PIDTEMP, Temperature::temp_hotend[H].pid.Ki, NAN)
+#define _PID_Kd(H) TERN(PIDTEMP, Temperature::temp_hotend[H].pid.Kd, NAN)
+#if ENABLED(PIDTEMP)
+  #define _PID_Kc(H) TERN(PID_EXTRUSION_SCALING, Temperature::temp_hotend[H].pid.Kc, 1)
+  #define _PID_Kf(H) TERN(PID_FAN_SCALING,       Temperature::temp_hotend[H].pid.Kf, 0)
+#else
+  #define _PID_Kc(H) 1
+  #define _PID_Kf(H) 0
+#endif
+
+/**
+ * States for ADC reading in the ISR
+ */
+enum ADCSensorState : char {
+  StartSampling,
+  #if HAS_TEMP_ADC_0
+    PrepareTemp_0, MeasureTemp_0,
+  #endif
+  #if HAS_TEMP_ADC_BED
+    PrepareTemp_BED, MeasureTemp_BED,
+  #endif
+  #if HAS_TEMP_ADC_CHAMBER
+    PrepareTemp_CHAMBER, MeasureTemp_CHAMBER,
+  #endif
+  #if HAS_TEMP_ADC_PROBE
+    PrepareTemp_PROBE, MeasureTemp_PROBE,
+  #endif
+  #if HAS_TEMP_ADC_1
+    PrepareTemp_1, MeasureTemp_1,
+  #endif
+  #if HAS_TEMP_ADC_2
+    PrepareTemp_2, MeasureTemp_2,
+  #endif
+  #if HAS_TEMP_ADC_3
+    PrepareTemp_3, MeasureTemp_3,
+  #endif
+  #if HAS_TEMP_ADC_4
+    PrepareTemp_4, MeasureTemp_4,
+  #endif
+  #if HAS_TEMP_ADC_5
+    PrepareTemp_5, MeasureTemp_5,
+  #endif
+  #if HAS_TEMP_ADC_6
+    PrepareTemp_6, MeasureTemp_6,
+  #endif
+  #if HAS_TEMP_ADC_7
+    PrepareTemp_7, MeasureTemp_7,
+  #endif
+  #if HAS_JOY_ADC_X
+    PrepareJoy_X, MeasureJoy_X,
+  #endif
+  #if HAS_JOY_ADC_Y
+    PrepareJoy_Y, MeasureJoy_Y,
+  #endif
+  #if HAS_JOY_ADC_Z
+    PrepareJoy_Z, MeasureJoy_Z,
+  #endif
+  #if ENABLED(FILAMENT_WIDTH_SENSOR)
+    Prepare_FILWIDTH, Measure_FILWIDTH,
+  #endif
+  #if ENABLED(POWER_MONITOR_CURRENT)
+    Prepare_POWER_MONITOR_CURRENT,
+    Measure_POWER_MONITOR_CURRENT,
+  #endif
+  #if ENABLED(POWER_MONITOR_VOLTAGE)
+    Prepare_POWER_MONITOR_VOLTAGE,
+    Measure_POWER_MONITOR_VOLTAGE,
+  #endif
+  #if HAS_ADC_BUTTONS
+    Prepare_ADC_KEY, Measure_ADC_KEY,
+  #endif
+  SensorsReady, // Temperatures ready. Delay the next round of readings to let ADC pins settle.
+  StartupDelay  // Startup, delay initial temp reading a tiny bit so the hardware can settle
+};
+
+// Minimum number of Temperature::ISR loops between sensor readings.
+// Multiplied by 16 (OVERSAMPLENR) to obtain the total time to
+// get all oversampled sensor readings
+#define MIN_ADC_ISR_LOOPS 10
+
+#define ACTUAL_ADC_SAMPLES _MAX(int(MIN_ADC_ISR_LOOPS), int(SensorsReady))
+
+#if HAS_PID_HEATING
+  #define PID_K2 (1-float(PID_K1))
+  #define PID_dT ((OVERSAMPLENR * float(ACTUAL_ADC_SAMPLES)) / TEMP_TIMER_FREQUENCY)
+
+  // Apply the scale factors to the PID values
+  #define scalePID_i(i)   ( float(i) * PID_dT )
+  #define unscalePID_i(i) ( float(i) / PID_dT )
+  #define scalePID_d(d)   ( float(d) / PID_dT )
+  #define unscalePID_d(d) ( float(d) * PID_dT )
+#endif
+
+#if BOTH(HAS_LCD_MENU, G26_MESH_VALIDATION)
+  #define G26_CLICK_CAN_CANCEL 1
+#endif
+
+// A temperature sensor
+typedef struct TempInfo {
+  uint16_t acc;
+  int16_t raw;
+  float celsius;
+  inline void reset() { acc = 0; }
+  inline void sample(const uint16_t s) { acc += s; }
+  inline void update() { raw = acc; }
+} temp_info_t;
+
+// A PWM heater with temperature sensor
+typedef struct HeaterInfo : public TempInfo {
+  int16_t target;
+  uint8_t soft_pwm_amount;
+} heater_info_t;
+
+// A heater with PID stabilization
+template<typename T>
+struct PIDHeaterInfo : public HeaterInfo {
+  T pid;  // Initialized by settings.load()
+};
+
+#if ENABLED(PIDTEMP)
+  typedef struct PIDHeaterInfo<hotend_pid_t> hotend_info_t;
+#else
+  typedef heater_info_t hotend_info_t;
+#endif
+#if HAS_HEATED_BED
+  #if ENABLED(PIDTEMPBED)
+    typedef struct PIDHeaterInfo<PID_t> bed_info_t;
+  #else
+    typedef heater_info_t bed_info_t;
+  #endif
+#endif
+#if HAS_TEMP_PROBE
+  typedef temp_info_t probe_info_t;
+#endif
+#if HAS_HEATED_CHAMBER
+  typedef heater_info_t chamber_info_t;
+#elif HAS_TEMP_CHAMBER
+  typedef temp_info_t chamber_info_t;
+#endif
+
+// Heater watch handling
+template <int INCREASE, int HYSTERESIS, millis_t PERIOD>
+struct HeaterWatch {
+  uint16_t target;
+  millis_t next_ms;
+  inline bool elapsed(const millis_t &ms) { return next_ms && ELAPSED(ms, next_ms); }
+  inline bool elapsed() { return elapsed(millis()); }
+
+  inline void restart(const int16_t curr, const int16_t tgt) {
+    if (tgt) {
+      const int16_t newtarget = curr + INCREASE;
+      if (newtarget < tgt - HYSTERESIS - 1) {
+        target = newtarget;
+        next_ms = millis() + SEC_TO_MS(PERIOD);
+        return;
+      }
+    }
+    next_ms = 0;
+  }
+};
+
+#if WATCH_HOTENDS
+  typedef struct HeaterWatch<WATCH_TEMP_INCREASE, TEMP_HYSTERESIS, WATCH_TEMP_PERIOD> hotend_watch_t;
+#endif
+#if WATCH_BED
+  typedef struct HeaterWatch<WATCH_BED_TEMP_INCREASE, TEMP_BED_HYSTERESIS, WATCH_BED_TEMP_PERIOD> bed_watch_t;
+#endif
+#if WATCH_CHAMBER
+  typedef struct HeaterWatch<WATCH_CHAMBER_TEMP_INCREASE, TEMP_CHAMBER_HYSTERESIS, WATCH_CHAMBER_TEMP_PERIOD> chamber_watch_t;
+#endif
+
+// Temperature sensor read value ranges
+typedef struct { int16_t raw_min, raw_max; } raw_range_t;
+typedef struct { int16_t mintemp, maxtemp; } celsius_range_t;
+typedef struct { int16_t raw_min, raw_max, mintemp, maxtemp; } temp_range_t;
+
+#define THERMISTOR_ABS_ZERO_C           -273.15f  // bbbbrrrrr cold !
+#define THERMISTOR_RESISTANCE_NOMINAL_C 25.0f     // mmmmm comfortable
+
+#if HAS_USER_THERMISTORS
+
+  enum CustomThermistorIndex : uint8_t {
+    #if HEATER_0_USER_THERMISTOR
+      CTI_HOTEND_0,
+    #endif
+    #if HEATER_1_USER_THERMISTOR
+      CTI_HOTEND_1,
+    #endif
+    #if HEATER_2_USER_THERMISTOR
+      CTI_HOTEND_2,
+    #endif
+    #if HEATER_3_USER_THERMISTOR
+      CTI_HOTEND_3,
+    #endif
+    #if HEATER_4_USER_THERMISTOR
+      CTI_HOTEND_4,
+    #endif
+    #if HEATER_5_USER_THERMISTOR
+      CTI_HOTEND_5,
+    #endif
+    #if HEATER_BED_USER_THERMISTOR
+      CTI_BED,
+    #endif
+    #if HEATER_PROBE_USER_THERMISTOR
+      CTI_PROBE,
+    #endif
+    #if HEATER_CHAMBER_USER_THERMISTOR
+      CTI_CHAMBER,
+    #endif
+    USER_THERMISTORS
+  };
+
+  // User-defined thermistor
+  typedef struct {
+    bool pre_calc;     // true if pre-calculations update needed
+    float sh_c_coeff,  // Steinhart-Hart C coefficient .. defaults to '0.0'
+          sh_alpha,
+          series_res,
+          res_25, res_25_recip,
+          res_25_log,
+          beta, beta_recip;
+  } user_thermistor_t;
+
+#endif
+
+class Temperature {
+
+  public:
+
+    #if HAS_HOTEND
+      #define HOTEND_TEMPS (HOTENDS + ENABLED(TEMP_SENSOR_1_AS_REDUNDANT))
+      static hotend_info_t temp_hotend[HOTEND_TEMPS];
+      static const uint16_t heater_maxtemp[HOTENDS];
+    #endif
+    TERN_(HAS_HEATED_BED, static bed_info_t temp_bed);
+    TERN_(HAS_TEMP_PROBE, static probe_info_t temp_probe);
+    TERN_(HAS_TEMP_CHAMBER, static chamber_info_t temp_chamber);
+
+    TERN_(AUTO_POWER_E_FANS, static uint8_t autofan_speed[HOTENDS]);
+    TERN_(AUTO_POWER_CHAMBER_FAN, static uint8_t chamberfan_speed);
+
+    #if ENABLED(FAN_SOFT_PWM)
+      static uint8_t soft_pwm_amount_fan[FAN_COUNT],
+                     soft_pwm_count_fan[FAN_COUNT];
+    #endif
+
+    #if ENABLED(PREVENT_COLD_EXTRUSION)
+      static bool allow_cold_extrude;
+      static int16_t extrude_min_temp;
+      FORCE_INLINE static bool tooCold(const int16_t temp) { return allow_cold_extrude ? false : temp < extrude_min_temp - (TEMP_WINDOW); }
+      FORCE_INLINE static bool tooColdToExtrude(const uint8_t E_NAME) {
+        return tooCold(degHotend(HOTEND_INDEX));
+      }
+      FORCE_INLINE static bool targetTooColdToExtrude(const uint8_t E_NAME) {
+        return tooCold(degTargetHotend(HOTEND_INDEX));
+      }
+    #else
+      FORCE_INLINE static bool tooColdToExtrude(const uint8_t) { return false; }
+      FORCE_INLINE static bool targetTooColdToExtrude(const uint8_t) { return false; }
+    #endif
+
+    FORCE_INLINE static bool hotEnoughToExtrude(const uint8_t e) { return !tooColdToExtrude(e); }
+    FORCE_INLINE static bool targetHotEnoughToExtrude(const uint8_t e) { return !targetTooColdToExtrude(e); }
+
+    #if ENABLED(SINGLENOZZLE_STANDBY_FAN)
+      static uint16_t singlenozzle_temp[EXTRUDERS];
+      #if HAS_FAN
+        static uint8_t singlenozzle_fan_speed[EXTRUDERS];
+      #endif
+      static void singlenozzle_change(const uint8_t old_tool, const uint8_t new_tool);
+    #endif
+
+    #if HEATER_IDLE_HANDLER
+
+      // Heater idle handling. Marlin creates one per hotend and one for the heated bed.
+      typedef struct {
+        millis_t timeout_ms;
+        bool timed_out;
+        inline void update(const millis_t &ms) { if (!timed_out && timeout_ms && ELAPSED(ms, timeout_ms)) timed_out = true; }
+        inline void start(const millis_t &ms) { timeout_ms = millis() + ms; timed_out = false; }
+        inline void reset() { timeout_ms = 0; timed_out = false; }
+        inline void expire() { start(0); }
+      } heater_idle_t;
+
+      // Indices and size for the heater_idle array
+      #define _ENUM_FOR_E(N) IDLE_INDEX_E##N,
+      enum IdleIndex : uint8_t {
+        REPEAT(HOTENDS, _ENUM_FOR_E)
+        #if ENABLED(HAS_HEATED_BED)
+          IDLE_INDEX_BED,
+        #endif
+        NR_HEATER_IDLE
+      };
+      #undef _ENUM_FOR_E
+
+      // Convert the given heater_id_t to idle array index
+      static inline IdleIndex idle_index_for_id(const int8_t heater_id) {
+        #if HAS_HEATED_BED
+          if (heater_id == H_BED) return IDLE_INDEX_BED;
+        #endif
+        return (IdleIndex)_MAX(heater_id, 0);
+      }
+
+      static heater_idle_t heater_idle[NR_HEATER_IDLE];
+
+    #endif
+
+  private:
+
+    TERN_(EARLY_WATCHDOG, static bool inited);   // If temperature controller is running
+
+    static volatile bool raw_temps_ready;
+
+    TERN_(WATCH_HOTENDS, static hotend_watch_t watch_hotend[HOTENDS]);
+
+    #if ENABLED(TEMP_SENSOR_1_AS_REDUNDANT)
+      static uint16_t redundant_temperature_raw;
+      static float redundant_temperature;
+    #endif
+
+    #if ENABLED(PID_EXTRUSION_SCALING)
+      static int32_t last_e_position, lpq[LPQ_MAX_LEN];
+      static lpq_ptr_t lpq_ptr;
+    #endif
+
+    TERN_(HAS_HOTEND, static temp_range_t temp_range[HOTENDS]);
+
+    #if HAS_HEATED_BED
+      TERN_(WATCH_BED, static bed_watch_t watch_bed);
+      IF_DISABLED(PIDTEMPBED, static millis_t next_bed_check_ms);
+      #ifdef BED_MINTEMP
+        static int16_t mintemp_raw_BED;
+      #endif
+      #ifdef BED_MAXTEMP
+        static int16_t maxtemp_raw_BED;
+      #endif
+    #endif
+
+    #if HAS_HEATED_CHAMBER
+      TERN_(WATCH_CHAMBER, static chamber_watch_t watch_chamber);
+      static millis_t next_chamber_check_ms;
+      #ifdef CHAMBER_MINTEMP
+        static int16_t mintemp_raw_CHAMBER;
+      #endif
+      #ifdef CHAMBER_MAXTEMP
+        static int16_t maxtemp_raw_CHAMBER;
+      #endif
+    #endif
+
+    #ifdef MAX_CONSECUTIVE_LOW_TEMPERATURE_ERROR_ALLOWED
+      static uint8_t consecutive_low_temperature_error[HOTENDS];
+    #endif
+
+    #ifdef MILLISECONDS_PREHEAT_TIME
+      static millis_t preheat_end_time[HOTENDS];
+    #endif
+
+    TERN_(HAS_AUTO_FAN, static millis_t next_auto_fan_check_ms);
+
+    TERN_(PROBING_HEATERS_OFF, static bool paused);
+
+  public:
+    #if HAS_ADC_BUTTONS
+      static uint32_t current_ADCKey_raw;
+      static uint16_t ADCKey_count;
+    #endif
+
+    TERN_(PID_EXTRUSION_SCALING, static int16_t lpq_len);
+
+    /**
+     * Instance Methods
+     */
+
+    void init();
+
+    /**
+     * Static (class) methods
+     */
+
+    #if HAS_USER_THERMISTORS
+      static user_thermistor_t user_thermistor[USER_THERMISTORS];
+      static void log_user_thermistor(const uint8_t t_index, const bool eprom=false);
+      static void reset_user_thermistors();
+      static float user_thermistor_to_deg_c(const uint8_t t_index, const int raw);
+      static bool set_pull_up_res(int8_t t_index, float value) {
+        //if (!WITHIN(t_index, 0, USER_THERMISTORS - 1)) return false;
+        if (!WITHIN(value, 1, 1000000)) return false;
+        user_thermistor[t_index].series_res = value;
+        return true;
+      }
+      static bool set_res25(int8_t t_index, float value) {
+        if (!WITHIN(value, 1, 10000000)) return false;
+        user_thermistor[t_index].res_25 = value;
+        user_thermistor[t_index].pre_calc = true;
+        return true;
+      }
+      static bool set_beta(int8_t t_index, float value) {
+        if (!WITHIN(value, 1, 1000000)) return false;
+        user_thermistor[t_index].beta = value;
+        user_thermistor[t_index].pre_calc = true;
+        return true;
+      }
+      static bool set_sh_coeff(int8_t t_index, float value) {
+        if (!WITHIN(value, -0.01f, 0.01f)) return false;
+        user_thermistor[t_index].sh_c_coeff = value;
+        user_thermistor[t_index].pre_calc = true;
+        return true;
+      }
+    #endif
+
+    #if HAS_HOTEND
+      static float analog_to_celsius_hotend(const int raw, const uint8_t e);
+    #endif
+
+    #if HAS_HEATED_BED
+      static float analog_to_celsius_bed(const int raw);
+    #endif
+    #if HAS_TEMP_PROBE
+      static float analog_to_celsius_probe(const int raw);
+    #endif
+    #if HAS_TEMP_CHAMBER
+      static float analog_to_celsius_chamber(const int raw);
+    #endif
+
+    #if HAS_FAN
+
+      static uint8_t fan_speed[FAN_COUNT];
+      #define FANS_LOOP(I) LOOP_L_N(I, FAN_COUNT)
+
+      static void set_fan_speed(const uint8_t target, const uint16_t speed);
+
+      #if ENABLED(REPORT_FAN_CHANGE)
+        static void report_fan_speed(const uint8_t target);
+      #endif
+
+      #if EITHER(PROBING_FANS_OFF, ADVANCED_PAUSE_FANS_PAUSE)
+        static bool fans_paused;
+        static uint8_t saved_fan_speed[FAN_COUNT];
+      #endif
+
+      static constexpr inline uint8_t fanPercent(const uint8_t speed) { return ui8_to_percent(speed); }
+
+      TERN_(ADAPTIVE_FAN_SLOWING, static uint8_t fan_speed_scaler[FAN_COUNT]);
+
+      static inline uint8_t scaledFanSpeed(const uint8_t target, const uint8_t fs) {
+        UNUSED(target); // Potentially unused!
+        return (fs * uint16_t(TERN(ADAPTIVE_FAN_SLOWING, fan_speed_scaler[target], 128))) >> 7;
+      }
+
+      static inline uint8_t scaledFanSpeed(const uint8_t target) {
+        return scaledFanSpeed(target, fan_speed[target]);
+      }
+
+      #if ENABLED(EXTRA_FAN_SPEED)
+        static uint8_t old_fan_speed[FAN_COUNT], new_fan_speed[FAN_COUNT];
+        static void set_temp_fan_speed(const uint8_t fan, const uint16_t tmp_temp);
+      #endif
+
+      #if EITHER(PROBING_FANS_OFF, ADVANCED_PAUSE_FANS_PAUSE)
+        void set_fans_paused(const bool p);
+      #endif
+
+    #endif // HAS_FAN
+
+    static inline void zero_fan_speeds() {
+      #if HAS_FAN
+        FANS_LOOP(i) set_fan_speed(i, 0);
+      #endif
+    }
+
+    /**
+     * Called from the Temperature ISR
+     */
+    static void readings_ready();
+    static void tick();
+
+    /**
+     * Call periodically to manage heaters
+     */
+    static void manage_heater() _O2; // Added _O2 to work around a compiler error
+
+    /**
+     * Preheating hotends
+     */
+    #ifdef MILLISECONDS_PREHEAT_TIME
+      static bool is_preheating(const uint8_t E_NAME) {
+        return preheat_end_time[HOTEND_INDEX] && PENDING(millis(), preheat_end_time[HOTEND_INDEX]);
+      }
+      static void start_preheat_time(const uint8_t E_NAME) {
+        preheat_end_time[HOTEND_INDEX] = millis() + MILLISECONDS_PREHEAT_TIME;
+      }
+      static void reset_preheat_time(const uint8_t E_NAME) {
+        preheat_end_time[HOTEND_INDEX] = 0;
+      }
+    #else
+      #define is_preheating(n) (false)
+    #endif
+
+    //high level conversion routines, for use outside of temperature.cpp
+    //inline so that there is no performance decrease.
+    //deg=degreeCelsius
+
+    FORCE_INLINE static float degHotend(const uint8_t E_NAME) {
+      return TERN0(HAS_HOTEND, temp_hotend[HOTEND_INDEX].celsius);
+    }
+
+    #if ENABLED(SHOW_TEMP_ADC_VALUES)
+      FORCE_INLINE static int16_t rawHotendTemp(const uint8_t E_NAME) {
+        return TERN0(HAS_HOTEND, temp_hotend[HOTEND_INDEX].raw);
+      }
+    #endif
+
+    FORCE_INLINE static int16_t degTargetHotend(const uint8_t E_NAME) {
+      return TERN0(HAS_HOTEND, temp_hotend[HOTEND_INDEX].target);
+    }
+
+    #if WATCH_HOTENDS
+      static void start_watching_hotend(const uint8_t e=0);
+    #else
+      static inline void start_watching_hotend(const uint8_t=0) {}
+    #endif
+
+    #if HAS_HOTEND
+
+      static void setTargetHotend(const int16_t celsius, const uint8_t E_NAME) {
+        const uint8_t ee = HOTEND_INDEX;
+        #ifdef MILLISECONDS_PREHEAT_TIME
+          if (celsius == 0)
+            reset_preheat_time(ee);
+          else if (temp_hotend[ee].target == 0)
+            start_preheat_time(ee);
+        #endif
+        TERN_(AUTO_POWER_CONTROL, if (celsius) powerManager.power_on());
+        temp_hotend[ee].target = _MIN(celsius, temp_range[ee].maxtemp - HOTEND_OVERSHOOT);
+        start_watching_hotend(ee);
+      }
+
+      FORCE_INLINE static bool isHeatingHotend(const uint8_t E_NAME) {
+        return temp_hotend[HOTEND_INDEX].target > temp_hotend[HOTEND_INDEX].celsius;
+      }
+
+      FORCE_INLINE static bool isCoolingHotend(const uint8_t E_NAME) {
+        return temp_hotend[HOTEND_INDEX].target < temp_hotend[HOTEND_INDEX].celsius;
+      }
+
+      #if HAS_TEMP_HOTEND
+        static bool wait_for_hotend(const uint8_t target_extruder, const bool no_wait_for_cooling=true
+          #if G26_CLICK_CAN_CANCEL
+            , const bool click_to_cancel=false
+          #endif
+        );
+      #endif
+
+      FORCE_INLINE static bool still_heating(const uint8_t e) {
+        return degTargetHotend(e) > TEMP_HYSTERESIS && ABS(degHotend(e) - degTargetHotend(e)) > TEMP_HYSTERESIS;
+      }
+
+      FORCE_INLINE static bool degHotendNear(const uint8_t e, const float &temp) {
+        return ABS(degHotend(e) - temp) < (TEMP_HYSTERESIS);
+      }
+
+    #endif // HAS_HOTEND
+
+    #if HAS_HEATED_BED
+
+      #if ENABLED(SHOW_TEMP_ADC_VALUES)
+        FORCE_INLINE static int16_t rawBedTemp()  { return temp_bed.raw; }
+      #endif
+      FORCE_INLINE static float degBed()          { return temp_bed.celsius; }
+      FORCE_INLINE static int16_t degTargetBed()  { return temp_bed.target; }
+      FORCE_INLINE static bool isHeatingBed()     { return temp_bed.target > temp_bed.celsius; }
+      FORCE_INLINE static bool isCoolingBed()     { return temp_bed.target < temp_bed.celsius; }
+
+      #if WATCH_BED
+        static void start_watching_bed();
+      #else
+        static inline void start_watching_bed() {}
+      #endif
+
+      static void setTargetBed(const int16_t celsius) {
+        TERN_(AUTO_POWER_CONTROL, if (celsius) powerManager.power_on());
+        temp_bed.target =
+          #ifdef BED_MAX_TARGET
+            _MIN(celsius, BED_MAX_TARGET)
+          #else
+            celsius
+          #endif
+        ;
+        start_watching_bed();
+      }
+
+      static bool wait_for_bed(const bool no_wait_for_cooling=true
+        #if G26_CLICK_CAN_CANCEL
+          , const bool click_to_cancel=false
+        #endif
+      );
+
+      static void wait_for_bed_heating();
+
+      FORCE_INLINE static bool degBedNear(const float &temp) {
+        return ABS(degBed() - temp) < (TEMP_BED_HYSTERESIS);
+      }
+
+    #endif // HAS_HEATED_BED
+
+    #if HAS_TEMP_PROBE
+      #if ENABLED(SHOW_TEMP_ADC_VALUES)
+        FORCE_INLINE static int16_t rawProbeTemp()    { return temp_probe.raw; }
+      #endif
+      FORCE_INLINE static float degProbe()            { return temp_probe.celsius; }
+      FORCE_INLINE static bool isProbeBelowTemp(const float target_temp) { return temp_probe.celsius < target_temp; }
+      FORCE_INLINE static bool isProbeAboveTemp(const float target_temp) { return temp_probe.celsius > target_temp; }
+      static bool wait_for_probe(const float target_temp, bool no_wait_for_cooling=true);
+    #endif
+
+    #if WATCH_PROBE
+      static void start_watching_probe();
+    #else
+      static inline void start_watching_probe() {}
+    #endif
+
+    #if HAS_TEMP_CHAMBER
+      #if ENABLED(SHOW_TEMP_ADC_VALUES)
+        FORCE_INLINE static int16_t rawChamberTemp()    { return temp_chamber.raw; }
+      #endif
+      FORCE_INLINE static float degChamber()            { return temp_chamber.celsius; }
+      #if HAS_HEATED_CHAMBER
+        FORCE_INLINE static int16_t degTargetChamber()  { return temp_chamber.target; }
+        FORCE_INLINE static bool isHeatingChamber()     { return temp_chamber.target > temp_chamber.celsius; }
+        FORCE_INLINE static bool isCoolingChamber()     { return temp_chamber.target < temp_chamber.celsius; }
+
+        static bool wait_for_chamber(const bool no_wait_for_cooling=true);
+      #endif
+    #endif
+
+    #if WATCH_CHAMBER
+      static void start_watching_chamber();
+    #else
+      static inline void start_watching_chamber() {}
+    #endif
+
+    #if HAS_HEATED_CHAMBER
+      static void setTargetChamber(const int16_t celsius) {
+        temp_chamber.target =
+          #ifdef CHAMBER_MAXTEMP
+            _MIN(celsius, CHAMBER_MAXTEMP - 10)
+          #else
+            celsius
+          #endif
+        ;
+        start_watching_chamber();
+      }
+    #endif
+
+    /**
+     * The software PWM power for a heater
+     */
+    static int16_t getHeaterPower(const heater_id_t heater_id);
+
+    /**
+     * Switch off all heaters, set all target temperatures to 0
+     */
+    static void disable_all_heaters();
+
+    #if ENABLED(PRINTJOB_TIMER_AUTOSTART)
+      /**
+       * Methods to check if heaters are enabled, indicating an active job
+       */
+      static bool auto_job_over_threshold();
+      static void auto_job_check_timer(const bool can_start, const bool can_stop);
+    #endif
+
+    /**
+     * Perform auto-tuning for hotend or bed in response to M303
+     */
+    #if HAS_PID_HEATING
+      static void PID_autotune(const float &target, const heater_id_t heater_id, const int8_t ncycles, const bool set_result=false);
+
+      #if ENABLED(NO_FAN_SLOWING_IN_PID_TUNING)
+        static bool adaptive_fan_slowing;
+      #elif ENABLED(ADAPTIVE_FAN_SLOWING)
+        static constexpr bool adaptive_fan_slowing = true;
+      #endif
+
+      /**
+       * Update the temp manager when PID values change
+       */
+      #if ENABLED(PIDTEMP)
+        FORCE_INLINE static void updatePID() {
+          TERN_(PID_EXTRUSION_SCALING, last_e_position = 0);
+        }
+      #endif
+
+    #endif
+
+    #if ENABLED(PROBING_HEATERS_OFF)
+      static void pause(const bool p);
+      FORCE_INLINE static bool is_paused() { return paused; }
+    #endif
+
+    #if HEATER_IDLE_HANDLER
+
+      static void reset_hotend_idle_timer(const uint8_t E_NAME) {
+        heater_idle[HOTEND_INDEX].reset();
+        start_watching_hotend(HOTEND_INDEX);
+      }
+
+      #if HAS_HEATED_BED
+        static void reset_bed_idle_timer() {
+          heater_idle[IDLE_INDEX_BED].reset();
+          start_watching_bed();
+        }
+      #endif
+
+    #endif // HEATER_IDLE_HANDLER
+
+    #if HAS_TEMP_SENSOR
+      static void print_heater_states(const uint8_t target_extruder
+        #if ENABLED(TEMP_SENSOR_1_AS_REDUNDANT)
+          , const bool include_r=false
+        #endif
+      );
+      #if ENABLED(AUTO_REPORT_TEMPERATURES)
+        struct AutoReportTemp { static void report(); };
+        static AutoReporter<AutoReportTemp> auto_reporter;
+      #endif
+    #endif
+
+    TERN_(HAS_DISPLAY, static void set_heating_message(const uint8_t e));
+
+    #if HAS_LCD_MENU && HAS_TEMPERATURE
+      static void lcd_preheat(const int16_t e, const int8_t indh, const int8_t indb);
+    #endif
+
+  private:
+    static void update_raw_temperatures();
+    static void updateTemperaturesFromRawValues();
+
+    #define HAS_MAX6675 EITHER(HEATER_0_USES_MAX6675, HEATER_1_USES_MAX6675)
+    #if HAS_MAX6675
+      #define COUNT_6675 1 + BOTH(HEATER_0_USES_MAX6675, HEATER_1_USES_MAX6675)
+      #if COUNT_6675 > 1
+        #define HAS_MULTI_6675 1
+        #define READ_MAX6675(N) read_max6675(N)
+      #else
+        #define READ_MAX6675(N) read_max6675()
+      #endif
+      static int read_max6675(TERN_(HAS_MULTI_6675, const uint8_t hindex=0));
+    #endif
+
+    static void checkExtruderAutoFans();
+
+    static float get_pid_output_hotend(const uint8_t e);
+
+    TERN_(PIDTEMPBED, static float get_pid_output_bed());
+
+    TERN_(HAS_HEATED_CHAMBER, static float get_pid_output_chamber());
+
+    static void _temp_error(const heater_id_t e, PGM_P const serial_msg, PGM_P const lcd_msg);
+    static void min_temp_error(const heater_id_t e);
+    static void max_temp_error(const heater_id_t e);
+
+    #define HAS_THERMAL_PROTECTION ANY(THERMAL_PROTECTION_HOTENDS, THERMAL_PROTECTION_CHAMBER, HAS_THERMALLY_PROTECTED_BED)
+
+    #if HAS_THERMAL_PROTECTION
+
+      // Indices and size for the tr_state_machine array. One for each protected heater.
+      #define _ENUM_FOR_E(N) RUNAWAY_IND_E##N,
+      enum RunawayIndex : uint8_t {
+        #if ENABLED(THERMAL_PROTECTION_HOTENDS)
+          REPEAT(HOTENDS, _ENUM_FOR_E)
+        #endif
+        #if ENABLED(HAS_THERMALLY_PROTECTED_BED)
+          RUNAWAY_IND_BED,
+        #endif
+        #if ENABLED(THERMAL_PROTECTION_CHAMBER)
+          RUNAWAY_IND_CHAMBER,
+        #endif
+        NR_HEATER_RUNAWAY
+      };
+      #undef _ENUM_FOR_E
+
+      // Convert the given heater_id_t to runaway state array index
+      static inline RunawayIndex runaway_index_for_id(const int8_t heater_id) {
+        #if HAS_THERMALLY_PROTECTED_CHAMBER
+          if (heater_id == H_CHAMBER) return RUNAWAY_IND_CHAMBER;
+        #endif
+        #if HAS_THERMALLY_PROTECTED_BED
+          if (heater_id == H_BED) return RUNAWAY_IND_BED;
+        #endif
+        return (RunawayIndex)_MAX(heater_id, 0);
+      }
+
+      enum TRState : char { TRInactive, TRFirstHeating, TRStable, TRRunaway };
+
+      typedef struct {
+        millis_t timer = 0;
+        TRState state = TRInactive;
+        float running_temp;
+        void run(const float &current, const float &target, const heater_id_t heater_id, const uint16_t period_seconds, const uint16_t hysteresis_degc);
+      } tr_state_machine_t;
+
+      static tr_state_machine_t tr_state_machine[NR_HEATER_RUNAWAY];
+
+    #endif // HAS_THERMAL_PROTECTION
+};
+
+extern Temperature thermalManager;
diff --git a/Marlin/src/module/thermistor/thermistor_1.h b/Marlin/src/module/thermistor/thermistor_1.h
new file mode 100644
index 0000000..fad7908
--- /dev/null
+++ b/Marlin/src/module/thermistor/thermistor_1.h
@@ -0,0 +1,90 @@
+/**
+ * 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
+
+// R25 = 100 kOhm, beta25 = 4092 K, 4.7 kOhm pull-up, bed thermistor
+const temp_entry_t temptable_1[] PROGMEM = {
+  { OV(  23), 300 },
+  { OV(  25), 295 },
+  { OV(  27), 290 },
+  { OV(  28), 285 },
+  { OV(  31), 280 },
+  { OV(  33), 275 },
+  { OV(  35), 270 },
+  { OV(  38), 265 },
+  { OV(  41), 260 },
+  { OV(  44), 255 },
+  { OV(  48), 250 },
+  { OV(  52), 245 },
+  { OV(  56), 240 },
+  { OV(  61), 235 },
+  { OV(  66), 230 },
+  { OV(  71), 225 },
+  { OV(  78), 220 },
+  { OV(  84), 215 },
+  { OV(  92), 210 },
+  { OV( 100), 205 },
+  { OV( 109), 200 },
+  { OV( 120), 195 },
+  { OV( 131), 190 },
+  { OV( 143), 185 },
+  { OV( 156), 180 },
+  { OV( 171), 175 },
+  { OV( 187), 170 },
+  { OV( 205), 165 },
+  { OV( 224), 160 },
+  { OV( 245), 155 },
+  { OV( 268), 150 },
+  { OV( 293), 145 },
+  { OV( 320), 140 },
+  { OV( 348), 135 },
+  { OV( 379), 130 },
+  { OV( 411), 125 },
+  { OV( 445), 120 },
+  { OV( 480), 115 },
+  { OV( 516), 110 },
+  { OV( 553), 105 },
+  { OV( 591), 100 },
+  { OV( 628),  95 },
+  { OV( 665),  90 },
+  { OV( 702),  85 },
+  { OV( 737),  80 },
+  { OV( 770),  75 },
+  { OV( 801),  70 },
+  { OV( 830),  65 },
+  { OV( 857),  60 },
+  { OV( 881),  55 },
+  { OV( 903),  50 },
+  { OV( 922),  45 },
+  { OV( 939),  40 },
+  { OV( 954),  35 },
+  { OV( 966),  30 },
+  { OV( 977),  25 },
+  { OV( 985),  20 },
+  { OV( 993),  15 },
+  { OV( 999),  10 },
+  { OV(1004),   5 },
+  { OV(1008),   0 },
+  { OV(1012),  -5 },
+  { OV(1016), -10 },
+  { OV(1020), -15 }
+};
diff --git a/Marlin/src/module/thermistor/thermistor_10.h b/Marlin/src/module/thermistor/thermistor_10.h
new file mode 100644
index 0000000..c24ad40
--- /dev/null
+++ b/Marlin/src/module/thermistor/thermistor_10.h
@@ -0,0 +1,57 @@
+/**
+ * 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
+
+// R25 = 100 kOhm, beta25 = 3960 K, 4.7 kOhm pull-up, RS thermistor 198-961
+const temp_entry_t temptable_10[] PROGMEM = {
+  { OV(   1), 929 },
+  { OV(  36), 299 },
+  { OV(  71), 246 },
+  { OV( 106), 217 },
+  { OV( 141), 198 },
+  { OV( 176), 184 },
+  { OV( 211), 173 },
+  { OV( 246), 163 },
+  { OV( 281), 154 },
+  { OV( 316), 147 },
+  { OV( 351), 140 },
+  { OV( 386), 134 },
+  { OV( 421), 128 },
+  { OV( 456), 122 },
+  { OV( 491), 117 },
+  { OV( 526), 112 },
+  { OV( 561), 107 },
+  { OV( 596), 102 },
+  { OV( 631),  97 },
+  { OV( 666),  91 },
+  { OV( 701),  86 },
+  { OV( 736),  81 },
+  { OV( 771),  76 },
+  { OV( 806),  70 },
+  { OV( 841),  63 },
+  { OV( 876),  56 },
+  { OV( 911),  48 },
+  { OV( 946),  38 },
+  { OV( 981),  23 },
+  { OV(1005),   5 },
+  { OV(1016),   0 }
+};
diff --git a/Marlin/src/module/thermistor/thermistor_1010.h b/Marlin/src/module/thermistor/thermistor_1010.h
new file mode 100644
index 0000000..8ab5e3b
--- /dev/null
+++ b/Marlin/src/module/thermistor/thermistor_1010.h
@@ -0,0 +1,41 @@
+/**
+ * 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
+
+#define REVERSE_TEMP_SENSOR_RANGE_1010 1
+
+// Pt1000 with 1k0 pullup
+const temp_entry_t temptable_1010[] PROGMEM = {
+  PtLine(  0, 1000, 1000),
+  PtLine( 25, 1000, 1000),
+  PtLine( 50, 1000, 1000),
+  PtLine( 75, 1000, 1000),
+  PtLine(100, 1000, 1000),
+  PtLine(125, 1000, 1000),
+  PtLine(150, 1000, 1000),
+  PtLine(175, 1000, 1000),
+  PtLine(200, 1000, 1000),
+  PtLine(225, 1000, 1000),
+  PtLine(250, 1000, 1000),
+  PtLine(275, 1000, 1000),
+  PtLine(300, 1000, 1000)
+};
diff --git a/Marlin/src/module/thermistor/thermistor_1047.h b/Marlin/src/module/thermistor/thermistor_1047.h
new file mode 100644
index 0000000..6e1b61f
--- /dev/null
+++ b/Marlin/src/module/thermistor/thermistor_1047.h
@@ -0,0 +1,40 @@
+/**
+ * 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
+
+#define REVERSE_TEMP_SENSOR_RANGE_1047 1
+
+// Pt1000 with 4k7 pullup
+const temp_entry_t temptable_1047[] PROGMEM = {
+  // only a few values are needed as the curve is very flat
+  PtLine(  0, 1000, 4700),
+  PtLine( 50, 1000, 4700),
+  PtLine(100, 1000, 4700),
+  PtLine(150, 1000, 4700),
+  PtLine(200, 1000, 4700),
+  PtLine(250, 1000, 4700),
+  PtLine(300, 1000, 4700),
+  PtLine(350, 1000, 4700),
+  PtLine(400, 1000, 4700),
+  PtLine(450, 1000, 4700),
+  PtLine(500, 1000, 4700)
+};
diff --git a/Marlin/src/module/thermistor/thermistor_11.h b/Marlin/src/module/thermistor/thermistor_11.h
new file mode 100644
index 0000000..345d009
--- /dev/null
+++ b/Marlin/src/module/thermistor/thermistor_11.h
@@ -0,0 +1,76 @@
+/**
+ * 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
+
+// R25 = 100 kOhm, beta25 = 3950 K, 4.7 kOhm pull-up, QU-BD silicone bed QWG-104F-3950 thermistor
+const temp_entry_t temptable_11[] PROGMEM = {
+  { OV(   1), 938 },
+  { OV(  31), 314 },
+  { OV(  41), 290 },
+  { OV(  51), 272 },
+  { OV(  61), 258 },
+  { OV(  71), 247 },
+  { OV(  81), 237 },
+  { OV(  91), 229 },
+  { OV( 101), 221 },
+  { OV( 111), 215 },
+  { OV( 121), 209 },
+  { OV( 131), 204 },
+  { OV( 141), 199 },
+  { OV( 151), 195 },
+  { OV( 161), 190 },
+  { OV( 171), 187 },
+  { OV( 181), 183 },
+  { OV( 191), 179 },
+  { OV( 201), 176 },
+  { OV( 221), 170 },
+  { OV( 241), 165 },
+  { OV( 261), 160 },
+  { OV( 281), 155 },
+  { OV( 301), 150 },
+  { OV( 331), 144 },
+  { OV( 361), 139 },
+  { OV( 391), 133 },
+  { OV( 421), 128 },
+  { OV( 451), 123 },
+  { OV( 491), 117 },
+  { OV( 531), 111 },
+  { OV( 571), 105 },
+  { OV( 611), 100 },
+  { OV( 641),  95 },
+  { OV( 681),  90 },
+  { OV( 711),  85 },
+  { OV( 751),  79 },
+  { OV( 791),  72 },
+  { OV( 811),  69 },
+  { OV( 831),  65 },
+  { OV( 871),  57 },
+  { OV( 881),  55 },
+  { OV( 901),  51 },
+  { OV( 921),  45 },
+  { OV( 941),  39 },
+  { OV( 971),  28 },
+  { OV( 981),  23 },
+  { OV( 991),  17 },
+  { OV(1001),   9 },
+  { OV(1021), -27 }
+};
diff --git a/Marlin/src/module/thermistor/thermistor_110.h b/Marlin/src/module/thermistor/thermistor_110.h
new file mode 100644
index 0000000..215495e
--- /dev/null
+++ b/Marlin/src/module/thermistor/thermistor_110.h
@@ -0,0 +1,36 @@
+/**
+ * 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
+
+#define REVERSE_TEMP_SENSOR_RANGE_110 1
+
+// Pt100 with 1k0 pullup
+const temp_entry_t temptable_110[] PROGMEM = {
+  // only a few values are needed as the curve is very flat
+  PtLine(  0, 100, 1000),
+  PtLine( 50, 100, 1000),
+  PtLine(100, 100, 1000),
+  PtLine(150, 100, 1000),
+  PtLine(200, 100, 1000),
+  PtLine(250, 100, 1000),
+  PtLine(300, 100, 1000)
+};
diff --git a/Marlin/src/module/thermistor/thermistor_12.h b/Marlin/src/module/thermistor/thermistor_12.h
new file mode 100644
index 0000000..d1ee23b
--- /dev/null
+++ b/Marlin/src/module/thermistor/thermistor_12.h
@@ -0,0 +1,56 @@
+/**
+ * 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
+
+// R25 = 100 kOhm, beta25 = 4700 K, 4.7 kOhm pull-up, (personal calibration for Makibox hot bed)
+const temp_entry_t temptable_12[] PROGMEM = {
+  { OV(  35), 180 }, // top rating 180C
+  { OV( 211), 140 },
+  { OV( 233), 135 },
+  { OV( 261), 130 },
+  { OV( 290), 125 },
+  { OV( 328), 120 },
+  { OV( 362), 115 },
+  { OV( 406), 110 },
+  { OV( 446), 105 },
+  { OV( 496), 100 },
+  { OV( 539),  95 },
+  { OV( 585),  90 },
+  { OV( 629),  85 },
+  { OV( 675),  80 },
+  { OV( 718),  75 },
+  { OV( 758),  70 },
+  { OV( 793),  65 },
+  { OV( 822),  60 },
+  { OV( 841),  55 },
+  { OV( 875),  50 },
+  { OV( 899),  45 },
+  { OV( 926),  40 },
+  { OV( 946),  35 },
+  { OV( 962),  30 },
+  { OV( 977),  25 },
+  { OV( 987),  20 },
+  { OV( 995),  15 },
+  { OV(1001),  10 },
+  { OV(1010),   0 },
+  { OV(1023), -40 }
+};
diff --git a/Marlin/src/module/thermistor/thermistor_13.h b/Marlin/src/module/thermistor/thermistor_13.h
new file mode 100644
index 0000000..bb62224
--- /dev/null
+++ b/Marlin/src/module/thermistor/thermistor_13.h
@@ -0,0 +1,49 @@
+/**
+ * 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
+
+// R25 = 100 kOhm, beta25 = 4100 K, 4.7 kOhm pull-up, Hisens thermistor
+const temp_entry_t temptable_13[] PROGMEM = {
+  { OV( 20.04), 300 },
+  { OV( 23.19), 290 },
+  { OV( 26.71), 280 },
+  { OV( 31.23), 270 },
+  { OV( 36.52), 260 },
+  { OV( 42.75), 250 },
+  { OV( 50.68), 240 },
+  { OV( 60.22), 230 },
+  { OV( 72.03), 220 },
+  { OV( 86.84), 210 },
+  { OV(102.79), 200 },
+  { OV(124.46), 190 },
+  { OV(151.02), 180 },
+  { OV(182.86), 170 },
+  { OV(220.72), 160 },
+  { OV(316.96), 140 },
+  { OV(447.17), 120 },
+  { OV(590.61), 100 },
+  { OV(737.31),  80 },
+  { OV(857.77),  60 },
+  { OV(939.52),  40 },
+  { OV(986.03),  20 },
+  { OV(1008.7),   0 }
+};
diff --git a/Marlin/src/module/thermistor/thermistor_147.h b/Marlin/src/module/thermistor/thermistor_147.h
new file mode 100644
index 0000000..6d846ad
--- /dev/null
+++ b/Marlin/src/module/thermistor/thermistor_147.h
@@ -0,0 +1,36 @@
+/**
+ * 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
+
+#define REVERSE_TEMP_SENSOR_RANGE_147 1
+
+// Pt100 with 4k7 pullup
+const temp_entry_t temptable_147[] PROGMEM = {
+  // only a few values are needed as the curve is very flat
+  PtLine(  0, 100, 4700),
+  PtLine( 50, 100, 4700),
+  PtLine(100, 100, 4700),
+  PtLine(150, 100, 4700),
+  PtLine(200, 100, 4700),
+  PtLine(250, 100, 4700),
+  PtLine(300, 100, 4700)
+};
diff --git a/Marlin/src/module/thermistor/thermistor_15.h b/Marlin/src/module/thermistor/thermistor_15.h
new file mode 100644
index 0000000..46dcce8
--- /dev/null
+++ b/Marlin/src/module/thermistor/thermistor_15.h
@@ -0,0 +1,65 @@
+/**
+ * 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
+
+ // 100k bed thermistor in JGAurora A5. Calibrated by Sam Pinches 21st Jan 2018 using cheap k-type thermocouple inserted into heater block, using TM-902C meter.
+const temp_entry_t temptable_15[] PROGMEM = {
+  { OV(  31), 275 },
+  { OV(  33), 270 },
+  { OV(  35), 260 },
+  { OV(  38), 253 },
+  { OV(  41), 248 },
+  { OV(  48), 239 },
+  { OV(  56), 232 },
+  { OV(  66), 222 },
+  { OV(  78), 212 },
+  { OV(  93), 206 },
+  { OV( 106), 199 },
+  { OV( 118), 191 },
+  { OV( 130), 186 },
+  { OV( 158), 176 },
+  { OV( 187), 167 },
+  { OV( 224), 158 },
+  { OV( 270), 148 },
+  { OV( 321), 137 },
+  { OV( 379), 127 },
+  { OV( 446), 117 },
+  { OV( 518), 106 },
+  { OV( 593),  96 },
+  { OV( 668),  86 },
+  { OV( 739),  76 },
+  { OV( 767),  72 },
+  { OV( 830),  62 },
+  { OV( 902),  48 },
+  { OV( 926),  45 },
+  { OV( 955),  35 },
+  { OV( 966),  30 },
+  { OV( 977),  25 },
+  { OV( 985),  20 },
+  { OV( 993),  15 },
+  { OV( 999),  10 },
+  { OV(1004),   5 },
+  { OV(1008),   0 },
+  { OV(1012),  -5 },
+  { OV(1016), -10 },
+  { OV(1020), -15 }
+};
diff --git a/Marlin/src/module/thermistor/thermistor_17.h b/Marlin/src/module/thermistor/thermistor_17.h
new file mode 100644
index 0000000..32b5bb7
--- /dev/null
+++ b/Marlin/src/module/thermistor/thermistor_17.h
@@ -0,0 +1,78 @@
+/**
+ * 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
+
+// Dagoma NTC 100k white thermistor
+const temp_entry_t temptable_17[] PROGMEM = {
+  { OV(  16),  309 },
+  { OV(  18),  307 },
+  { OV(  20),  300 },
+  { OV(  22),  293 },
+  { OV(  26),  284 },
+  { OV(  29),  272 },
+  { OV(  33),  266 },
+  { OV(  36),  260 },
+  { OV(  42),  252 },
+  { OV(  46),  247 },
+  { OV(  48),  244 },
+  { OV(  51),  241 },
+  { OV(  62),  231 },
+  { OV(  73),  222 },
+  { OV(  78),  219 },
+  { OV(  87),  212 },
+  { OV(  98),  207 },
+  { OV( 109),  201 },
+  { OV( 118),  197 },
+  { OV( 131),  191 },
+  { OV( 145),  186 },
+  { OV( 160),  181 },
+  { OV( 177),  175 },
+  { OV( 203),  169 },
+  { OV( 222),  164 },
+  { OV( 256),  156 },
+  { OV( 283),  151 },
+  { OV( 312),  145 },
+  { OV( 343),  140 },
+  { OV( 377),  131 },
+  { OV( 413),  125 },
+  { OV( 454),  119 },
+  { OV( 496),  113 },
+  { OV( 537),  108 },
+  { OV( 578),  102 },
+  { OV( 619),   97 },
+  { OV( 658),   92 },
+  { OV( 695),   87 },
+  { OV( 735),   81 },
+  { OV( 773),   75 },
+  { OV( 808),   70 },
+  { OV( 844),   64 },
+  { OV( 868),   59 },
+  { OV( 892),   54 },
+  { OV( 914),   49 },
+  { OV( 935),   42 },
+  { OV( 951),   38 },
+  { OV( 967),   32 },
+  { OV( 975),   28 },
+  { OV(1000),   20 },
+  { OV(1010),   10 },
+  { OV(1024), -273 } // for safety
+};
diff --git a/Marlin/src/module/thermistor/thermistor_18.h b/Marlin/src/module/thermistor/thermistor_18.h
new file mode 100644
index 0000000..9c2d81b
--- /dev/null
+++ b/Marlin/src/module/thermistor/thermistor_18.h
@@ -0,0 +1,59 @@
+/**
+ * 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
+
+// ATC Semitec 204GT-2 (4.7k pullup) Dagoma.Fr - MKS_Base_DKU001327 - version (measured/tested/approved)
+const temp_entry_t temptable_18[] PROGMEM = {
+  { OV(   1), 713 },
+  { OV(  17), 284 },
+  { OV(  20), 275 },
+  { OV(  23), 267 },
+  { OV(  27), 257 },
+  { OV(  31), 250 },
+  { OV(  37), 240 },
+  { OV(  43), 232 },
+  { OV(  51), 222 },
+  { OV(  61), 213 },
+  { OV(  73), 204 },
+  { OV(  87), 195 },
+  { OV( 106), 185 },
+  { OV( 128), 175 },
+  { OV( 155), 166 },
+  { OV( 189), 156 },
+  { OV( 230), 146 },
+  { OV( 278), 137 },
+  { OV( 336), 127 },
+  { OV( 402), 117 },
+  { OV( 476), 107 },
+  { OV( 554),  97 },
+  { OV( 635),  87 },
+  { OV( 713),  78 },
+  { OV( 784),  68 },
+  { OV( 846),  58 },
+  { OV( 897),  49 },
+  { OV( 937),  39 },
+  { OV( 966),  30 },
+  { OV( 986),  20 },
+  { OV(1000),  10 },
+  { OV(1010),   0 },
+  { OV(1024),-273 } // for safety
+};
diff --git a/Marlin/src/module/thermistor/thermistor_2.h b/Marlin/src/module/thermistor/thermistor_2.h
new file mode 100644
index 0000000..d0e1e4f
--- /dev/null
+++ b/Marlin/src/module/thermistor/thermistor_2.h
@@ -0,0 +1,62 @@
+/**
+ * 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
+
+//
+// R25 = 200 kOhm, beta25 = 4338 K, 4.7 kOhm pull-up, ATC Semitec 204GT-2
+// Verified by linagee. Source: https://www.mouser.com/datasheet/2/362/semitec%20usa%20corporation_gtthermistor-1202937.pdf
+// Calculated using 4.7kohm pullup, voltage divider math, and manufacturer provided temp/resistance
+//
+const temp_entry_t temptable_2[] PROGMEM = {
+  { OV(   1), 848 },
+  { OV(  30), 300 }, // top rating 300C
+  { OV(  34), 290 },
+  { OV(  39), 280 },
+  { OV(  46), 270 },
+  { OV(  53), 260 },
+  { OV(  63), 250 },
+  { OV(  74), 240 },
+  { OV(  87), 230 },
+  { OV( 104), 220 },
+  { OV( 124), 210 },
+  { OV( 148), 200 },
+  { OV( 176), 190 },
+  { OV( 211), 180 },
+  { OV( 252), 170 },
+  { OV( 301), 160 },
+  { OV( 357), 150 },
+  { OV( 420), 140 },
+  { OV( 489), 130 },
+  { OV( 562), 120 },
+  { OV( 636), 110 },
+  { OV( 708), 100 },
+  { OV( 775),  90 },
+  { OV( 835),  80 },
+  { OV( 884),  70 },
+  { OV( 924),  60 },
+  { OV( 955),  50 },
+  { OV( 977),  40 },
+  { OV( 993),  30 },
+  { OV(1004),  20 },
+  { OV(1012),  10 },
+  { OV(1016),   0 }
+};
diff --git a/Marlin/src/module/thermistor/thermistor_20.h b/Marlin/src/module/thermistor/thermistor_20.h
new file mode 100644
index 0000000..73094f1
--- /dev/null
+++ b/Marlin/src/module/thermistor/thermistor_20.h
@@ -0,0 +1,77 @@
+/**
+ * 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
+
+#define REVERSE_TEMP_SENSOR_RANGE_20 1
+
+// Pt100 with INA826 amp on Ultimaker v2.0 electronics
+const temp_entry_t temptable_20[] PROGMEM = {
+  { OV(  0),    0 },
+  { OV(227),    1 },
+  { OV(236),   10 },
+  { OV(245),   20 },
+  { OV(253),   30 },
+  { OV(262),   40 },
+  { OV(270),   50 },
+  { OV(279),   60 },
+  { OV(287),   70 },
+  { OV(295),   80 },
+  { OV(304),   90 },
+  { OV(312),  100 },
+  { OV(320),  110 },
+  { OV(329),  120 },
+  { OV(337),  130 },
+  { OV(345),  140 },
+  { OV(353),  150 },
+  { OV(361),  160 },
+  { OV(369),  170 },
+  { OV(377),  180 },
+  { OV(385),  190 },
+  { OV(393),  200 },
+  { OV(401),  210 },
+  { OV(409),  220 },
+  { OV(417),  230 },
+  { OV(424),  240 },
+  { OV(432),  250 },
+  { OV(440),  260 },
+  { OV(447),  270 },
+  { OV(455),  280 },
+  { OV(463),  290 },
+  { OV(470),  300 },
+  { OV(478),  310 },
+  { OV(485),  320 },
+  { OV(493),  330 },
+  { OV(500),  340 },
+  { OV(507),  350 },
+  { OV(515),  360 },
+  { OV(522),  370 },
+  { OV(529),  380 },
+  { OV(537),  390 },
+  { OV(544),  400 },
+  { OV(614),  500 },
+  { OV(681),  600 },
+  { OV(744),  700 },
+  { OV(805),  800 },
+  { OV(862),  900 },
+  { OV(917), 1000 },
+  { OV(968), 1100 }
+};
diff --git a/Marlin/src/module/thermistor/thermistor_201.h b/Marlin/src/module/thermistor/thermistor_201.h
new file mode 100644
index 0000000..44d5280
--- /dev/null
+++ b/Marlin/src/module/thermistor/thermistor_201.h
@@ -0,0 +1,57 @@
+/**
+ * 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
+
+#define REVERSE_TEMP_SENSOR_RANGE_201 1
+
+// Pt100 with LMV324 amp on Overlord v1.1 electronics
+const temp_entry_t temptable_201[] PROGMEM = {
+  { OV(   0),   0 },
+  { OV(   8),   1 },
+  { OV(  23),   6 },
+  { OV(  41),  15 },
+  { OV(  51),  20 },
+  { OV(  68),  28 },
+  { OV(  74),  30 },
+  { OV(  88),  35 },
+  { OV(  99),  40 },
+  { OV( 123),  50 },
+  { OV( 148),  60 },
+  { OV( 173),  70 },
+  { OV( 198),  80 },
+  { OV( 221),  90 },
+  { OV( 245), 100 },
+  { OV( 269), 110 },
+  { OV( 294), 120 },
+  { OV( 316), 130 },
+  { OV( 342), 140 },
+  { OV( 364), 150 },
+  { OV( 387), 160 },
+  { OV( 412), 170 },
+  { OV( 433), 180 },
+  { OV( 456), 190 },
+  { OV( 480), 200 },
+  { OV( 500), 210 },
+  { OV( 548), 224 },
+  { OV( 572), 233 },
+  { OV(1155), 490 }
+};
diff --git a/Marlin/src/module/thermistor/thermistor_202.h b/Marlin/src/module/thermistor/thermistor_202.h
new file mode 100644
index 0000000..c522960
--- /dev/null
+++ b/Marlin/src/module/thermistor/thermistor_202.h
@@ -0,0 +1,69 @@
+//
+// Unknown 200K thermistor on a Copymaster 3D hotend
+// Temptable sent from dealer technologyoutlet.co.uk
+//
+
+const temp_entry_t temptable_202[] PROGMEM = {
+  { OV(   1), 864 },
+  { OV(  35), 300 },
+  { OV(  38), 295 },
+  { OV(  41), 290 },
+  { OV(  44), 285 },
+  { OV(  47), 280 },
+  { OV(  51), 275 },
+  { OV(  55), 270 },
+  { OV(  60), 265 },
+  { OV(  65), 260 },
+  { OV(  70), 255 },
+  { OV(  76), 250 },
+  { OV(  83), 245 },
+  { OV(  90), 240 },
+  { OV(  98), 235 },
+  { OV( 107), 230 },
+  { OV( 116), 225 },
+  { OV( 127), 220 },
+  { OV( 138), 215 },
+  { OV( 151), 210 },
+  { OV( 164), 205 },
+  { OV( 179), 200 },
+  { OV( 195), 195 },
+  { OV( 213), 190 },
+  { OV( 232), 185 },
+  { OV( 253), 180 },
+  { OV( 275), 175 },
+  { OV( 299), 170 },
+  { OV( 325), 165 },
+  { OV( 352), 160 },
+  { OV( 381), 155 },
+  { OV( 411), 150 },
+  { OV( 443), 145 },
+  { OV( 476), 140 },
+  { OV( 511), 135 },
+  { OV( 546), 130 },
+  { OV( 581), 125 },
+  { OV( 617), 120 },
+  { OV( 652), 115 },
+  { OV( 687), 110 },
+  { OV( 720), 105 },
+  { OV( 753), 100 },
+  { OV( 783),  95 },
+  { OV( 812),  90 },
+  { OV( 839),  85 },
+  { OV( 864),  80 },
+  { OV( 886),  75 },
+  { OV( 906),  70 },
+  { OV( 924),  65 },
+  { OV( 940),  60 },
+  { OV( 954),  55 },
+  { OV( 966),  50 },
+  { OV( 976),  45 },
+  { OV( 985),  40 },
+  { OV( 992),  35 },
+  { OV( 998),  30 },
+  { OV(1003),  25 },
+  { OV(1007),  20 },
+  { OV(1011),  15 },
+  { OV(1014),  10 },
+  { OV(1016),   5 },
+  { OV(1018),   0 }
+};
diff --git a/Marlin/src/module/thermistor/thermistor_21.h b/Marlin/src/module/thermistor/thermistor_21.h
new file mode 100644
index 0000000..2ca705b
--- /dev/null
+++ b/Marlin/src/module/thermistor/thermistor_21.h
@@ -0,0 +1,78 @@
+/**
+ * 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
+
+#define REVERSE_TEMP_SENSOR_RANGE_21 1
+
+#undef OV_SCALE
+#define OV_SCALE(N) (float((N) * 5) / 3.3f)
+
+// Pt100 with INA826 amplifier board with 5v supply based on Thermistor 20, with 3v3 ADC reference on the mainboard.
+// If the ADC reference and INA826 board supply voltage are identical, Thermistor 20 instead.
+const temp_entry_t temptable_21[] PROGMEM = {
+  { OV(  0),    0 },
+  { OV(227),    1 },
+  { OV(236),   10 },
+  { OV(245),   20 },
+  { OV(253),   30 },
+  { OV(262),   40 },
+  { OV(270),   50 },
+  { OV(279),   60 },
+  { OV(287),   70 },
+  { OV(295),   80 },
+  { OV(304),   90 },
+  { OV(312),  100 },
+  { OV(320),  110 },
+  { OV(329),  120 },
+  { OV(337),  130 },
+  { OV(345),  140 },
+  { OV(353),  150 },
+  { OV(361),  160 },
+  { OV(369),  170 },
+  { OV(377),  180 },
+  { OV(385),  190 },
+  { OV(393),  200 },
+  { OV(401),  210 },
+  { OV(409),  220 },
+  { OV(417),  230 },
+  { OV(424),  240 },
+  { OV(432),  250 },
+  { OV(440),  260 },
+  { OV(447),  270 },
+  { OV(455),  280 },
+  { OV(463),  290 },
+  { OV(470),  300 },
+  { OV(478),  310 },
+  { OV(485),  320 },
+  { OV(493),  330 },
+  { OV(500),  340 },
+  { OV(507),  350 },
+  { OV(515),  360 },
+  { OV(522),  370 },
+  { OV(529),  380 },
+  { OV(537),  390 },
+  { OV(544),  400 },
+  { OV(614),  500 }
+};
+
+#undef OV_SCALE
+#define OV_SCALE(N) (N)
diff --git a/Marlin/src/module/thermistor/thermistor_22.h b/Marlin/src/module/thermistor/thermistor_22.h
new file mode 100644
index 0000000..6f4a310
--- /dev/null
+++ b/Marlin/src/module/thermistor/thermistor_22.h
@@ -0,0 +1,72 @@
+/**
+ * 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/>.
+ *
+ */
+
+// 100k hotend thermistor with 4.7k pull up to 3.3v and 220R to analog input as in GTM32 Pro vB
+const temp_entry_t temptable_22[] PROGMEM = {
+  { OV(   1), 352 },
+  { OV(   6), 341 },
+  { OV(  11), 330 },
+  { OV(  16), 319 },
+  { OV(  20), 307 },
+  { OV(  26), 296 },
+  { OV(  31), 285 },
+  { OV(  40), 274 },
+  { OV(  51), 263 },
+  { OV(  61), 251 },
+  { OV(  72), 245 },
+  { OV(  77), 240 },
+  { OV(  82), 237 },
+  { OV(  87), 232 },
+  { OV(  91), 229 },
+  { OV(  94), 227 },
+  { OV(  97), 225 },
+  { OV( 100), 223 },
+  { OV( 104), 221 },
+  { OV( 108), 219 },
+  { OV( 115), 214 },
+  { OV( 126), 209 },
+  { OV( 137), 204 },
+  { OV( 147), 200 },
+  { OV( 158), 193 },
+  { OV( 167), 192 },
+  { OV( 177), 189 },
+  { OV( 197), 163 },
+  { OV( 230), 174 },
+  { OV( 267), 165 },
+  { OV( 310), 158 },
+  { OV( 336), 151 },
+  { OV( 379), 143 },
+  { OV( 413), 138 },
+  { OV( 480), 127 },
+  { OV( 580), 110 },
+  { OV( 646), 100 },
+  { OV( 731),  88 },
+  { OV( 768),  84 },
+  { OV( 861),  69 },
+  { OV( 935),  50 },
+  { OV( 975),  38 },
+  { OV(1001),  27 },
+  { OV(1011),  22 },
+  { OV(1015),  13 },
+  { OV(1020),   6 },
+  { OV(1023),   0 }
+};
diff --git a/Marlin/src/module/thermistor/thermistor_23.h b/Marlin/src/module/thermistor/thermistor_23.h
new file mode 100644
index 0000000..02ff9fb
--- /dev/null
+++ b/Marlin/src/module/thermistor/thermistor_23.h
@@ -0,0 +1,128 @@
+/**
+ * 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/>.
+ *
+ */
+
+// 100k hotbed thermistor with 4.7k pull up to 3.3v and 220R to analog input as in GTM32 Pro vB
+const temp_entry_t temptable_23[] PROGMEM = {
+  { OV(   1), 938 },
+  { OV(  11), 423 },
+  { OV(  21), 351 },
+  { OV(  31), 314 },
+  { OV(  41), 290 },
+  { OV(  51), 272 },
+  { OV(  61), 258 },
+  { OV(  71), 247 },
+  { OV(  81), 237 },
+  { OV(  91), 229 },
+  { OV( 101), 221 },
+  { OV( 111), 215 },
+  { OV( 121), 209 },
+  { OV( 131), 204 },
+  { OV( 141), 199 },
+  { OV( 151), 195 },
+  { OV( 161), 190 },
+  { OV( 171), 187 },
+  { OV( 181), 183 },
+  { OV( 191), 179 },
+  { OV( 201), 176 },
+  { OV( 211), 173 },
+  { OV( 221), 170 },
+  { OV( 231), 167 },
+  { OV( 241), 165 },
+  { OV( 251), 162 },
+  { OV( 261), 160 },
+  { OV( 271), 157 },
+  { OV( 281), 155 },
+  { OV( 291), 153 },
+  { OV( 301), 150 },
+  { OV( 311), 148 },
+  { OV( 321), 146 },
+  { OV( 331), 144 },
+  { OV( 341), 142 },
+  { OV( 351), 140 },
+  { OV( 361), 139 },
+  { OV( 371), 137 },
+  { OV( 381), 135 },
+  { OV( 391), 133 },
+  { OV( 401), 131 },
+  { OV( 411), 130 },
+  { OV( 421), 128 },
+  { OV( 431), 126 },
+  { OV( 441), 125 },
+  { OV( 451), 123 },
+  { OV( 461), 122 },
+  { OV( 471), 120 },
+  { OV( 481), 119 },
+  { OV( 491), 117 },
+  { OV( 501), 116 },
+  { OV( 511), 114 },
+  { OV( 521), 113 },
+  { OV( 531), 111 },
+  { OV( 541), 110 },
+  { OV( 551), 108 },
+  { OV( 561), 107 },
+  { OV( 571), 105 },
+  { OV( 581), 104 },
+  { OV( 591), 102 },
+  { OV( 601), 101 },
+  { OV( 611), 100 },
+  { OV( 621),  98 },
+  { OV( 631),  97 },
+  { OV( 641),  95 },
+  { OV( 651),  94 },
+  { OV( 661),  92 },
+  { OV( 671),  91 },
+  { OV( 681),  90 },
+  { OV( 691),  88 },
+  { OV( 701),  87 },
+  { OV( 711),  85 },
+  { OV( 721),  84 },
+  { OV( 731),  82 },
+  { OV( 741),  81 },
+  { OV( 751),  79 },
+  { OV( 761),  77 },
+  { OV( 771),  76 },
+  { OV( 781),  74 },
+  { OV( 791),  72 },
+  { OV( 801),  71 },
+  { OV( 811),  69 },
+  { OV( 821),  67 },
+  { OV( 831),  65 },
+  { OV( 841),  63 },
+  { OV( 851),  62 },
+  { OV( 861),  60 },
+  { OV( 871),  57 },
+  { OV( 881),  55 },
+  { OV( 891),  53 },
+  { OV( 901),  51 },
+  { OV( 911),  48 },
+  { OV( 921),  45 },
+  { OV( 931),  42 },
+  { OV( 941),  39 },
+  { OV( 951),  36 },
+  { OV( 961),  32 },
+  { OV( 971),  28 },
+  { OV( 981),  25 },
+  { OV( 991),  23 },
+  { OV(1001),  21 },
+  { OV(1011),  19 },
+  { OV(1021),   5 }
+};
diff --git a/Marlin/src/module/thermistor/thermistor_3.h b/Marlin/src/module/thermistor/thermistor_3.h
new file mode 100644
index 0000000..74e00e2
--- /dev/null
+++ b/Marlin/src/module/thermistor/thermistor_3.h
@@ -0,0 +1,54 @@
+/**
+ * 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
+
+// R25 = 100 kOhm, beta25 = 4120 K, 4.7 kOhm pull-up, mendel-parts
+const temp_entry_t temptable_3[] PROGMEM = {
+  { OV(   1), 864 },
+  { OV(  21), 300 },
+  { OV(  25), 290 },
+  { OV(  29), 280 },
+  { OV(  33), 270 },
+  { OV(  39), 260 },
+  { OV(  46), 250 },
+  { OV(  54), 240 },
+  { OV(  64), 230 },
+  { OV(  75), 220 },
+  { OV(  90), 210 },
+  { OV( 107), 200 },
+  { OV( 128), 190 },
+  { OV( 154), 180 },
+  { OV( 184), 170 },
+  { OV( 221), 160 },
+  { OV( 265), 150 },
+  { OV( 316), 140 },
+  { OV( 375), 130 },
+  { OV( 441), 120 },
+  { OV( 513), 110 },
+  { OV( 588), 100 },
+  { OV( 734),  80 },
+  { OV( 856),  60 },
+  { OV( 938),  40 },
+  { OV( 986),  20 },
+  { OV(1008),   0 },
+  { OV(1018), -20 }
+};
diff --git a/Marlin/src/module/thermistor/thermistor_30.h b/Marlin/src/module/thermistor/thermistor_30.h
new file mode 100644
index 0000000..bc1781b
--- /dev/null
+++ b/Marlin/src/module/thermistor/thermistor_30.h
@@ -0,0 +1,66 @@
+/**
+ * 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
+
+// R25 = 100 kOhm, beta25 = 3950 K, 4.7 kOhm pull-up
+// Resistance         100k Ohms at 25deg. C
+// Resistance Tolerance     + / -1%
+// B Value             3950K at 25/50 deg. C
+// B Value Tolerance         + / - 1%
+// Kis3d Silicone Heater 24V 200W/300W with 6mm Precision cast plate (EN AW 5083)
+// Temperature setting time 10 min to determine the 12Bit ADC value on the surface. (le3tspeak)
+const temp_entry_t temptable_30[] PROGMEM = {
+  { OV(   1), 938 },
+  { OV( 298), 125 }, // 1193 - 125°
+  { OV( 321), 121 }, // 1285 - 121°
+  { OV( 348), 117 }, // 1392 - 117°
+  { OV( 387), 113 }, // 1550 - 113°
+  { OV( 411), 110 }, // 1644 - 110°
+  { OV( 445), 106 }, // 1780 - 106°
+  { OV( 480), 101 }, // 1920 - 101°
+  { OV( 516),  97 }, // 2064 - 97°
+  { OV( 553),  92 }, // 2212 - 92°
+  { OV( 591),  88 }, // 2364 - 88°
+  { OV( 628),  84 }, // 2512 - 84°
+  { OV( 665),  79 }, // 2660 - 79°
+  { OV( 702),  75 }, // 2808 - 75°
+  { OV( 736),  71 }, // 2945 - 71°
+  { OV( 770),  67 }, // 3080 - 67°
+  { OV( 801),  63 }, // 3204 - 63°
+  { OV( 830),  59 }, // 3320 - 59°
+  { OV( 857),  55 }, // 3428 - 55°
+  { OV( 881),  51 }, // 3524 - 51°
+  { OV( 902),  47 }, // 3611 - 47°
+  { OV( 922),  42 }, // 3688 - 42°
+  { OV( 938),  38 }, // 3754 - 38°
+  { OV( 952),  34 }, // 3811 - 34°
+  { OV( 964),  29 }, // 3857 - 29°
+  { OV( 975),  25 }, // 3900 - 25°
+  { OV( 980),  23 }, // 3920 - 23°
+  { OV( 991),  17 }, // 3964 - 17°
+  { OV(1001),   9 }, // Calculated
+  { OV(1004),   5 }, // Calculated
+  { OV(1008),   0 }, // Calculated
+  { OV(1012),  -5 }, // Calculated
+  { OV(1016), -10 }, // Calculated
+  { OV(1020), -15 }  // Calculated
+};
diff --git a/Marlin/src/module/thermistor/thermistor_331.h b/Marlin/src/module/thermistor/thermistor_331.h
new file mode 100644
index 0000000..ccb0f6b
--- /dev/null
+++ b/Marlin/src/module/thermistor/thermistor_331.h
@@ -0,0 +1,92 @@
+/**
+ * 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
+
+#define OVM(V) OV((V)*(0.327/0.5))
+
+// R25 = 100 kOhm, beta25 = 4092 K, 4.7 kOhm pull-up, bed thermistor
+const temp_entry_t temptable_331[] PROGMEM = {
+  { OVM(  23), 300 },
+  { OVM(  25), 295 },
+  { OVM(  27), 290 },
+  { OVM(  28), 285 },
+  { OVM(  31), 280 },
+  { OVM(  33), 275 },
+  { OVM(  35), 270 },
+  { OVM(  38), 265 },
+  { OVM(  41), 260 },
+  { OVM(  44), 255 },
+  { OVM(  48), 250 },
+  { OVM(  52), 245 },
+  { OVM(  56), 240 },
+  { OVM(  61), 235 },
+  { OVM(  66), 230 },
+  { OVM(  71), 225 },
+  { OVM(  78), 220 },
+  { OVM(  84), 215 },
+  { OVM(  92), 210 },
+  { OVM( 100), 205 },
+  { OVM( 109), 200 },
+  { OVM( 120), 195 },
+  { OVM( 131), 190 },
+  { OVM( 143), 185 },
+  { OVM( 156), 180 },
+  { OVM( 171), 175 },
+  { OVM( 187), 170 },
+  { OVM( 205), 165 },
+  { OVM( 224), 160 },
+  { OVM( 245), 155 },
+  { OVM( 268), 150 },
+  { OVM( 293), 145 },
+  { OVM( 320), 140 },
+  { OVM( 348), 135 },
+  { OVM( 379), 130 },
+  { OVM( 411), 125 },
+  { OVM( 445), 120 },
+  { OVM( 480), 115 },
+  { OVM( 516), 110 },
+  { OVM( 553), 105 },
+  { OVM( 591), 100 },
+  { OVM( 628),  95 },
+  { OVM( 665),  90 },
+  { OVM( 702),  85 },
+  { OVM( 737),  80 },
+  { OVM( 770),  75 },
+  { OVM( 801),  70 },
+  { OVM( 830),  65 },
+  { OVM( 857),  60 },
+  { OVM( 881),  55 },
+  { OVM( 903),  50 },
+  { OVM( 922),  45 },
+  { OVM( 939),  40 },
+  { OVM( 954),  35 },
+  { OVM( 966),  30 },
+  { OVM( 977),  25 },
+  { OVM( 985),  20 },
+  { OVM( 993),  15 },
+  { OVM( 999),  10 },
+  { OVM(1004),   5 },
+  { OVM(1008),   0 },
+  { OVM(1012),  -5 },
+  { OVM(1016), -10 },
+  { OVM(1020), -15 }
+};
diff --git a/Marlin/src/module/thermistor/thermistor_332.h b/Marlin/src/module/thermistor/thermistor_332.h
new file mode 100644
index 0000000..9502f1b
--- /dev/null
+++ b/Marlin/src/module/thermistor/thermistor_332.h
@@ -0,0 +1,50 @@
+/**
+ * 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
+
+#define OVM(V) OV((V)*(0.327/0.327))
+
+// R25 = 100 kOhm, beta25 = 4092 K, 4.7 kOhm pull-up, bed thermistor
+const temp_entry_t temptable_332[] PROGMEM = {
+  { OVM( 268), 150 },
+  { OVM( 293), 145 },
+  { OVM( 320), 141 },
+  { OVM( 379), 133 },
+  { OVM( 445), 122 },
+  { OVM( 516), 108 },
+  { OVM( 591),  98 },
+  { OVM( 665),  88 },
+  { OVM( 737),  79 },
+  { OVM( 801),  70 },
+  { OVM( 857),  55 },
+  { OVM( 903),  46 },
+  { OVM( 939),  39 },
+  { OVM( 954),  33 },
+  { OVM( 966),  27 },
+  { OVM( 977),  22 },
+  { OVM( 999),  15 },
+  { OVM(1004),   5 },
+  { OVM(1008),   0 },
+  { OVM(1012),  -5 },
+  { OVM(1016), -10 },
+  { OVM(1020), -15 }
+};
diff --git a/Marlin/src/module/thermistor/thermistor_4.h b/Marlin/src/module/thermistor/thermistor_4.h
new file mode 100644
index 0000000..92d9072
--- /dev/null
+++ b/Marlin/src/module/thermistor/thermistor_4.h
@@ -0,0 +1,46 @@
+/**
+ * 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
+
+// R25 = 10 kOhm, beta25 = 3950 K, 4.7 kOhm pull-up, Generic 10k thermistor
+const temp_entry_t temptable_4[] PROGMEM = {
+  { OV(   1), 430 },
+  { OV(  54), 137 },
+  { OV( 107), 107 },
+  { OV( 160),  91 },
+  { OV( 213),  80 },
+  { OV( 266),  71 },
+  { OV( 319),  64 },
+  { OV( 372),  57 },
+  { OV( 425),  51 },
+  { OV( 478),  46 },
+  { OV( 531),  41 },
+  { OV( 584),  35 },
+  { OV( 637),  30 },
+  { OV( 690),  25 },
+  { OV( 743),  20 },
+  { OV( 796),  14 },
+  { OV( 849),   7 },
+  { OV( 902),   0 },
+  { OV( 955), -11 },
+  { OV(1008), -35 }
+};
diff --git a/Marlin/src/module/thermistor/thermistor_5.h b/Marlin/src/module/thermistor/thermistor_5.h
new file mode 100644
index 0000000..1d5fa2f
--- /dev/null
+++ b/Marlin/src/module/thermistor/thermistor_5.h
@@ -0,0 +1,62 @@
+/**
+ * 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
+
+// R25 = 100 kOhm, beta25 = 4267 K, 4.7 kOhm pull-up
+// 100k ParCan thermistor (104GT-2)
+// ATC Semitec 104GT-2/104NT-4-R025H42G (Used in ParCan)
+// Verified by linagee. Source: https://www.mouser.com/datasheet/2/362/semitec%20usa%20corporation_gtthermistor-1202937.pdf
+// Calculated using 4.7kohm pullup, voltage divider math, and manufacturer provided temp/resistance
+const temp_entry_t temptable_5[] PROGMEM = {
+  { OV(   1), 713 },
+  { OV(  17), 300 }, // top rating 300C
+  { OV(  20), 290 },
+  { OV(  23), 280 },
+  { OV(  27), 270 },
+  { OV(  31), 260 },
+  { OV(  37), 250 },
+  { OV(  43), 240 },
+  { OV(  51), 230 },
+  { OV(  61), 220 },
+  { OV(  73), 210 },
+  { OV(  87), 200 },
+  { OV( 106), 190 },
+  { OV( 128), 180 },
+  { OV( 155), 170 },
+  { OV( 189), 160 },
+  { OV( 230), 150 },
+  { OV( 278), 140 },
+  { OV( 336), 130 },
+  { OV( 402), 120 },
+  { OV( 476), 110 },
+  { OV( 554), 100 },
+  { OV( 635),  90 },
+  { OV( 713),  80 },
+  { OV( 784),  70 },
+  { OV( 846),  60 },
+  { OV( 897),  50 },
+  { OV( 937),  40 },
+  { OV( 966),  30 },
+  { OV( 986),  20 },
+  { OV(1000),  10 },
+  { OV(1010),   0 }
+};
diff --git a/Marlin/src/module/thermistor/thermistor_501.h b/Marlin/src/module/thermistor/thermistor_501.h
new file mode 100644
index 0000000..d40e341
--- /dev/null
+++ b/Marlin/src/module/thermistor/thermistor_501.h
@@ -0,0 +1,58 @@
+/**
+ * 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
+
+// 100k Zonestar thermistor. Adjusted By Hally
+const temp_entry_t temptable_501[] PROGMEM = {
+   { OV(   1), 713 },
+   { OV(  14), 300 }, // Top rating 300C
+   { OV(  16), 290 },
+   { OV(  19), 280 },
+   { OV(  23), 270 },
+   { OV(  27), 260 },
+   { OV(  31), 250 },
+   { OV(  37), 240 },
+   { OV(  47), 230 },
+   { OV(  57), 220 },
+   { OV(  68), 210 },
+   { OV(  84), 200 },
+   { OV( 100), 190 },
+   { OV( 128), 180 },
+   { OV( 155), 170 },
+   { OV( 189), 160 },
+   { OV( 230), 150 },
+   { OV( 278), 140 },
+   { OV( 336), 130 },
+   { OV( 402), 120 },
+   { OV( 476), 110 },
+   { OV( 554), 100 },
+   { OV( 635),  90 },
+   { OV( 713),  80 },
+   { OV( 784),  70 },
+   { OV( 846),  60 },
+   { OV( 897),  50 },
+   { OV( 937),  40 },
+   { OV( 966),  30 },
+   { OV( 986),  20 },
+   { OV(1000),  10 },
+   { OV(1010),   0 }
+};
diff --git a/Marlin/src/module/thermistor/thermistor_502.h b/Marlin/src/module/thermistor/thermistor_502.h
new file mode 100644
index 0000000..69cee24
--- /dev/null
+++ b/Marlin/src/module/thermistor/thermistor_502.h
@@ -0,0 +1,60 @@
+/**
+ * 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
+
+// Unknown thermistor for the Zonestar P802M hot bed. Adjusted By Nerseth
+// These were the shipped settings from Zonestar in original firmware: P802M_8_Repetier_V1.6_Zonestar.zip
+const temp_entry_t temptable_502[] PROGMEM = {
+   { OV(  56.0 / 4), 300 },
+   { OV( 187.0 / 4), 250 },
+   { OV( 615.0 / 4), 190 },
+   { OV( 690.0 / 4), 185 },
+   { OV( 750.0 / 4), 180 },
+   { OV( 830.0 / 4), 175 },
+   { OV( 920.0 / 4), 170 },
+   { OV(1010.0 / 4), 165 },
+   { OV(1118.0 / 4), 160 },
+   { OV(1215.0 / 4), 155 },
+   { OV(1330.0 / 4), 145 },
+   { OV(1460.0 / 4), 140 },
+   { OV(1594.0 / 4), 135 },
+   { OV(1752.0 / 4), 130 },
+   { OV(1900.0 / 4), 125 },
+   { OV(2040.0 / 4), 120 },
+   { OV(2200.0 / 4), 115 },
+   { OV(2350.0 / 4), 110 },
+   { OV(2516.0 / 4), 105 },
+   { OV(2671.0 / 4),  98 },
+   { OV(2831.0 / 4),  92 },
+   { OV(2975.0 / 4),  85 },
+   { OV(3115.0 / 4),  76 },
+   { OV(3251.0 / 4),  72 },
+   { OV(3480.0 / 4),  62 },
+   { OV(3580.0 / 4),  52 },
+   { OV(3660.0 / 4),  46 },
+   { OV(3740.0 / 4),  40 },
+   { OV(3869.0 / 4),  30 },
+   { OV(3912.0 / 4),  25 },
+   { OV(3948.0 / 4),  20 },
+   { OV(4077.0 / 4), -20 },
+   { OV(4094.0 / 4), -55 }
+};
diff --git a/Marlin/src/module/thermistor/thermistor_503.h b/Marlin/src/module/thermistor/thermistor_503.h
new file mode 100644
index 0000000..fc4bfff
--- /dev/null
+++ b/Marlin/src/module/thermistor/thermistor_503.h
@@ -0,0 +1,57 @@
+/**
+ * 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
+
+// Zonestar (Z8XM2) Heated Bed thermistor. Added By AvanOsch
+// These are taken from the Zonestar settings in original Repetier firmware: Z8XM2_ZRIB_LCD12864_V51.zip
+const temp_entry_t temptable_503[] PROGMEM = {
+   { OV(  12), 300 },
+   { OV(  27), 270 },
+   { OV(  47), 250 },
+   { OV(  68), 230 },
+   { OV(  99), 210 },
+   { OV( 120), 200 },
+   { OV( 141), 190 },
+   { OV( 171), 180 },
+   { OV( 201), 170 },
+   { OV( 261), 160 },
+   { OV( 321), 150 },
+   { OV( 401), 140 },
+   { OV( 451), 130 },
+   { OV( 551), 120 },
+   { OV( 596), 110 },
+   { OV( 626), 105 },
+   { OV( 666), 100 },
+   { OV( 697), 90 },
+   { OV( 717), 85 },
+   { OV( 798), 69 },
+   { OV( 819), 65 },
+   { OV( 870), 55 },
+   { OV( 891), 51 },
+   { OV( 922), 39 },
+   { OV( 968), 28 },
+   { OV( 980), 23 },
+   { OV( 991), 17 },
+   { OV( 1001), 9 },
+   { OV(1021), -27 },
+   { OV(1023), -200}
+};
diff --git a/Marlin/src/module/thermistor/thermistor_51.h b/Marlin/src/module/thermistor/thermistor_51.h
new file mode 100644
index 0000000..d02dd47
--- /dev/null
+++ b/Marlin/src/module/thermistor/thermistor_51.h
@@ -0,0 +1,83 @@
+/**
+ * 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
+
+// R25 = 100 kOhm, beta25 = 4092 K, 1 kOhm pull-up,
+// 100k EPCOS (WITH 1kohm RESISTOR FOR PULLUP, R9 ON SANGUINOLOLU! NOT FOR 4.7kohm PULLUP! THIS IS NOT NORMAL!)
+// Verified by linagee.
+// Calculated using 1kohm pullup, voltage divider math, and manufacturer provided temp/resistance
+// Advantage: Twice the resolution and better linearity from 150C to 200C
+const temp_entry_t temptable_51[] PROGMEM = {
+  { OV(   1), 350 },
+  { OV( 190), 250 }, // top rating 250C
+  { OV( 203), 245 },
+  { OV( 217), 240 },
+  { OV( 232), 235 },
+  { OV( 248), 230 },
+  { OV( 265), 225 },
+  { OV( 283), 220 },
+  { OV( 302), 215 },
+  { OV( 322), 210 },
+  { OV( 344), 205 },
+  { OV( 366), 200 },
+  { OV( 390), 195 },
+  { OV( 415), 190 },
+  { OV( 440), 185 },
+  { OV( 467), 180 },
+  { OV( 494), 175 },
+  { OV( 522), 170 },
+  { OV( 551), 165 },
+  { OV( 580), 160 },
+  { OV( 609), 155 },
+  { OV( 638), 150 },
+  { OV( 666), 145 },
+  { OV( 695), 140 },
+  { OV( 722), 135 },
+  { OV( 749), 130 },
+  { OV( 775), 125 },
+  { OV( 800), 120 },
+  { OV( 823), 115 },
+  { OV( 845), 110 },
+  { OV( 865), 105 },
+  { OV( 884), 100 },
+  { OV( 901),  95 },
+  { OV( 917),  90 },
+  { OV( 932),  85 },
+  { OV( 944),  80 },
+  { OV( 956),  75 },
+  { OV( 966),  70 },
+  { OV( 975),  65 },
+  { OV( 982),  60 },
+  { OV( 989),  55 },
+  { OV( 995),  50 },
+  { OV(1000),  45 },
+  { OV(1004),  40 },
+  { OV(1007),  35 },
+  { OV(1010),  30 },
+  { OV(1013),  25 },
+  { OV(1015),  20 },
+  { OV(1017),  15 },
+  { OV(1018),  10 },
+  { OV(1019),   5 },
+  { OV(1020),   0 },
+  { OV(1021),  -5 }
+};
diff --git a/Marlin/src/module/thermistor/thermistor_512.h b/Marlin/src/module/thermistor/thermistor_512.h
new file mode 100644
index 0000000..e8021e1
--- /dev/null
+++ b/Marlin/src/module/thermistor/thermistor_512.h
@@ -0,0 +1,87 @@
+/**
+ * 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/>.
+ *
+ */
+
+// 100k thermistor supplied with RPW-Ultra hotend, 4.7k pullup
+
+const temp_entry_t temptable_512[] PROGMEM = {
+  { OV(26),  300 },
+  { OV(28),  295 },
+  { OV(30),  290 },
+  { OV(32),  285 },
+  { OV(34),  280 },
+  { OV(37),  275 },
+  { OV(39),  270 },
+  { OV(42),  265 },
+  { OV(46),  260 },
+  { OV(49),  255 },
+  { OV(53),  250 }, // 256.5
+  { OV(57),  245 },
+  { OV(62),  240 },
+  { OV(67),  235 },
+  { OV(73),  230 },
+  { OV(79),  225 },
+  { OV(86),  220 },
+  { OV(94),  215 },
+  { OV(103), 210 },
+  { OV(112), 205 },
+  { OV(123), 200 },
+  { OV(135), 195 },
+  { OV(148), 190 },
+  { OV(162), 185 },
+  { OV(178), 180 },
+  { OV(195), 175 },
+  { OV(215), 170 },
+  { OV(235), 165 },
+  { OV(258), 160 },
+  { OV(283), 155 },
+  { OV(310), 150 }, // 2040.6
+  { OV(338), 145 },
+  { OV(369), 140 },
+  { OV(401), 135 },
+  { OV(435), 130 },
+  { OV(470), 125 },
+  { OV(505), 120 },
+  { OV(542), 115 },
+  { OV(579), 110 },
+  { OV(615), 105 },
+  { OV(651), 100 },
+  { OV(686),  95 },
+  { OV(720),  90 },
+  { OV(751),  85 },
+  { OV(781),  80 },
+  { OV(809),  75 },
+  { OV(835),  70 },
+  { OV(858),  65 },
+  { OV(880),  60 },
+  { OV(899),  55 },
+  { OV(915),  50 },
+  { OV(930),  45 },
+  { OV(944),  40 },
+  { OV(955),  35 },
+  { OV(965),  30 }, // 78279.3
+  { OV(974),  25 },
+  { OV(981),  20 },
+  { OV(988),  15 },
+  { OV(993),  10 },
+  { OV(998),   5 },
+  { OV(1002),  0 },
+};
diff --git a/Marlin/src/module/thermistor/thermistor_52.h b/Marlin/src/module/thermistor/thermistor_52.h
new file mode 100644
index 0000000..5c9cb9d
--- /dev/null
+++ b/Marlin/src/module/thermistor/thermistor_52.h
@@ -0,0 +1,62 @@
+/**
+ * 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
+
+// R25 = 200 kOhm, beta25 = 4338 K, 1 kOhm pull-up,
+// 200k ATC Semitec 204GT-2 (WITH 1kohm RESISTOR FOR PULLUP, R9 ON SANGUINOLOLU! NOT FOR 4.7kohm PULLUP! THIS IS NOT NORMAL!)
+// Verified by linagee. Source: https://www.mouser.com/datasheet/2/362/semitec%20usa%20corporation_gtthermistor-1202937.pdf
+// Calculated using 1kohm pullup, voltage divider math, and manufacturer provided temp/resistance
+// Advantage: More resolution and better linearity from 150C to 200C
+const temp_entry_t temptable_52[] PROGMEM = {
+  { OV(   1), 500 },
+  { OV( 125), 300 }, // top rating 300C
+  { OV( 142), 290 },
+  { OV( 162), 280 },
+  { OV( 185), 270 },
+  { OV( 211), 260 },
+  { OV( 240), 250 },
+  { OV( 274), 240 },
+  { OV( 312), 230 },
+  { OV( 355), 220 },
+  { OV( 401), 210 },
+  { OV( 452), 200 },
+  { OV( 506), 190 },
+  { OV( 563), 180 },
+  { OV( 620), 170 },
+  { OV( 677), 160 },
+  { OV( 732), 150 },
+  { OV( 783), 140 },
+  { OV( 830), 130 },
+  { OV( 871), 120 },
+  { OV( 906), 110 },
+  { OV( 935), 100 },
+  { OV( 958),  90 },
+  { OV( 976),  80 },
+  { OV( 990),  70 },
+  { OV(1000),  60 },
+  { OV(1008),  50 },
+  { OV(1013),  40 },
+  { OV(1017),  30 },
+  { OV(1019),  20 },
+  { OV(1021),  10 },
+  { OV(1022),   0 }
+};
diff --git a/Marlin/src/module/thermistor/thermistor_55.h b/Marlin/src/module/thermistor/thermistor_55.h
new file mode 100644
index 0000000..707b7d4
--- /dev/null
+++ b/Marlin/src/module/thermistor/thermistor_55.h
@@ -0,0 +1,62 @@
+/**
+ * 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
+
+// R25 = 100 kOhm, beta25 = 4267 K, 1 kOhm pull-up,
+// 100k ATC Semitec 104GT-2 (Used on ParCan) (WITH 1kohm RESISTOR FOR PULLUP, R9 ON SANGUINOLOLU! NOT FOR 4.7kohm PULLUP! THIS IS NOT NORMAL!)
+// Verified by linagee. Source: https://www.mouser.com/datasheet/2/362/semitec%20usa%20corporation_gtthermistor-1202937.pdf
+// Calculated using 1kohm pullup, voltage divider math, and manufacturer provided temp/resistance
+// Advantage: More resolution and better linearity from 150C to 200C
+const temp_entry_t temptable_55[] PROGMEM = {
+  { OV(   1), 500 },
+  { OV(  76), 300 },
+  { OV(  87), 290 },
+  { OV( 100), 280 },
+  { OV( 114), 270 },
+  { OV( 131), 260 },
+  { OV( 152), 250 },
+  { OV( 175), 240 },
+  { OV( 202), 230 },
+  { OV( 234), 220 },
+  { OV( 271), 210 },
+  { OV( 312), 200 },
+  { OV( 359), 190 },
+  { OV( 411), 180 },
+  { OV( 467), 170 },
+  { OV( 527), 160 },
+  { OV( 590), 150 },
+  { OV( 652), 140 },
+  { OV( 713), 130 },
+  { OV( 770), 120 },
+  { OV( 822), 110 },
+  { OV( 867), 100 },
+  { OV( 905),  90 },
+  { OV( 936),  80 },
+  { OV( 961),  70 },
+  { OV( 979),  60 },
+  { OV( 993),  50 },
+  { OV(1003),  40 },
+  { OV(1010),  30 },
+  { OV(1015),  20 },
+  { OV(1018),  10 },
+  { OV(1020),   0 }
+};
diff --git a/Marlin/src/module/thermistor/thermistor_6.h b/Marlin/src/module/thermistor/thermistor_6.h
new file mode 100644
index 0000000..6811341
--- /dev/null
+++ b/Marlin/src/module/thermistor/thermistor_6.h
@@ -0,0 +1,64 @@
+/**
+ * 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
+
+// R25 = 100 kOhm, beta25 = 4092 K, 8.2 kOhm pull-up, 100k Epcos (?) thermistor
+const temp_entry_t temptable_6[] PROGMEM = {
+  { OV(   1), 350 },
+  { OV(  28), 250 }, // top rating 250C
+  { OV(  31), 245 },
+  { OV(  35), 240 },
+  { OV(  39), 235 },
+  { OV(  42), 230 },
+  { OV(  44), 225 },
+  { OV(  49), 220 },
+  { OV(  53), 215 },
+  { OV(  62), 210 },
+  { OV(  71), 205 }, // fitted graphically
+  { OV(  78), 200 }, // fitted graphically
+  { OV(  94), 190 },
+  { OV( 102), 185 },
+  { OV( 116), 170 },
+  { OV( 143), 160 },
+  { OV( 183), 150 },
+  { OV( 223), 140 },
+  { OV( 270), 130 },
+  { OV( 318), 120 },
+  { OV( 383), 110 },
+  { OV( 413), 105 },
+  { OV( 439), 100 },
+  { OV( 484),  95 },
+  { OV( 513),  90 },
+  { OV( 607),  80 },
+  { OV( 664),  70 },
+  { OV( 781),  60 },
+  { OV( 810),  55 },
+  { OV( 849),  50 },
+  { OV( 914),  45 },
+  { OV( 914),  40 },
+  { OV( 935),  35 },
+  { OV( 954),  30 },
+  { OV( 970),  25 },
+  { OV( 978),  22 },
+  { OV(1008),   3 },
+  { OV(1023),   0 } // to allow internal 0 degrees C
+};
diff --git a/Marlin/src/module/thermistor/thermistor_60.h b/Marlin/src/module/thermistor/thermistor_60.h
new file mode 100644
index 0000000..a3fe505
--- /dev/null
+++ b/Marlin/src/module/thermistor/thermistor_60.h
@@ -0,0 +1,107 @@
+/**
+ * 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
+
+// R25 = 100 kOhm, beta25 = 3950 K, 4.7 kOhm pull-up,
+// Maker's Tool Works Kapton Bed Thermistor
+// ./createTemperatureLookup.py --r0=100000 --t0=25 --r1=0 --r2=4700 --beta=3950
+// r0: 100000
+// t0: 25
+// r1: 0 (parallel with rTherm)
+// r2: 4700 (series with rTherm)
+// beta: 3950
+// min adc: 1 at 0.0048828125 V
+// max adc: 1023 at 4.9951171875 V
+const temp_entry_t temptable_60[] PROGMEM = {
+  { OV(  51), 272 },
+  { OV(  61), 258 },
+  { OV(  71), 247 },
+  { OV(  81), 237 },
+  { OV(  91), 229 },
+  { OV( 101), 221 },
+  { OV( 131), 204 },
+  { OV( 161), 190 },
+  { OV( 191), 179 },
+  { OV( 231), 167 },
+  { OV( 271), 157 },
+  { OV( 311), 148 },
+  { OV( 351), 140 },
+  { OV( 381), 135 },
+  { OV( 411), 130 },
+  { OV( 441), 125 },
+  { OV( 451), 123 },
+  { OV( 461), 122 },
+  { OV( 471), 120 },
+  { OV( 481), 119 },
+  { OV( 491), 117 },
+  { OV( 501), 116 },
+  { OV( 511), 114 },
+  { OV( 521), 113 },
+  { OV( 531), 111 },
+  { OV( 541), 110 },
+  { OV( 551), 108 },
+  { OV( 561), 107 },
+  { OV( 571), 105 },
+  { OV( 581), 104 },
+  { OV( 591), 102 },
+  { OV( 601), 101 },
+  { OV( 611), 100 },
+  { OV( 621),  98 },
+  { OV( 631),  97 },
+  { OV( 641),  95 },
+  { OV( 651),  94 },
+  { OV( 661),  92 },
+  { OV( 671),  91 },
+  { OV( 681),  90 },
+  { OV( 691),  88 },
+  { OV( 701),  87 },
+  { OV( 711),  85 },
+  { OV( 721),  84 },
+  { OV( 731),  82 },
+  { OV( 741),  81 },
+  { OV( 751),  79 },
+  { OV( 761),  77 },
+  { OV( 771),  76 },
+  { OV( 781),  74 },
+  { OV( 791),  72 },
+  { OV( 801),  71 },
+  { OV( 811),  69 },
+  { OV( 821),  67 },
+  { OV( 831),  65 },
+  { OV( 841),  63 },
+  { OV( 851),  62 },
+  { OV( 861),  60 },
+  { OV( 871),  57 },
+  { OV( 881),  55 },
+  { OV( 891),  53 },
+  { OV( 901),  51 },
+  { OV( 911),  48 },
+  { OV( 921),  45 },
+  { OV( 931),  42 },
+  { OV( 941),  39 },
+  { OV( 951),  36 },
+  { OV( 961),  32 },
+  { OV( 981),  23 },
+  { OV( 991),  17 },
+  { OV(1001),   9 },
+  { OV(1008),   0 }
+};
diff --git a/Marlin/src/module/thermistor/thermistor_61.h b/Marlin/src/module/thermistor/thermistor_61.h
new file mode 100644
index 0000000..ed4c4c8
--- /dev/null
+++ b/Marlin/src/module/thermistor/thermistor_61.h
@@ -0,0 +1,116 @@
+/**
+ * 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
+
+// R25 = 100 kOhm, beta25 = 3950 K, 4.7 kOhm pull-up,
+// Formbot / Vivedino high temp 100k thermistor
+// 100KR13950181203
+// Generated with modified version of https://www.thingiverse.com/thing:103668
+// Using table 1 with datasheet values
+// Resistance         100k Ohms at 25deg. C
+// Resistance Tolerance     + / -1%
+// B Value             3950K at 25/50 deg. C
+// B Value Tolerance         + / - 1%
+const temp_entry_t temptable_61[] PROGMEM = {
+  { OV(   2.00), 420 }, // Guestimate to ensure we dont lose a reading and drop temps to -50 when over
+  { OV(  12.07), 350 },
+  { OV(  12.79), 345 },
+  { OV(  13.59), 340 },
+  { OV(  14.44), 335 },
+  { OV(  15.37), 330 },
+  { OV(  16.38), 325 },
+  { OV(  17.46), 320 },
+  { OV(  18.63), 315 },
+  { OV(  19.91), 310 },
+  { OV(  21.29), 305 },
+  { OV(  22.79), 300 },
+  { OV(  24.43), 295 },
+  { OV(  26.21), 290 },
+  { OV(  28.15), 285 },
+  { OV(  30.27), 280 },
+  { OV(  32.58), 275 },
+  { OV(  35.10), 270 },
+  { OV(  38.44), 265 },
+  { OV(  40.89), 260 },
+  { OV(  44.19), 255 },
+  { OV(  47.83), 250 },
+  { OV(  51.80), 245 },
+  { OV(  56.20), 240 },
+  { OV(  61.00), 235 },
+  { OV(  66.30), 230 },
+  { OV(  72.11), 225 },
+  { OV(  78.51), 220 },
+  { OV(  85.57), 215 },
+  { OV(  93.34), 210 },
+  { OV( 101.91), 205 },
+  { OV( 111.34), 200 },
+  { OV( 121.73), 195 },
+  { OV( 133.17), 190 },
+  { OV( 145.74), 185 },
+  { OV( 159.57), 180 },
+  { OV( 174.73), 175 },
+  { OV( 191.35), 170 },
+  { OV( 209.53), 165 },
+  { OV( 229.35), 160 },
+  { OV( 250.90), 155 },
+  { OV( 274.25), 150 },
+  { OV( 299.46), 145 },
+  { OV( 326.52), 140 },
+  { OV( 355.44), 135 },
+  { OV( 386.15), 130 },
+  { OV( 418.53), 125 },
+  { OV( 452.43), 120 },
+  { OV( 487.62), 115 },
+  { OV( 523.82), 110 },
+  { OV( 560.70), 105 },
+  { OV( 597.88), 100 },
+  { OV( 634.97),  95 },
+  { OV( 671.55),  90 },
+  { OV( 707.21),  85 },
+  { OV( 741.54),  80 },
+  { OV( 779.65),  75 },
+  { OV( 809.57),  70 },
+  { OV( 833.40),  65 },
+  { OV( 859.55),  60 },
+  { OV( 883.27),  55 },
+  { OV( 904.53),  50 },
+  { OV( 923.38),  45 },
+  { OV( 939.91),  40 },
+  { OV( 954.26),  35 },
+  { OV( 966.59),  30 },
+  { OV( 977.08),  25 },
+  { OV( 985.92),  20 },
+  { OV( 993.39),  15 },
+  { OV( 999.42),  10 },
+  { OV(1004.43),   5 },
+  { OV(1008.51),   0 },
+  { OV(1011.79),  -5 },
+  { OV(1014.40), -10 },
+  { OV(1016.48), -15 },
+  { OV(1018.10), -20 },
+  { OV(1019.35), -25 },
+  { OV(1020.32), -30 },
+  { OV(1021.05), -35 },
+  { OV(1021.60), -40 },
+  { OV(1022.01), -45 },
+  { OV(1022.31), -50 }
+};
diff --git a/Marlin/src/module/thermistor/thermistor_66.h b/Marlin/src/module/thermistor/thermistor_66.h
new file mode 100644
index 0000000..0ad5994
--- /dev/null
+++ b/Marlin/src/module/thermistor/thermistor_66.h
@@ -0,0 +1,53 @@
+/**
+ * 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
+
+// R25 = 2.5 MOhm, beta25 = 4500 K, 4.7 kOhm pull-up, DyzeDesign 500 °C Thermistor
+const temp_entry_t temptable_66[] PROGMEM = {
+  { OV(  17.5), 850 },
+  { OV(  17.9), 500 },
+  { OV(  21.7), 480 },
+  { OV(  26.6), 460 },
+  { OV(  33.1), 440 },
+  { OV(  41.0), 420 },
+  { OV(  52.3), 400 },
+  { OV(  67.7), 380 },
+  { OV(  86.5), 360 },
+  { OV( 112.0), 340 },
+  { OV( 147.2), 320 },
+  { OV( 194.0), 300 },
+  { OV( 254.3), 280 },
+  { OV( 330.2), 260 },
+  { OV( 427.9), 240 },
+  { OV( 533.4), 220 },
+  { OV( 646.5), 200 },
+  { OV( 754.4), 180 },
+  { OV( 844.3), 160 },
+  { OV( 911.7), 140 },
+  { OV( 958.6), 120 },
+  { OV( 988.8), 100 },
+  { OV(1006.6),  80 },
+  { OV(1015.8),  60 },
+  { OV(1021.3),  30 },
+  { OV(  1022),  25 },
+  { OV(  1023),  20 }
+};
diff --git a/Marlin/src/module/thermistor/thermistor_666.h b/Marlin/src/module/thermistor/thermistor_666.h
new file mode 100644
index 0000000..490dbd5
--- /dev/null
+++ b/Marlin/src/module/thermistor/thermistor_666.h
@@ -0,0 +1,98 @@
+/**
+ * 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
+
+/**
+ * This file was generated by tltgen on Thu Jul  5 15:46:43 2018.
+ * tltgen was created by Pieter Agten (pieter.agten@gmail.com).
+ */
+//#include "output_table.h"
+
+/*
+ * Parameters:
+ * A: -0.000480634
+ * B: 0.00031362
+ * C: -2.03978e-07
+ */
+const temp_entry_t temptable_666[] PROGMEM = {
+  { OV(  1), 794 },
+  { OV( 18), 288 },
+  { OV( 35), 234 },
+  { OV( 52), 207 },
+  { OV( 69), 189 },
+  { OV( 86), 176 },
+  { OV(103), 166 },
+  { OV(120), 157 },
+  { OV(137) ,150 },
+  { OV(154), 144 },
+  { OV(172), 138 },
+  { OV(189), 134 },
+  { OV(206), 129 },
+  { OV(223), 125 },
+  { OV(240), 121 },
+  { OV(257), 118 },
+  { OV(274), 115 },
+  { OV(291), 112 },
+  { OV(308), 109 },
+  { OV(325), 106 },
+  { OV(342), 103 },
+  { OV(359), 101 },
+  { OV(376),  99 },
+  { OV(393),  96 },
+  { OV(410),  94 },
+  { OV(427),  92 },
+  { OV(444),  90 },
+  { OV(461),  88 },
+  { OV(478),  86 },
+  { OV(495),  84 },
+  { OV(512),  82 },
+  { OV(530),  80 },
+  { OV(547),  78 },
+  { OV(564),  76 },
+  { OV(581),  74 },
+  { OV(598),  72 },
+  { OV(615),  70 },
+  { OV(632),  68 },
+  { OV(649),  67 },
+  { OV(666),  65 },
+  { OV(683),  63 },
+  { OV(700),  61 },
+  { OV(717),  59 },
+  { OV(734),  57 },
+  { OV(751),  55 },
+  { OV(768),  53 },
+  { OV(785),  51 },
+  { OV(802),  49 },
+  { OV(819),  47 },
+  { OV(836),  44 },
+  { OV(853),  42 },
+  { OV(871),  39 },
+  { OV(888),  37 },
+  { OV(905),  34 },
+  { OV(922),  30 },
+  { OV(939),  27 },
+  { OV(956),  23 },
+  { OV(973),  18 },
+  { OV(990),  11 },
+  { OV(1007),  2 },
+  { OV(1023),-25 }
+};
diff --git a/Marlin/src/module/thermistor/thermistor_67.h b/Marlin/src/module/thermistor/thermistor_67.h
new file mode 100644
index 0000000..7d6d7f6
--- /dev/null
+++ b/Marlin/src/module/thermistor/thermistor_67.h
@@ -0,0 +1,81 @@
+/**
+ * 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
+
+// R25 = 500 KOhm, beta25 = 3800 K, 4.7 kOhm pull-up, SliceEngineering 450 °C Thermistor
+const temp_entry_t temptable_67[] PROGMEM = {
+  { OV(  22 ),  500 },
+  { OV(  23 ),  490 },
+  { OV(  25 ),  480 },
+  { OV(  27 ),  470 },
+  { OV(  29 ),  460 },
+  { OV(  32 ),  450 },
+  { OV(  35 ),  440 },
+  { OV(  38 ),  430 },
+  { OV(  41 ),  420 },
+  { OV(  45 ),  410 },
+  { OV(  50 ),  400 },
+  { OV(  55 ),  390 },
+  { OV(  60 ),  380 },
+  { OV(  67 ),  370 },
+  { OV(  74 ),  360 },
+  { OV(  82 ),  350 },
+  { OV(  91 ),  340 },
+  { OV( 102 ),  330 },
+  { OV( 114 ),  320 },
+  { OV( 127 ),  310 },
+  { OV( 143 ),  300 },
+  { OV( 161 ),  290 },
+  { OV( 181 ),  280 },
+  { OV( 204 ),  270 },
+  { OV( 229 ),  260 },
+  { OV( 259 ),  250 },
+  { OV( 290 ),  240 },
+  { OV( 325 ),  230 },
+  { OV( 364 ),  220 },
+  { OV( 407 ),  210 },
+  { OV( 453 ),  200 },
+  { OV( 501 ),  190 },
+  { OV( 551 ),  180 },
+  { OV( 603 ),  170 },
+  { OV( 655 ),  160 },
+  { OV( 706 ),  150 },
+  { OV( 755 ),  140 },
+  { OV( 801 ),  130 },
+  { OV( 842 ),  120 },
+  { OV( 879 ),  110 },
+  { OV( 910 ),  100 },
+  { OV( 936 ),   90 },
+  { OV( 948 ),   85 },
+  { OV( 958 ),   80 },
+  { OV( 975 ),   70 },
+  { OV( 988 ),   60 },
+  { OV( 998 ),   50 },
+  { OV(1006 ),   40 },
+  { OV(1011 ),   30 },
+  { OV(1013 ),   25 },
+  { OV(1015 ),   20 },
+  { OV(1018 ),   10 },
+  { OV(1020 ),    0 },
+  { OV(1021 ),  -10 },
+  { OV(1022 ),  -20 }
+};
diff --git a/Marlin/src/module/thermistor/thermistor_7.h b/Marlin/src/module/thermistor/thermistor_7.h
new file mode 100644
index 0000000..7a73755
--- /dev/null
+++ b/Marlin/src/module/thermistor/thermistor_7.h
@@ -0,0 +1,84 @@
+/**
+ * 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
+
+// R25 = 100 kOhm, beta25 = 3974 K, 4.7 kOhm pull-up, Honeywell 135-104LAG-J01
+const temp_entry_t temptable_7[] PROGMEM = {
+  { OV(   1), 941 },
+  { OV(  19), 362 },
+  { OV(  37), 299 }, // top rating 300C
+  { OV(  55), 266 },
+  { OV(  73), 245 },
+  { OV(  91), 229 },
+  { OV( 109), 216 },
+  { OV( 127), 206 },
+  { OV( 145), 197 },
+  { OV( 163), 190 },
+  { OV( 181), 183 },
+  { OV( 199), 177 },
+  { OV( 217), 171 },
+  { OV( 235), 166 },
+  { OV( 253), 162 },
+  { OV( 271), 157 },
+  { OV( 289), 153 },
+  { OV( 307), 149 },
+  { OV( 325), 146 },
+  { OV( 343), 142 },
+  { OV( 361), 139 },
+  { OV( 379), 135 },
+  { OV( 397), 132 },
+  { OV( 415), 129 },
+  { OV( 433), 126 },
+  { OV( 451), 123 },
+  { OV( 469), 121 },
+  { OV( 487), 118 },
+  { OV( 505), 115 },
+  { OV( 523), 112 },
+  { OV( 541), 110 },
+  { OV( 559), 107 },
+  { OV( 577), 105 },
+  { OV( 595), 102 },
+  { OV( 613),  99 },
+  { OV( 631),  97 },
+  { OV( 649),  94 },
+  { OV( 667),  92 },
+  { OV( 685),  89 },
+  { OV( 703),  86 },
+  { OV( 721),  84 },
+  { OV( 739),  81 },
+  { OV( 757),  78 },
+  { OV( 775),  75 },
+  { OV( 793),  72 },
+  { OV( 811),  69 },
+  { OV( 829),  66 },
+  { OV( 847),  62 },
+  { OV( 865),  59 },
+  { OV( 883),  55 },
+  { OV( 901),  51 },
+  { OV( 919),  46 },
+  { OV( 937),  41 },
+  { OV( 955),  35 },
+  { OV( 973),  27 },
+  { OV( 991),  17 },
+  { OV(1009),   1 },
+  { OV(1023),   0 } // to allow internal 0 degrees C
+};
diff --git a/Marlin/src/module/thermistor/thermistor_70.h b/Marlin/src/module/thermistor/thermistor_70.h
new file mode 100644
index 0000000..466b925
--- /dev/null
+++ b/Marlin/src/module/thermistor/thermistor_70.h
@@ -0,0 +1,46 @@
+/**
+ * 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
+
+// Stock BQ Hephestos 2 100k thermistor.
+// Created on 29/12/2017 with an ambient temperature of 20C.
+// ANENG AN8009 DMM with a K-type probe used for measurements.
+
+// R25 = 100 kOhm, beta25 = 4100 K, 4.7 kOhm pull-up, bqh2 stock thermistor
+const temp_entry_t temptable_70[] PROGMEM = {
+  { OV(  18), 270 },
+  { OV(  27), 248 },
+  { OV(  34), 234 },
+  { OV(  45), 220 },
+  { OV(  61), 205 },
+  { OV(  86), 188 },
+  { OV( 123), 172 },
+  { OV( 420), 110 },
+  { OV( 590),  90 },
+  { OV( 845),  56 },
+  { OV( 970),  25 },
+  { OV( 986),  20 },
+  { OV( 994),  15 },
+  { OV(1000),  10 },
+  { OV(1005),   5 },
+  { OV(1009),   0 } // safety
+};
diff --git a/Marlin/src/module/thermistor/thermistor_71.h b/Marlin/src/module/thermistor/thermistor_71.h
new file mode 100644
index 0000000..abd7fc5
--- /dev/null
+++ b/Marlin/src/module/thermistor/thermistor_71.h
@@ -0,0 +1,94 @@
+/**
+ * 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
+
+// R25 = 100 kOhm, beta25 = 3974 K, 4.7 kOhm pull-up, Honeywell 135-104LAF-J01
+// R0 = 100000 Ohm
+// T0 = 25 °C
+// Beta = 3974
+// R1 = 0 Ohm
+// R2 = 4700 Ohm
+const temp_entry_t temptable_71[] PROGMEM = {
+  { OV(  35), 300 },
+  { OV(  51), 269 },
+  { OV(  59), 258 },
+  { OV(  64), 252 },
+  { OV(  71), 244 },
+  { OV(  81), 235 },
+  { OV(  87), 230 },
+  { OV(  92), 226 },
+  { OV( 102), 219 },
+  { OV( 110), 214 },
+  { OV( 115), 211 },
+  { OV( 126), 205 },
+  { OV( 128), 204 },
+  { OV( 130), 203 },
+  { OV( 132), 202 },
+  { OV( 134), 201 },
+  { OV( 136), 200 },
+  { OV( 147), 195 },
+  { OV( 154), 192 },
+  { OV( 159), 190 },
+  { OV( 164), 188 },
+  { OV( 172), 185 },
+  { OV( 175), 184 },
+  { OV( 178), 183 },
+  { OV( 181), 182 },
+  { OV( 184), 181 },
+  { OV( 187), 180 },
+  { OV( 190), 179 },
+  { OV( 193), 178 },
+  { OV( 196), 177 },
+  { OV( 199), 176 },
+  { OV( 202), 175 },
+  { OV( 205), 174 },
+  { OV( 208), 173 },
+  { OV( 215), 171 },
+  { OV( 237), 165 },
+  { OV( 256), 160 },
+  { OV( 300), 150 },
+  { OV( 351), 140 },
+  { OV( 470), 120 },
+  { OV( 504), 115 },
+  { OV( 538), 110 },
+  { OV( 745),  80 },
+  { OV( 770),  76 },
+  { OV( 806),  70 },
+  { OV( 829),  66 },
+  { OV( 860),  60 },
+  { OV( 879),  56 },
+  { OV( 888),  54 },
+  { OV( 905),  50 },
+  { OV( 924),  45 },
+  { OV( 940),  40 },
+  { OV( 955),  35 },
+  { OV( 972),  28 },
+  { OV( 974),  27 },
+  { OV( 976),  26 },
+  { OV( 978),  25 },
+  { OV( 980),  24 },
+  { OV( 987),  20 },
+  { OV( 995),  15 },
+  { OV(1001),  10 },
+  { OV(1006),   5 },
+  { OV(1010),   0 }
+};
diff --git a/Marlin/src/module/thermistor/thermistor_75.h b/Marlin/src/module/thermistor/thermistor_75.h
new file mode 100644
index 0000000..79800d2
--- /dev/null
+++ b/Marlin/src/module/thermistor/thermistor_75.h
@@ -0,0 +1,80 @@
+/**
+ * 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
+
+/**
+ * R25 = 100 kOhm, beta25 = 4100 K, 4.7 kOhm pull-up,
+ * Generic Silicon Heat Pad with NTC 100K thermistor
+ *
+ * Many generic silicone heat pads use the MGB18-104F39050L32 thermistor, applicable to various
+ * wattages and voltages. This table is correct if this part is used. It's been optimized
+ * to provide good granularity in the 60-110C range, good for PLA and ABS. For higher temperature
+ * filament (e.g., nylon) uncomment HIGH_TEMP_RANGE_75 for increased accuracy. If higher
+ * temperatures aren't used it can improve performance slightly to leave it commented out.
+ */
+
+//#define HIGH_TEMP_RANGE_75
+
+const temp_entry_t temptable_75[] PROGMEM = { // Generic Silicon Heat Pad with NTC 100K MGB18-104F39050L32 thermistor
+  { OV(111.06), 200 }, // v=0.542 r=571.747 res=0.501 degC/count
+
+  #ifdef HIGH_TEMP_RANGE_75
+    { OV(174.87), 175 }, // v=0.854 r=967.950 res=0.311 degC/count  These values are valid.  But they serve no
+    { OV(191.64), 170 }, // v=0.936 r=1082.139 res=0.284 degC/count  purpose.  It is better to delete them so
+    { OV(209.99), 165 }, // v=1.025 r=1212.472 res=0.260 degC/count  the search is quicker and get to the meaningful
+    { OV(230.02), 160 }, // v=1.123 r=1361.590 res=0.239 degC/count  part of the table sooner.
+    { OV(251.80), 155 }, // v=1.230 r=1532.621 res=0.220 degC/count
+  #endif
+
+  { OV(275.43), 150 }, // v=1.345 r=1729.283 res=0.203 degC/count
+
+  #ifdef HIGH_TEMP_RANGE_75
+    { OV(300.92), 145 }, // v=1.469 r=1956.004 res=0.189 degC/coun
+  #endif
+
+  { OV( 328.32), 140 }, // v=1.603 r=2218.081 res=0.176 degC/count
+  { OV( 388.65), 130 }, // v=1.898 r=2874.980 res=0.156 degC/count
+  { OV( 421.39), 125 }, // v=2.058 r=3286.644 res=0.149 degC/count
+  { OV( 455.65), 120 }, // v=2.225 r=3768.002 res=0.143 degC/count
+  { OV( 491.17), 115 }, // v=2.398 r=4332.590 res=0.139 degC/count
+  { OV( 527.68), 110 }, // v=2.577 r=4996.905 res=0.136 degC/count
+  { OV( 564.81), 105 }, // v=2.758 r=5781.120 res=0.134 degC/count
+  { OV( 602.19), 100 }, // v=2.940 r=6710.000 res=0.134 degC/count
+  { OV( 676.03),  90 }, // v=3.301 r=9131.018 res=0.138 degC/count
+  { OV( 745.85),  80 }, // v=3.642 r=12602.693 res=0.150 degC/count
+  { OV( 778.31),  75 }, // v=3.800 r=14889.001 res=0.159 degC/count
+  { OV( 808.75),  70 }, // v=3.949 r=17658.700 res=0.171 degC/count
+  { OV( 836.94),  65 }, // v=4.087 r=21028.040 res=0.185 degC/count
+  { OV( 862.74),  60 }, // v=4.213 r=25144.568 res=0.204 degC/count
+  { OV( 886.08),  55 }, // v=4.327 r=30196.449 res=0.227 degC/count
+  { OV( 906.97),  50 }, // v=4.429 r=36424.838 res=0.255 degC/count
+  { OV( 941.65),  40 }, // v=4.598 r=53745.337 res=0.333 degC/count
+  { OV( 967.76),  30 }, // v=4.725 r=80880.630 res=0.452 degC/count
+  { OV( 978.03),  25 }, // v=4.776 r=100000.000 res=0.535 degC/count
+  { OV( 981.68),  23 }, // v=4.793 r=109024.395 res=0.573 degC/count
+  { OV( 983.41),  22 }, // v=4.802 r=113875.430 res=0.594 degC/count
+  { OV( 985.08),  21 }, // v=4.810 r=118968.955 res=0.616 degC/count
+  { OV( 986.70),  20 }, // v=4.818 r=124318.354 res=0.638 degC/count
+  { OV( 993.94),  15 }, // v=4.853 r=155431.302 res=0.768 degC/count
+  { OV( 999.96),  10 }, // v=4.883 r=195480.023 res=0.934 degC/count
+  { OV(1008.95),   0 }  // v=4.926 r=314997.575 res=1.418 degC/count
+};
diff --git a/Marlin/src/module/thermistor/thermistor_8.h b/Marlin/src/module/thermistor/thermistor_8.h
new file mode 100644
index 0000000..9e19168
--- /dev/null
+++ b/Marlin/src/module/thermistor/thermistor_8.h
@@ -0,0 +1,46 @@
+/**
+ * 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
+
+// R25 = 100 kOhm, beta25 = 3950 K, 10 kOhm pull-up, NTCS0603E3104FHT
+const temp_entry_t temptable_8[] PROGMEM = {
+  { OV(   1), 704 },
+  { OV(  54), 216 },
+  { OV( 107), 175 },
+  { OV( 160), 152 },
+  { OV( 213), 137 },
+  { OV( 266), 125 },
+  { OV( 319), 115 },
+  { OV( 372), 106 },
+  { OV( 425),  99 },
+  { OV( 478),  91 },
+  { OV( 531),  85 },
+  { OV( 584),  78 },
+  { OV( 637),  71 },
+  { OV( 690),  65 },
+  { OV( 743),  58 },
+  { OV( 796),  50 },
+  { OV( 849),  42 },
+  { OV( 902),  31 },
+  { OV( 955),  17 },
+  { OV(1008),   0 }
+};
diff --git a/Marlin/src/module/thermistor/thermistor_9.h b/Marlin/src/module/thermistor/thermistor_9.h
new file mode 100644
index 0000000..2909742
--- /dev/null
+++ b/Marlin/src/module/thermistor/thermistor_9.h
@@ -0,0 +1,57 @@
+/**
+ * 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
+
+// R25 = 100 kOhm, beta25 = 3960 K, 4.7 kOhm pull-up, GE Sensing AL03006-58.2K-97-G1
+const temp_entry_t temptable_9[] PROGMEM = {
+  { OV(   1), 936 },
+  { OV(  36), 300 },
+  { OV(  71), 246 },
+  { OV( 106), 218 },
+  { OV( 141), 199 },
+  { OV( 176), 185 },
+  { OV( 211), 173 },
+  { OV( 246), 163 },
+  { OV( 281), 155 },
+  { OV( 316), 147 },
+  { OV( 351), 140 },
+  { OV( 386), 134 },
+  { OV( 421), 128 },
+  { OV( 456), 122 },
+  { OV( 491), 117 },
+  { OV( 526), 112 },
+  { OV( 561), 107 },
+  { OV( 596), 102 },
+  { OV( 631),  97 },
+  { OV( 666),  92 },
+  { OV( 701),  87 },
+  { OV( 736),  81 },
+  { OV( 771),  76 },
+  { OV( 806),  70 },
+  { OV( 841),  63 },
+  { OV( 876),  56 },
+  { OV( 911),  48 },
+  { OV( 946),  38 },
+  { OV( 981),  23 },
+  { OV(1005),   5 },
+  { OV(1016),   0 }
+};
diff --git a/Marlin/src/module/thermistor/thermistor_99.h b/Marlin/src/module/thermistor/thermistor_99.h
new file mode 100644
index 0000000..839a511
--- /dev/null
+++ b/Marlin/src/module/thermistor/thermistor_99.h
@@ -0,0 +1,64 @@
+/**
+ * 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
+
+// 100k bed thermistor with a 10K pull-up resistor - made by $ buildroot/share/scripts/createTemperatureLookupMarlin.py --rp=10000
+
+const temp_entry_t temptable_99[] PROGMEM = {
+  { OV(  5.81), 350 }, // v=0.028   r=    57.081  res=13.433 degC/count
+  { OV(  6.54), 340 }, // v=0.032   r=    64.248  res=11.711 degC/count
+  { OV(  7.38), 330 }, // v=0.036   r=    72.588  res=10.161 degC/count
+  { OV(  8.36), 320 }, // v=0.041   r=    82.336  res= 8.772 degC/count
+  { OV(  9.51), 310 }, // v=0.046   r=    93.780  res= 7.535 degC/count
+  { OV( 10.87), 300 }, // v=0.053   r=   107.281  res= 6.439 degC/count
+  { OV( 12.47), 290 }, // v=0.061   r=   123.286  res= 5.473 degC/count
+  { OV( 14.37), 280 }, // v=0.070   r=   142.360  res= 4.627 degC/count
+  { OV( 16.64), 270 }, // v=0.081   r=   165.215  res= 3.891 degC/count
+  { OV( 19.37), 260 }, // v=0.095   r=   192.758  res= 3.253 degC/count
+  { OV( 22.65), 250 }, // v=0.111   r=   226.150  res= 2.705 degC/count
+  { OV( 26.62), 240 }, // v=0.130   r=   266.891  res= 2.236 degC/count
+  { OV( 31.46), 230 }, // v=0.154   r=   316.931  res= 1.839 degC/count
+  { OV( 37.38), 220 }, // v=0.182   r=   378.822  res= 1.504 degC/count
+  { OV( 44.65), 210 }, // v=0.218   r=   455.939  res= 1.224 degC/count
+  { OV( 53.64), 200 }, // v=0.262   r=   552.778  res= 0.991 degC/count
+  { OV( 64.78), 190 }, // v=0.316   r=   675.386  res= 0.799 degC/count
+  { OV( 78.65), 180 }, // v=0.384   r=   831.973  res= 0.643 degC/count
+  { OV( 95.94), 170 }, // v=0.468   r=  1033.801  res= 0.516 degC/count
+  { OV(117.52), 160 }, // v=0.574   r=  1296.481  res= 0.414 degC/count
+  { OV(144.42), 150 }, // v=0.705   r=  1641.900  res= 0.333 degC/count
+  { OV(177.80), 140 }, // v=0.868   r=  2101.110  res= 0.269 degC/count
+  { OV(218.89), 130 }, // v=1.069   r=  2718.725  res= 0.220 degC/count
+  { OV(268.82), 120 }, // v=1.313   r=  3559.702  res= 0.183 degC/count
+  { OV(328.35), 110 }, // v=1.603   r=  4719.968  res= 0.155 degC/count
+  { OV(397.44), 100 }, // v=1.941   r=  6343.323  res= 0.136 degC/count
+  { OV(474.90),  90 }, // v=2.319   r=  8648.807  res= 0.124 degC/count
+  { OV(558.03),  80 }, // v=2.725   r= 11975.779  res= 0.118 degC/count
+  { OV(642.76),  70 }, // v=3.138   r= 16859.622  res= 0.119 degC/count
+  { OV(724.25),  60 }, // v=3.536   r= 24161.472  res= 0.128 degC/count
+  { OV(797.93),  50 }, // v=3.896   r= 35295.361  res= 0.146 degC/count
+  { OV(860.51),  40 }, // v=4.202   r= 52635.209  res= 0.178 degC/count
+  { OV(910.55),  30 }, // v=4.446   r= 80262.251  res= 0.229 degC/count
+  { OV(948.36),  20 }, // v=4.631   r=125374.433  res= 0.313 degC/count
+  { OV(975.47),  10 }, // v=4.763   r=201020.458  res= 0.449 degC/count
+  { OV(994.02),   0 }  // v=4.854   r=331567.870  res= 0.676 degC/count
+};
diff --git a/Marlin/src/module/thermistor/thermistor_998.h b/Marlin/src/module/thermistor/thermistor_998.h
new file mode 100644
index 0000000..e4cfbba
--- /dev/null
+++ b/Marlin/src/module/thermistor/thermistor_998.h
@@ -0,0 +1,33 @@
+/**
+ * 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
+
+// User-defined table 1
+// Dummy Thermistor table.. It will ALWAYS read a fixed value.
+#ifndef DUMMY_THERMISTOR_998_VALUE
+  #define DUMMY_THERMISTOR_998_VALUE 25
+#endif
+
+const temp_entry_t temptable_998[] PROGMEM = {
+  { OV(   1), DUMMY_THERMISTOR_998_VALUE },
+  { OV(1023), DUMMY_THERMISTOR_998_VALUE }
+};
diff --git a/Marlin/src/module/thermistor/thermistor_999.h b/Marlin/src/module/thermistor/thermistor_999.h
new file mode 100644
index 0000000..0271c47
--- /dev/null
+++ b/Marlin/src/module/thermistor/thermistor_999.h
@@ -0,0 +1,33 @@
+/**
+ * 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
+
+// User-defined table 2
+// Dummy Thermistor table.. It will ALWAYS read a fixed value.
+#ifndef DUMMY_THERMISTOR_999_VALUE
+  #define DUMMY_THERMISTOR_999_VALUE 25
+#endif
+
+const temp_entry_t temptable_999[] PROGMEM = {
+  { OV(   1), DUMMY_THERMISTOR_999_VALUE },
+  { OV(1023), DUMMY_THERMISTOR_999_VALUE }
+};
diff --git a/Marlin/src/module/thermistor/thermistors.h b/Marlin/src/module/thermistor/thermistors.h
new file mode 100644
index 0000000..03ed5c2
--- /dev/null
+++ b/Marlin/src/module/thermistor/thermistors.h
@@ -0,0 +1,507 @@
+/**
+ * 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/MarlinConfig.h"
+
+#define THERMISTOR_TABLE_ADC_RESOLUTION 10
+#define THERMISTOR_TABLE_SCALE (HAL_ADC_RANGE / _BV(THERMISTOR_TABLE_ADC_RESOLUTION))
+#if ENABLED(HAL_ADC_FILTERED)
+  #define OVERSAMPLENR 1
+#elif HAL_ADC_RESOLUTION > 10
+  #define OVERSAMPLENR (20 - HAL_ADC_RESOLUTION)
+#else
+  #define OVERSAMPLENR 16
+#endif
+#define MAX_RAW_THERMISTOR_VALUE (HAL_ADC_RANGE * (OVERSAMPLENR) - 1)
+
+// Currently Marlin stores all oversampled ADC values as int16_t, make sure the HAL settings do not overflow 15bit
+#if MAX_RAW_THERMISTOR_VALUE > ((1 << 15) - 1)
+  #error "MAX_RAW_THERMISTOR_VALUE is too large for int16_t. Reduce OVERSAMPLENR or HAL_ADC_RESOLUTION."
+#endif
+
+#define OV_SCALE(N) (N)
+#define OV(N) int16_t(OV_SCALE(N) * (OVERSAMPLENR) * (THERMISTOR_TABLE_SCALE))
+
+#define ANY_THERMISTOR_IS(n) (THERMISTOR_HEATER_0 == n || THERMISTOR_HEATER_1 == n || THERMISTOR_HEATER_2 == n || THERMISTOR_HEATER_3 == n || THERMISTOR_HEATER_4 == n || THERMISTOR_HEATER_5 == n || THERMISTOR_HEATER_6 == n || THERMISTOR_HEATER_7 == n || THERMISTORBED == n || THERMISTORCHAMBER == n || THERMISTORPROBE == n)
+
+typedef struct { int16_t value, celsius; } temp_entry_t;
+
+// Pt1000 and Pt100 handling
+//
+// Rt=R0*(1+a*T+b*T*T) [for T>0]
+// a=3.9083E-3, b=-5.775E-7
+#define PtA 3.9083E-3
+#define PtB -5.775E-7
+#define PtRt(T,R0) ((R0) * (1.0 + (PtA) * (T) + (PtB) * (T) * (T)))
+#define PtAdVal(T,R0,Rup) (short)(1024 / (Rup / PtRt(T, R0) + 1))
+#define PtLine(T,R0,Rup) { OV(PtAdVal(T, R0, Rup)), T }
+
+#if ANY_THERMISTOR_IS(1) // beta25 = 4092 K, R25 = 100 kOhm, Pull-up = 4.7 kOhm, "EPCOS"
+  #include "thermistor_1.h"
+#endif
+#if ANY_THERMISTOR_IS(2) // 4338 K, R25 = 200 kOhm, Pull-up = 4.7 kOhm, "ATC Semitec 204GT-2"
+  #include "thermistor_2.h"
+#endif
+#if ANY_THERMISTOR_IS(3) // beta25 = 4120 K, R25 = 100 kOhm, Pull-up = 4.7 kOhm, "Mendel-parts"
+  #include "thermistor_3.h"
+#endif
+#if ANY_THERMISTOR_IS(4) // beta25 = 3950 K, R25 = 10 kOhm, Pull-up = 4.7 kOhm, "Generic"
+  #include "thermistor_4.h"
+#endif
+#if ANY_THERMISTOR_IS(5) // beta25 = 4267 K, R25 = 100 kOhm, Pull-up = 4.7 kOhm, "ParCan, ATC 104GT-2"
+  #include "thermistor_5.h"
+#endif
+#if ANY_THERMISTOR_IS(501) // 100K Zonestar thermistor
+  #include "thermistor_501.h"
+#endif
+#if ANY_THERMISTOR_IS(502) // Unknown thermistor used by the Zonestar Průša P802M hot bed
+  #include "thermistor_502.h"
+#endif
+#if ANY_THERMISTOR_IS(503) // Zonestar (Z8XM2) Heated Bed thermistor
+  #include "thermistor_503.h"
+#endif
+#if ANY_THERMISTOR_IS(512) // 100k thermistor in RPW-Ultra hotend, Pull-up = 4.7 kOhm, "unknown model"
+  #include "thermistor_512.h"
+#endif
+#if ANY_THERMISTOR_IS(6) // beta25 = 4092 K, R25 = 100 kOhm, Pull-up = 8.2 kOhm, "EPCOS ?"
+  #include "thermistor_6.h"
+#endif
+#if ANY_THERMISTOR_IS(7) // beta25 = 3974 K, R25 = 100 kOhm, Pull-up = 4.7 kOhm, "Honeywell 135-104LAG-J01"
+  #include "thermistor_7.h"
+#endif
+#if ANY_THERMISTOR_IS(71) // beta25 = 3974 K, R25 = 100 kOhm, Pull-up = 4.7 kOhm, "Honeywell 135-104LAF-J01"
+  #include "thermistor_71.h"
+#endif
+#if ANY_THERMISTOR_IS(8) // beta25 = 3950 K, R25 = 100 kOhm, Pull-up = 10 kOhm, "Vishay E3104FHT"
+  #include "thermistor_8.h"
+#endif
+#if ANY_THERMISTOR_IS(9) // beta25 = 3960 K, R25 = 100 kOhm, Pull-up = 4.7 kOhm, "GE Sensing AL03006-58.2K-97-G1"
+  #include "thermistor_9.h"
+#endif
+#if ANY_THERMISTOR_IS(10) // beta25 = 3960 K, R25 = 100 kOhm, Pull-up = 4.7 kOhm, "RS 198-961"
+  #include "thermistor_10.h"
+#endif
+#if ANY_THERMISTOR_IS(11) // beta25 = 3950 K, R25 = 100 kOhm, Pull-up = 4.7 kOhm, "QU-BD silicone bed, QWG-104F-3950"
+  #include "thermistor_11.h"
+#endif
+#if ANY_THERMISTOR_IS(13) // beta25 = 4100 K, R25 = 100 kOhm, Pull-up = 4.7 kOhm, "Hisens"
+  #include "thermistor_13.h"
+#endif
+#if ANY_THERMISTOR_IS(15) // JGAurora A5 thermistor calibration
+  #include "thermistor_15.h"
+#endif
+#if ANY_THERMISTOR_IS(17) // Dagoma NTC 100k white thermistor
+  #include "thermistor_17.h"
+#endif
+#if ANY_THERMISTOR_IS(18) // ATC Semitec 204GT-2 (4.7k pullup) Dagoma.Fr - MKS_Base_DKU001327
+  #include "thermistor_18.h"
+#endif
+#if ANY_THERMISTOR_IS(20) // Pt100 with INA826 amp on Ultimaker v2.0 electronics
+  #include "thermistor_20.h"
+#endif
+#if ANY_THERMISTOR_IS(21) // Pt100 with INA826 amp with 3.3v excitation based on "Pt100 with INA826 amp on Ultimaker v2.0 electronics"
+  #include "thermistor_21.h"
+#endif
+#if ANY_THERMISTOR_IS(22) // Thermistor in a Rostock 301 hot end, calibrated with a multimeter
+  #include "thermistor_22.h"
+#endif
+#if ANY_THERMISTOR_IS(23) // By AluOne #12622. Formerly 22 above. May need calibration/checking.
+  #include "thermistor_23.h"
+#endif
+#if ANY_THERMISTOR_IS(30) // Kis3d Silicone mat 24V 200W/300W with 6mm Precision cast plate (EN AW 5083)
+  #include "thermistor_30.h"
+#endif
+#if ANY_THERMISTOR_IS(51) // beta25 = 4092 K, R25 = 100 kOhm, Pull-up = 1 kOhm, "EPCOS"
+  #include "thermistor_51.h"
+#endif
+#if ANY_THERMISTOR_IS(52) // beta25 = 4338 K, R25 = 200 kOhm, Pull-up = 1 kOhm, "ATC Semitec 204GT-2"
+  #include "thermistor_52.h"
+#endif
+#if ANY_THERMISTOR_IS(55) // beta25 = 4267 K, R25 = 100 kOhm, Pull-up = 1 kOhm, "ATC Semitec 104GT-2 (Used on ParCan)"
+  #include "thermistor_55.h"
+#endif
+#if ANY_THERMISTOR_IS(60) // beta25 = 3950 K, R25 = 100 kOhm, Pull-up = 4.7 kOhm, "Maker's Tool Works Kapton Bed"
+  #include "thermistor_60.h"
+#endif
+#if ANY_THERMISTOR_IS(61) // beta25 = 3950 K, R25 = 100 kOhm, Pull-up = 4.7 kOhm, "Formbot 350°C Thermistor"
+  #include "thermistor_61.h"
+#endif
+#if ANY_THERMISTOR_IS(66) // beta25 = 4500 K, R25 = 2.5 MOhm, Pull-up = 4.7 kOhm, "DyzeDesign 500 °C Thermistor"
+  #include "thermistor_66.h"
+#endif
+#if ANY_THERMISTOR_IS(67) // R25 = 500 KOhm, beta25 = 3800 K, 4.7 kOhm pull-up, SliceEngineering 450 °C Thermistor
+  #include "thermistor_67.h"
+#endif
+#if ANY_THERMISTOR_IS(12) // beta25 = 4700 K, R25 = 100 kOhm, Pull-up = 4.7 kOhm, "Personal calibration for Makibox hot bed"
+  #include "thermistor_12.h"
+#endif
+#if ANY_THERMISTOR_IS(70) // beta25 = 4100 K, R25 = 100 kOhm, Pull-up = 4.7 kOhm, "Hephestos 2, bqh2 stock thermistor"
+  #include "thermistor_70.h"
+#endif
+#if ANY_THERMISTOR_IS(75) // beta25 = 4100 K, R25 = 100 kOhm, Pull-up = 4.7 kOhm, "MGB18-104F39050L32 thermistor"
+  #include "thermistor_75.h"
+#endif
+#if ANY_THERMISTOR_IS(99) // 100k bed thermistor with a 10K pull-up resistor (on some Wanhao i3 models)
+  #include "thermistor_99.h"
+#endif
+#if ANY_THERMISTOR_IS(110) // Pt100 with 1k0 pullup
+  #include "thermistor_110.h"
+#endif
+#if ANY_THERMISTOR_IS(147) // Pt100 with 4k7 pullup
+  #include "thermistor_147.h"
+#endif
+#if ANY_THERMISTOR_IS(201) // Pt100 with LMV324 Overlord
+  #include "thermistor_201.h"
+#endif
+#if ANY_THERMISTOR_IS(202) // 200K thermistor in Copymaker3D hotend
+  #include "thermistor_202.h"
+#endif
+#if ANY_THERMISTOR_IS(331) // Like table 1, but with 3V3 as input voltage for MEGA
+  #include "thermistor_331.h"
+#endif
+#if ANY_THERMISTOR_IS(332) // Like table 1, but with 3V3 as input voltage for DUE
+  #include "thermistor_332.h"
+#endif
+#if ANY_THERMISTOR_IS(666) // beta25 = UNK, R25 = 200K, Pull-up = 10 kOhm, "Unidentified 200K NTC thermistor (Einstart S)"
+  #include "thermistor_666.h"
+#endif
+#if ANY_THERMISTOR_IS(1010) // Pt1000 with 1k0 pullup
+  #include "thermistor_1010.h"
+#endif
+#if ANY_THERMISTOR_IS(1047) // Pt1000 with 4k7 pullup
+  #include "thermistor_1047.h"
+#endif
+#if ANY_THERMISTOR_IS(998) // User-defined table 1
+  #include "thermistor_998.h"
+#endif
+#if ANY_THERMISTOR_IS(999) // User-defined table 2
+  #include "thermistor_999.h"
+#endif
+#if ANY_THERMISTOR_IS(1000) // Custom
+  const temp_entry_t temptable_1000[] PROGMEM = { { 0, 0 } };
+#endif
+
+#define _TT_NAME(_N) temptable_ ## _N
+#define TT_NAME(_N) _TT_NAME(_N)
+
+
+#if THERMISTOR_HEATER_0
+  #define HEATER_0_TEMPTABLE TT_NAME(THERMISTOR_HEATER_0)
+  #define HEATER_0_TEMPTABLE_LEN COUNT(HEATER_0_TEMPTABLE)
+#elif HEATER_0_USES_THERMISTOR
+  #error "No heater 0 thermistor table specified"
+#else
+  #define HEATER_0_TEMPTABLE nullptr
+  #define HEATER_0_TEMPTABLE_LEN 0
+#endif
+
+#if THERMISTOR_HEATER_1
+  #define HEATER_1_TEMPTABLE TT_NAME(THERMISTOR_HEATER_1)
+  #define HEATER_1_TEMPTABLE_LEN COUNT(HEATER_1_TEMPTABLE)
+#elif HEATER_1_USES_THERMISTOR
+  #error "No heater 1 thermistor table specified"
+#else
+  #define HEATER_1_TEMPTABLE nullptr
+  #define HEATER_1_TEMPTABLE_LEN 0
+#endif
+
+#if THERMISTOR_HEATER_2
+  #define HEATER_2_TEMPTABLE TT_NAME(THERMISTOR_HEATER_2)
+  #define HEATER_2_TEMPTABLE_LEN COUNT(HEATER_2_TEMPTABLE)
+#elif HEATER_2_USES_THERMISTOR
+  #error "No heater 2 thermistor table specified"
+#else
+  #define HEATER_2_TEMPTABLE nullptr
+  #define HEATER_2_TEMPTABLE_LEN 0
+#endif
+
+#if THERMISTOR_HEATER_3
+  #define HEATER_3_TEMPTABLE TT_NAME(THERMISTOR_HEATER_3)
+  #define HEATER_3_TEMPTABLE_LEN COUNT(HEATER_3_TEMPTABLE)
+#elif HEATER_3_USES_THERMISTOR
+  #error "No heater 3 thermistor table specified"
+#else
+  #define HEATER_3_TEMPTABLE nullptr
+  #define HEATER_3_TEMPTABLE_LEN 0
+#endif
+
+#if THERMISTOR_HEATER_4
+  #define HEATER_4_TEMPTABLE TT_NAME(THERMISTOR_HEATER_4)
+  #define HEATER_4_TEMPTABLE_LEN COUNT(HEATER_4_TEMPTABLE)
+#elif HEATER_4_USES_THERMISTOR
+  #error "No heater 4 thermistor table specified"
+#else
+  #define HEATER_4_TEMPTABLE nullptr
+  #define HEATER_4_TEMPTABLE_LEN 0
+#endif
+
+#if THERMISTOR_HEATER_5
+  #define HEATER_5_TEMPTABLE TT_NAME(THERMISTOR_HEATER_5)
+  #define HEATER_5_TEMPTABLE_LEN COUNT(HEATER_5_TEMPTABLE)
+#elif HEATER_5_USES_THERMISTOR
+  #error "No heater 5 thermistor table specified"
+#else
+  #define HEATER_5_TEMPTABLE nullptr
+  #define HEATER_5_TEMPTABLE_LEN 0
+#endif
+
+#if THERMISTOR_HEATER_6
+  #define HEATER_6_TEMPTABLE TT_NAME(THERMISTOR_HEATER_6)
+  #define HEATER_6_TEMPTABLE_LEN COUNT(HEATER_6_TEMPTABLE)
+#elif HEATER_6_USES_THERMISTOR
+  #error "No heater 6 thermistor table specified"
+#else
+  #define HEATER_6_TEMPTABLE nullptr
+  #define HEATER_6_TEMPTABLE_LEN 0
+#endif
+
+#if THERMISTOR_HEATER_7
+  #define HEATER_7_TEMPTABLE TT_NAME(THERMISTOR_HEATER_7)
+  #define HEATER_7_TEMPTABLE_LEN COUNT(HEATER_7_TEMPTABLE)
+#elif HEATER_7_USES_THERMISTOR
+  #error "No heater 7 thermistor table specified"
+#else
+  #define HEATER_7_TEMPTABLE nullptr
+  #define HEATER_7_TEMPTABLE_LEN 0
+#endif
+
+#ifdef THERMISTORBED
+  #define BED_TEMPTABLE TT_NAME(THERMISTORBED)
+  #define BED_TEMPTABLE_LEN COUNT(BED_TEMPTABLE)
+#elif HEATER_BED_USES_THERMISTOR
+  #error "No bed thermistor table specified"
+#else
+  #define BED_TEMPTABLE_LEN 0
+#endif
+
+#ifdef THERMISTORCHAMBER
+  #define CHAMBER_TEMPTABLE TT_NAME(THERMISTORCHAMBER)
+  #define CHAMBER_TEMPTABLE_LEN COUNT(CHAMBER_TEMPTABLE)
+#elif HEATER_CHAMBER_USES_THERMISTOR
+  #error "No chamber thermistor table specified"
+#else
+  #define CHAMBER_TEMPTABLE_LEN 0
+#endif
+
+#ifdef THERMISTORPROBE
+  #define PROBE_TEMPTABLE TT_NAME(THERMISTORPROBE)
+  #define PROBE_TEMPTABLE_LEN COUNT(PROBE_TEMPTABLE)
+#elif HEATER_PROBE_USES_THERMISTOR
+  #error "No probe thermistor table specified"
+#else
+  #define PROBE_TEMPTABLE_LEN 0
+#endif
+
+// The SCAN_THERMISTOR_TABLE macro needs alteration?
+static_assert(
+     HEATER_0_TEMPTABLE_LEN < 256 && HEATER_1_TEMPTABLE_LEN < 256
+  && HEATER_2_TEMPTABLE_LEN < 256 && HEATER_3_TEMPTABLE_LEN < 256
+  && HEATER_4_TEMPTABLE_LEN < 256 && HEATER_5_TEMPTABLE_LEN < 256
+  && HEATER_6_TEMPTABLE_LEN < 256 && HEATER_7_TEMPTABLE_LEN < 256
+  &&      BED_TEMPTABLE_LEN < 256 &&  CHAMBER_TEMPTABLE_LEN < 256
+  &&    PROBE_TEMPTABLE_LEN < 256,
+  "Temperature conversion tables over 255 entries need special consideration."
+);
+
+// Set the high and low raw values for the heaters
+// For thermistors the highest temperature results in the lowest ADC value
+// For thermocouples the highest temperature results in the highest ADC value
+
+#define _TT_REV(N) REVERSE_TEMP_SENSOR_RANGE_##N
+#define TT_REV(N) _TT_REV(N)
+
+#ifdef HEATER_0_TEMPTABLE
+  #if TT_REV(THERMISTOR_HEATER_0)
+    #define HEATER_0_SENSOR_MINTEMP_IND 0
+    #define HEATER_0_SENSOR_MAXTEMP_IND HEATER_0_TEMPTABLE_LEN - 1
+  #else
+    #define HEATER_0_SENSOR_MINTEMP_IND HEATER_0_TEMPTABLE_LEN - 1
+    #define HEATER_0_SENSOR_MAXTEMP_IND 0
+  #endif
+#endif
+#ifdef HEATER_1_TEMPTABLE
+  #if TT_REV(THERMISTOR_HEATER_1)
+    #define HEATER_1_SENSOR_MINTEMP_IND 0
+    #define HEATER_1_SENSOR_MAXTEMP_IND HEATER_1_TEMPTABLE_LEN - 1
+  #else
+    #define HEATER_1_SENSOR_MINTEMP_IND HEATER_1_TEMPTABLE_LEN - 1
+    #define HEATER_1_SENSOR_MAXTEMP_IND 0
+  #endif
+#endif
+#ifdef HEATER_2_TEMPTABLE
+  #if TT_REV(THERMISTOR_HEATER_2)
+    #define HEATER_2_SENSOR_MINTEMP_IND 0
+    #define HEATER_2_SENSOR_MAXTEMP_IND HEATER_2_TEMPTABLE_LEN - 1
+  #else
+    #define HEATER_2_SENSOR_MINTEMP_IND HEATER_2_TEMPTABLE_LEN - 1
+    #define HEATER_2_SENSOR_MAXTEMP_IND 0
+  #endif
+#endif
+#ifdef HEATER_3_TEMPTABLE
+  #if TT_REV(THERMISTOR_HEATER_3)
+    #define HEATER_3_SENSOR_MINTEMP_IND 0
+    #define HEATER_3_SENSOR_MAXTEMP_IND HEATER_3_TEMPTABLE_LEN - 1
+  #else
+    #define HEATER_3_SENSOR_MINTEMP_IND HEATER_3_TEMPTABLE_LEN - 1
+    #define HEATER_3_SENSOR_MAXTEMP_IND 0
+  #endif
+#endif
+#ifdef HEATER_4_TEMPTABLE
+  #if TT_REV(THERMISTOR_HEATER_4)
+    #define HEATER_4_SENSOR_MINTEMP_IND 0
+    #define HEATER_4_SENSOR_MAXTEMP_IND HEATER_4_TEMPTABLE_LEN - 1
+  #else
+    #define HEATER_4_SENSOR_MINTEMP_IND HEATER_4_TEMPTABLE_LEN - 1
+    #define HEATER_4_SENSOR_MAXTEMP_IND 0
+  #endif
+#endif
+#ifdef HEATER_5_TEMPTABLE
+  #if TT_REV(THERMISTOR_HEATER_5)
+    #define HEATER_5_SENSOR_MINTEMP_IND 0
+    #define HEATER_5_SENSOR_MAXTEMP_IND HEATER_5_TEMPTABLE_LEN - 1
+  #else
+    #define HEATER_5_SENSOR_MINTEMP_IND HEATER_5_TEMPTABLE_LEN - 1
+    #define HEATER_5_SENSOR_MAXTEMP_IND 0
+  #endif
+#endif
+#ifdef HEATER_6_TEMPTABLE
+  #if TT_REV(THERMISTOR_HEATER_6)
+    #define HEATER_6_SENSOR_MINTEMP_IND 0
+    #define HEATER_6_SENSOR_MAXTEMP_IND HEATER_6_TEMPTABLE_LEN - 1
+  #else
+    #define HEATER_6_SENSOR_MINTEMP_IND HEATER_6_TEMPTABLE_LEN - 1
+    #define HEATER_6_SENSOR_MAXTEMP_IND 0
+  #endif
+#endif
+#ifdef HEATER_7_TEMPTABLE
+  #if TT_REV(THERMISTOR_HEATER_7)
+    #define HEATER_7_SENSOR_MINTEMP_IND 0
+    #define HEATER_7_SENSOR_MAXTEMP_IND HEATER_7_TEMPTABLE_LEN - 1
+  #else
+    #define HEATER_7_SENSOR_MINTEMP_IND HEATER_7_TEMPTABLE_LEN - 1
+    #define HEATER_7_SENSOR_MAXTEMP_IND 0
+  #endif
+#endif
+
+#ifndef HEATER_0_RAW_HI_TEMP
+  #if TT_REV(THERMISTOR_HEATER_0) || !HEATER_0_USES_THERMISTOR
+    #define HEATER_0_RAW_HI_TEMP MAX_RAW_THERMISTOR_VALUE
+    #define HEATER_0_RAW_LO_TEMP 0
+  #else
+    #define HEATER_0_RAW_HI_TEMP 0
+    #define HEATER_0_RAW_LO_TEMP MAX_RAW_THERMISTOR_VALUE
+  #endif
+#endif
+#ifndef HEATER_1_RAW_HI_TEMP
+  #if TT_REV(THERMISTOR_HEATER_1) || !HEATER_1_USES_THERMISTOR
+    #define HEATER_1_RAW_HI_TEMP MAX_RAW_THERMISTOR_VALUE
+    #define HEATER_1_RAW_LO_TEMP 0
+  #else
+    #define HEATER_1_RAW_HI_TEMP 0
+    #define HEATER_1_RAW_LO_TEMP MAX_RAW_THERMISTOR_VALUE
+  #endif
+#endif
+#ifndef HEATER_2_RAW_HI_TEMP
+  #if TT_REV(THERMISTOR_HEATER_2) || !HEATER_2_USES_THERMISTOR
+    #define HEATER_2_RAW_HI_TEMP MAX_RAW_THERMISTOR_VALUE
+    #define HEATER_2_RAW_LO_TEMP 0
+  #else
+    #define HEATER_2_RAW_HI_TEMP 0
+    #define HEATER_2_RAW_LO_TEMP MAX_RAW_THERMISTOR_VALUE
+  #endif
+#endif
+#ifndef HEATER_3_RAW_HI_TEMP
+  #if TT_REV(THERMISTOR_HEATER_3) || !HEATER_3_USES_THERMISTOR
+    #define HEATER_3_RAW_HI_TEMP MAX_RAW_THERMISTOR_VALUE
+    #define HEATER_3_RAW_LO_TEMP 0
+  #else
+    #define HEATER_3_RAW_HI_TEMP 0
+    #define HEATER_3_RAW_LO_TEMP MAX_RAW_THERMISTOR_VALUE
+  #endif
+#endif
+#ifndef HEATER_4_RAW_HI_TEMP
+  #if TT_REV(THERMISTOR_HEATER_4) || !HEATER_4_USES_THERMISTOR
+    #define HEATER_4_RAW_HI_TEMP MAX_RAW_THERMISTOR_VALUE
+    #define HEATER_4_RAW_LO_TEMP 0
+  #else
+    #define HEATER_4_RAW_HI_TEMP 0
+    #define HEATER_4_RAW_LO_TEMP MAX_RAW_THERMISTOR_VALUE
+  #endif
+#endif
+#ifndef HEATER_5_RAW_HI_TEMP
+  #if TT_REV(THERMISTOR_HEATER_5) || !HEATER_5_USES_THERMISTOR
+    #define HEATER_5_RAW_HI_TEMP MAX_RAW_THERMISTOR_VALUE
+    #define HEATER_5_RAW_LO_TEMP 0
+  #else
+    #define HEATER_5_RAW_HI_TEMP 0
+    #define HEATER_5_RAW_LO_TEMP MAX_RAW_THERMISTOR_VALUE
+  #endif
+#endif
+#ifndef HEATER_6_RAW_HI_TEMP
+  #if TT_REV(THERMISTOR_HEATER_6) || !HEATER_6_USES_THERMISTOR
+    #define HEATER_6_RAW_HI_TEMP MAX_RAW_THERMISTOR_VALUE
+    #define HEATER_6_RAW_LO_TEMP 0
+  #else
+    #define HEATER_6_RAW_HI_TEMP 0
+    #define HEATER_6_RAW_LO_TEMP MAX_RAW_THERMISTOR_VALUE
+  #endif
+#endif
+#ifndef HEATER_7_RAW_HI_TEMP
+  #if TT_REV(THERMISTOR_HEATER_7) || !HEATER_7_USES_THERMISTOR
+    #define HEATER_7_RAW_HI_TEMP MAX_RAW_THERMISTOR_VALUE
+    #define HEATER_7_RAW_LO_TEMP 0
+  #else
+    #define HEATER_7_RAW_HI_TEMP 0
+    #define HEATER_7_RAW_LO_TEMP MAX_RAW_THERMISTOR_VALUE
+  #endif
+#endif
+#ifndef HEATER_BED_RAW_HI_TEMP
+  #if TT_REV(THERMISTORBED) || !HEATER_BED_USES_THERMISTOR
+    #define HEATER_BED_RAW_HI_TEMP MAX_RAW_THERMISTOR_VALUE
+    #define HEATER_BED_RAW_LO_TEMP 0
+  #else
+    #define HEATER_BED_RAW_HI_TEMP 0
+    #define HEATER_BED_RAW_LO_TEMP MAX_RAW_THERMISTOR_VALUE
+  #endif
+#endif
+#ifndef HEATER_CHAMBER_RAW_HI_TEMP
+  #if TT_REV(THERMISTORCHAMBER) || !HEATER_CHAMBER_USES_THERMISTOR
+    #define HEATER_CHAMBER_RAW_HI_TEMP MAX_RAW_THERMISTOR_VALUE
+    #define HEATER_CHAMBER_RAW_LO_TEMP 0
+  #else
+    #define HEATER_CHAMBER_RAW_HI_TEMP 0
+    #define HEATER_CHAMBER_RAW_LO_TEMP MAX_RAW_THERMISTOR_VALUE
+  #endif
+#endif
+#ifndef HEATER_PROBE_RAW_HI_TEMP
+  #if TT_REV(THERMISTORPROBE) || !HEATER_PROBE_USES_THERMISTOR
+    #define HEATER_PROBE_RAW_HI_TEMP MAX_RAW_THERMISTOR_VALUE
+    #define HEATER_PROBE_RAW_LO_TEMP 0
+  #else
+    #define HEATER_PROBE_RAW_HI_TEMP 0
+    #define HEATER_PROBE_RAW_LO_TEMP MAX_RAW_THERMISTOR_VALUE
+  #endif
+#endif
+
+#undef _TT_REV
+#undef TT_REV
diff --git a/Marlin/src/module/tool_change.cpp b/Marlin/src/module/tool_change.cpp
new file mode 100644
index 0000000..4278e6b
--- /dev/null
+++ b/Marlin/src/module/tool_change.cpp
@@ -0,0 +1,1296 @@
+/**
+ * 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 "../inc/MarlinConfigPre.h"
+
+#include "tool_change.h"
+
+#include "probe.h"
+#include "motion.h"
+#include "planner.h"
+#include "temperature.h"
+
+#include "../MarlinCore.h"
+
+//#define DEBUG_TOOL_CHANGE
+
+#define DEBUG_OUT ENABLED(DEBUG_TOOL_CHANGE)
+#include "../core/debug_out.h"
+
+#if HAS_MULTI_EXTRUDER
+  toolchange_settings_t toolchange_settings;  // Initialized by settings.load()
+#endif
+
+#if ENABLED(TOOLCHANGE_MIGRATION_FEATURE)
+  migration_settings_t migration = migration_defaults;
+  bool enable_first_prime;
+#endif
+
+#if ENABLED(TOOLCHANGE_FS_INIT_BEFORE_SWAP)
+  bool toolchange_extruder_ready[EXTRUDERS];
+#endif
+
+#if ENABLED(MAGNETIC_PARKING_EXTRUDER) || defined(EVENT_GCODE_AFTER_TOOLCHANGE) || (ENABLED(PARKING_EXTRUDER) && PARKING_EXTRUDER_SOLENOIDS_DELAY > 0)
+  #include "../gcode/gcode.h"
+#endif
+
+#if ENABLED(DUAL_X_CARRIAGE)
+  #include "stepper.h"
+#endif
+
+#if ANY(SWITCHING_EXTRUDER, SWITCHING_NOZZLE, SWITCHING_TOOLHEAD)
+  #include "servo.h"
+#endif
+
+#if ENABLED(EXT_SOLENOID) && DISABLED(PARKING_EXTRUDER)
+  #include "../feature/solenoid.h"
+#endif
+
+#if ENABLED(MIXING_EXTRUDER)
+  #include "../feature/mixing.h"
+#endif
+
+#if HAS_LEVELING
+  #include "../feature/bedlevel/bedlevel.h"
+#endif
+
+#if HAS_FANMUX
+  #include "../feature/fanmux.h"
+#endif
+
+#if HAS_PRUSA_MMU1
+  #include "../feature/mmu/mmu.h"
+#elif HAS_PRUSA_MMU2
+  #include "../feature/mmu/mmu2.h"
+#endif
+
+#if HAS_LCD_MENU
+  #include "../lcd/marlinui.h"
+#endif
+
+#if ENABLED(ADVANCED_PAUSE_FEATURE)
+  #include "../feature/pause.h"
+#endif
+
+#if ENABLED(TOOLCHANGE_FILAMENT_SWAP)
+  #include "../gcode/gcode.h"
+  #if TOOLCHANGE_FS_WIPE_RETRACT <= 0
+    #undef TOOLCHANGE_FS_WIPE_RETRACT
+    #define TOOLCHANGE_FS_WIPE_RETRACT 0
+  #endif
+#endif
+
+#if DO_SWITCH_EXTRUDER
+
+  #if EXTRUDERS > 3
+    #define _SERVO_NR(E) ((E) < 2 ? SWITCHING_EXTRUDER_SERVO_NR : SWITCHING_EXTRUDER_E23_SERVO_NR)
+  #else
+    #define _SERVO_NR(E) SWITCHING_EXTRUDER_SERVO_NR
+  #endif
+
+  void move_extruder_servo(const uint8_t e) {
+    planner.synchronize();
+    #if EXTRUDERS & 1
+      if (e < EXTRUDERS - 1)
+    #endif
+    {
+      MOVE_SERVO(_SERVO_NR(e), servo_angles[_SERVO_NR(e)][e & 1]);
+      safe_delay(500);
+    }
+  }
+
+#endif // DO_SWITCH_EXTRUDER
+
+#if ENABLED(SWITCHING_NOZZLE)
+
+  #if SWITCHING_NOZZLE_TWO_SERVOS
+
+    inline void _move_nozzle_servo(const uint8_t e, const uint8_t angle_index) {
+      constexpr int8_t  sns_index[2] = { SWITCHING_NOZZLE_SERVO_NR, SWITCHING_NOZZLE_E1_SERVO_NR };
+      constexpr int16_t sns_angles[2] = SWITCHING_NOZZLE_SERVO_ANGLES;
+      planner.synchronize();
+      MOVE_SERVO(sns_index[e], sns_angles[angle_index]);
+      safe_delay(500);
+    }
+
+    void lower_nozzle(const uint8_t e) { _move_nozzle_servo(e, 0); }
+    void raise_nozzle(const uint8_t e) { _move_nozzle_servo(e, 1); }
+
+  #else
+
+    void move_nozzle_servo(const uint8_t angle_index) {
+      planner.synchronize();
+      MOVE_SERVO(SWITCHING_NOZZLE_SERVO_NR, servo_angles[SWITCHING_NOZZLE_SERVO_NR][angle_index]);
+      safe_delay(500);
+    }
+
+  #endif
+
+#endif // SWITCHING_NOZZLE
+
+inline void _line_to_current(const AxisEnum fr_axis, const float fscale=1) {
+  line_to_current_position(planner.settings.max_feedrate_mm_s[fr_axis] * fscale);
+}
+inline void slow_line_to_current(const AxisEnum fr_axis) { _line_to_current(fr_axis, 0.5f); }
+inline void fast_line_to_current(const AxisEnum fr_axis) { _line_to_current(fr_axis); }
+
+#if ENABLED(MAGNETIC_PARKING_EXTRUDER)
+
+  float parkingposx[2],           // M951 R L
+        parkinggrabdistance,      // M951 I
+        parkingslowspeed,         // M951 J
+        parkinghighspeed,         // M951 H
+        parkingtraveldistance,    // M951 D
+        compensationmultiplier;
+
+  inline void magnetic_parking_extruder_tool_change(const uint8_t new_tool) {
+
+    const float oldx = current_position.x,
+                grabpos = mpe_settings.parking_xpos[new_tool] + (new_tool ? mpe_settings.grab_distance : -mpe_settings.grab_distance),
+                offsetcompensation = TERN0(HAS_HOTEND_OFFSET, hotend_offset[active_extruder].x * mpe_settings.compensation_factor);
+
+    if (homing_needed_error(_BV(X_AXIS))) return;
+
+    /**
+     * Z Lift and Nozzle Offset shift ar defined in caller method to work equal with any Multi Hotend realization
+     *
+     * Steps:
+     *   1. Move high speed to park position of new extruder
+     *   2. Move to couple position of new extruder (this also discouple the old extruder)
+     *   3. Move to park position of new extruder
+     *   4. Move high speed to approach park position of old extruder
+     *   5. Move to park position of old extruder
+     *   6. Move to starting position
+     */
+
+    // STEP 1
+
+    current_position.x = mpe_settings.parking_xpos[new_tool] + offsetcompensation;
+
+    DEBUG_ECHOPAIR("(1) Move extruder ", int(new_tool));
+    DEBUG_POS(" to new extruder ParkPos", current_position);
+
+    planner.buffer_line(current_position, mpe_settings.fast_feedrate, new_tool);
+    planner.synchronize();
+
+    // STEP 2
+
+    current_position.x = grabpos + offsetcompensation;
+
+    DEBUG_ECHOPAIR("(2) Couple extruder ", int(new_tool));
+    DEBUG_POS(" to new extruder GrabPos", current_position);
+
+    planner.buffer_line(current_position, mpe_settings.slow_feedrate, new_tool);
+    planner.synchronize();
+
+    // Delay before moving tool, to allow magnetic coupling
+    gcode.dwell(150);
+
+    // STEP 3
+
+    current_position.x = mpe_settings.parking_xpos[new_tool] + offsetcompensation;
+
+    DEBUG_ECHOPAIR("(3) Move extruder ", int(new_tool));
+    DEBUG_POS(" back to new extruder ParkPos", current_position);
+
+    planner.buffer_line(current_position, mpe_settings.slow_feedrate, new_tool);
+    planner.synchronize();
+
+    // STEP 4
+
+    current_position.x = mpe_settings.parking_xpos[active_extruder] + (active_extruder == 0 ? MPE_TRAVEL_DISTANCE : -MPE_TRAVEL_DISTANCE) + offsetcompensation;
+
+    DEBUG_ECHOPAIR("(4) Move extruder ", int(new_tool));
+    DEBUG_POS(" close to old extruder ParkPos", current_position);
+
+    planner.buffer_line(current_position, mpe_settings.fast_feedrate, new_tool);
+    planner.synchronize();
+
+    // STEP 5
+
+    current_position.x = mpe_settings.parking_xpos[active_extruder] + offsetcompensation;
+
+    DEBUG_ECHOPAIR("(5) Park extruder ", int(new_tool));
+    DEBUG_POS(" at old extruder ParkPos", current_position);
+
+    planner.buffer_line(current_position, mpe_settings.slow_feedrate, new_tool);
+    planner.synchronize();
+
+    // STEP 6
+
+    current_position.x = oldx;
+
+    DEBUG_ECHOPAIR("(6) Move extruder ", int(new_tool));
+    DEBUG_POS(" to starting position", current_position);
+
+    planner.buffer_line(current_position, mpe_settings.fast_feedrate, new_tool);
+    planner.synchronize();
+
+    DEBUG_ECHOLNPGM("Autopark done.");
+  }
+
+#elif ENABLED(PARKING_EXTRUDER)
+
+  void pe_solenoid_init() {
+    LOOP_LE_N(n, 1) pe_solenoid_set_pin_state(n, !PARKING_EXTRUDER_SOLENOIDS_PINS_ACTIVE);
+  }
+
+  void pe_solenoid_set_pin_state(const uint8_t extruder_num, const uint8_t state) {
+    switch (extruder_num) {
+      case 1: OUT_WRITE(SOL1_PIN, state); break;
+      default: OUT_WRITE(SOL0_PIN, state); break;
+    }
+    #if PARKING_EXTRUDER_SOLENOIDS_DELAY > 0
+      gcode.dwell(PARKING_EXTRUDER_SOLENOIDS_DELAY);
+    #endif
+  }
+
+  bool extruder_parked = true, do_solenoid_activation = true;
+
+  // Modifies tool_change() behavior based on homing side
+  bool parking_extruder_unpark_after_homing(const uint8_t final_tool, bool homed_towards_final_tool) {
+    do_solenoid_activation = false; // Tell parking_extruder_tool_change to skip solenoid activation
+
+    if (!extruder_parked) return false; // nothing to do
+
+    if (homed_towards_final_tool) {
+      pe_solenoid_magnet_off(1 - final_tool);
+      DEBUG_ECHOLNPAIR("Disengage magnet", (int)(1 - final_tool));
+      pe_solenoid_magnet_on(final_tool);
+      DEBUG_ECHOLNPAIR("Engage magnet", (int)final_tool);
+      parking_extruder_set_parked(false);
+      return false;
+    }
+
+    return true;
+  }
+
+  inline void parking_extruder_tool_change(const uint8_t new_tool, bool no_move) {
+    if (!no_move) {
+
+      constexpr float parkingposx[] = PARKING_EXTRUDER_PARKING_X;
+
+      #if HAS_HOTEND_OFFSET
+        const float x_offset = hotend_offset[active_extruder].x;
+      #else
+        constexpr float x_offset = 0;
+      #endif
+
+      const float midpos = (parkingposx[0] + parkingposx[1]) * 0.5f + x_offset,
+                  grabpos = parkingposx[new_tool] + (new_tool ? PARKING_EXTRUDER_GRAB_DISTANCE : -(PARKING_EXTRUDER_GRAB_DISTANCE)) + x_offset;
+
+      /**
+       * 1. Move to park position of old extruder
+       * 2. Disengage magnetic field, wait for delay
+       * 3. Move near new extruder
+       * 4. Engage magnetic field for new extruder
+       * 5. Move to parking incl. offset of new extruder
+       * 6. Lower Z-Axis
+       */
+
+      // STEP 1
+
+      DEBUG_POS("Start PE Tool-Change", current_position);
+
+      // Don't park the active_extruder unless unparked
+      if (!extruder_parked) {
+        current_position.x = parkingposx[active_extruder] + x_offset;
+
+        DEBUG_ECHOLNPAIR("(1) Park extruder ", int(active_extruder));
+        DEBUG_POS("Moving ParkPos", current_position);
+
+        fast_line_to_current(X_AXIS);
+
+        // STEP 2
+
+        planner.synchronize();
+        DEBUG_ECHOLNPGM("(2) Disengage magnet");
+        pe_solenoid_magnet_off(active_extruder);
+
+        // STEP 3
+
+        current_position.x += active_extruder ? -10 : 10; // move 10mm away from parked extruder
+
+        DEBUG_ECHOLNPGM("(3) Move near new extruder");
+        DEBUG_POS("Move away from parked extruder", current_position);
+
+        fast_line_to_current(X_AXIS);
+      }
+
+      // STEP 4
+
+      planner.synchronize();
+      DEBUG_ECHOLNPGM("(4) Engage magnetic field");
+
+      // Just save power for inverted magnets
+      TERN_(PARKING_EXTRUDER_SOLENOIDS_INVERT, pe_solenoid_magnet_on(active_extruder));
+      pe_solenoid_magnet_on(new_tool);
+
+      // STEP 5
+
+      current_position.x = grabpos + (new_tool ? -10 : 10);
+      fast_line_to_current(X_AXIS);
+
+      current_position.x = grabpos;
+
+      DEBUG_SYNCHRONIZE();
+      DEBUG_POS("(5) Unpark extruder", current_position);
+
+      slow_line_to_current(X_AXIS);
+
+      // STEP 6
+
+      current_position.x = midpos - TERN0(HAS_HOTEND_OFFSET, hotend_offset[new_tool].x);
+
+      DEBUG_SYNCHRONIZE();
+      DEBUG_POS("(6) Move midway between hotends", current_position);
+
+      fast_line_to_current(X_AXIS);
+      planner.synchronize(); // Always sync the final move
+
+      DEBUG_POS("PE Tool-Change done.", current_position);
+      parking_extruder_set_parked(false);
+    }
+    else if (do_solenoid_activation) { // && nomove == true
+      // Deactivate current extruder solenoid
+      pe_solenoid_set_pin_state(active_extruder, !PARKING_EXTRUDER_SOLENOIDS_PINS_ACTIVE);
+      // Engage new extruder magnetic field
+      pe_solenoid_set_pin_state(new_tool, PARKING_EXTRUDER_SOLENOIDS_PINS_ACTIVE);
+    }
+
+    do_solenoid_activation = true; // Activate solenoid for subsequent tool_change()
+  }
+
+#endif // PARKING_EXTRUDER
+
+#if ENABLED(SWITCHING_TOOLHEAD)
+
+  inline void swt_lock(const bool locked=true) {
+    const uint16_t swt_angles[2] = SWITCHING_TOOLHEAD_SERVO_ANGLES;
+    MOVE_SERVO(SWITCHING_TOOLHEAD_SERVO_NR, swt_angles[locked ? 0 : 1]);
+  }
+
+  void swt_init() { swt_lock(); }
+
+  inline void switching_toolhead_tool_change(const uint8_t new_tool, bool no_move/*=false*/) {
+    if (no_move) return;
+
+    constexpr float toolheadposx[] = SWITCHING_TOOLHEAD_X_POS;
+    const float placexpos = toolheadposx[active_extruder],
+                grabxpos = toolheadposx[new_tool];
+
+    /**
+     * 1. Move to switch position of current toolhead
+     * 2. Unlock tool and drop it in the dock
+     * 3. Move to the new toolhead
+     * 4. Grab and lock the new toolhead
+     */
+
+    // 1. Move to switch position of current toolhead
+
+    DEBUG_POS("Start ST Tool-Change", current_position);
+
+    current_position.x = placexpos;
+
+    DEBUG_ECHOLNPAIR("(1) Place old tool ", int(active_extruder));
+    DEBUG_POS("Move X SwitchPos", current_position);
+
+    fast_line_to_current(X_AXIS);
+
+    current_position.y = SWITCHING_TOOLHEAD_Y_POS - (SWITCHING_TOOLHEAD_Y_SECURITY);
+
+    DEBUG_SYNCHRONIZE();
+    DEBUG_POS("Move Y SwitchPos + Security", current_position);
+
+    fast_line_to_current(Y_AXIS);
+
+    // 2. Unlock tool and drop it in the dock
+
+    planner.synchronize();
+    DEBUG_ECHOLNPGM("(2) Unlock and Place Toolhead");
+    swt_lock(false);
+    safe_delay(500);
+
+    current_position.y = SWITCHING_TOOLHEAD_Y_POS;
+    DEBUG_POS("Move Y SwitchPos", current_position);
+    slow_line_to_current(Y_AXIS);
+
+    // Wait for move to complete, then another 0.2s
+    planner.synchronize();
+    safe_delay(200);
+
+    current_position.y -= SWITCHING_TOOLHEAD_Y_CLEAR;
+    DEBUG_POS("Move back Y clear", current_position);
+    fast_line_to_current(Y_AXIS); // move away from docked toolhead
+
+    // 3. Move to the new toolhead
+
+    current_position.x = grabxpos;
+
+    DEBUG_SYNCHRONIZE();
+    DEBUG_ECHOLNPGM("(3) Move to new toolhead position");
+    DEBUG_POS("Move to new toolhead X", current_position);
+
+    fast_line_to_current(X_AXIS);
+
+    current_position.y = SWITCHING_TOOLHEAD_Y_POS - (SWITCHING_TOOLHEAD_Y_SECURITY);
+
+    DEBUG_SYNCHRONIZE();
+    DEBUG_POS("Move Y SwitchPos + Security", current_position);
+
+    fast_line_to_current(Y_AXIS);
+
+    // 4. Grab and lock the new toolhead
+
+    current_position.y = SWITCHING_TOOLHEAD_Y_POS;
+
+    DEBUG_SYNCHRONIZE();
+    DEBUG_ECHOLNPGM("(4) Grab and lock new toolhead");
+    DEBUG_POS("Move Y SwitchPos", current_position);
+
+    slow_line_to_current(Y_AXIS);
+
+    // Wait for move to finish, pause 0.2s, move servo, pause 0.5s
+    planner.synchronize();
+    safe_delay(200);
+    swt_lock();
+    safe_delay(500);
+
+    current_position.y -= SWITCHING_TOOLHEAD_Y_CLEAR;
+    DEBUG_POS("Move back Y clear", current_position);
+    fast_line_to_current(Y_AXIS); // Move away from docked toolhead
+    planner.synchronize();        // Always sync the final move
+
+    DEBUG_POS("ST Tool-Change done.", current_position);
+  }
+
+#elif ENABLED(MAGNETIC_SWITCHING_TOOLHEAD)
+
+  inline void magnetic_switching_toolhead_tool_change(const uint8_t new_tool, bool no_move/*=false*/) {
+    if (no_move) return;
+
+    constexpr float toolheadposx[] = SWITCHING_TOOLHEAD_X_POS,
+                    toolheadclearx[] = SWITCHING_TOOLHEAD_X_SECURITY;
+
+    const float placexpos = toolheadposx[active_extruder],
+                placexclear = toolheadclearx[active_extruder],
+                grabxpos = toolheadposx[new_tool],
+                grabxclear = toolheadclearx[new_tool];
+
+    /**
+     * 1. Move to switch position of current toolhead
+     * 2. Release and place toolhead in the dock
+     * 3. Move to the new toolhead
+     * 4. Grab the new toolhead and move to security position
+     */
+
+    DEBUG_POS("Start MST Tool-Change", current_position);
+
+    // 1. Move to switch position current toolhead
+
+    current_position.y = SWITCHING_TOOLHEAD_Y_POS + SWITCHING_TOOLHEAD_Y_CLEAR;
+
+    SERIAL_ECHOLNPAIR("(1) Place old tool ", int(active_extruder));
+    DEBUG_POS("Move Y SwitchPos + Security", current_position);
+
+    fast_line_to_current(Y_AXIS);
+
+    current_position.x = placexclear;
+
+    DEBUG_SYNCHRONIZE();
+    DEBUG_POS("Move X SwitchPos + Security", current_position);
+
+    fast_line_to_current(X_AXIS);
+
+    current_position.y = SWITCHING_TOOLHEAD_Y_POS;
+
+    DEBUG_SYNCHRONIZE();
+    DEBUG_POS("Move Y SwitchPos", current_position);
+
+    fast_line_to_current(Y_AXIS);
+
+    current_position.x = placexpos;
+
+    DEBUG_SYNCHRONIZE();
+    DEBUG_POS("Move X SwitchPos", current_position);
+
+    line_to_current_position(planner.settings.max_feedrate_mm_s[X_AXIS] * 0.25f);
+
+    // 2. Release and place toolhead in the dock
+
+    DEBUG_SYNCHRONIZE();
+    DEBUG_ECHOLNPGM("(2) Release and Place Toolhead");
+
+    current_position.y = SWITCHING_TOOLHEAD_Y_POS + SWITCHING_TOOLHEAD_Y_RELEASE;
+    DEBUG_POS("Move Y SwitchPos + Release", current_position);
+    line_to_current_position(planner.settings.max_feedrate_mm_s[Y_AXIS] * 0.1f);
+
+    current_position.y = SWITCHING_TOOLHEAD_Y_POS + SWITCHING_TOOLHEAD_Y_SECURITY;
+
+    DEBUG_SYNCHRONIZE();
+    DEBUG_POS("Move Y SwitchPos + Security", current_position);
+
+    line_to_current_position(planner.settings.max_feedrate_mm_s[Y_AXIS]);
+
+    // 3. Move to new toolhead position
+
+    DEBUG_SYNCHRONIZE();
+    DEBUG_ECHOLNPGM("(3) Move to new toolhead position");
+
+    current_position.x = grabxpos;
+    DEBUG_POS("Move to new toolhead X", current_position);
+    fast_line_to_current(X_AXIS);
+
+    // 4. Grab the new toolhead and move to security position
+
+    DEBUG_SYNCHRONIZE();
+    DEBUG_ECHOLNPGM("(4) Grab new toolhead, move to security position");
+
+    current_position.y = SWITCHING_TOOLHEAD_Y_POS + SWITCHING_TOOLHEAD_Y_RELEASE;
+    DEBUG_POS("Move Y SwitchPos + Release", current_position);
+    line_to_current_position(planner.settings.max_feedrate_mm_s[Y_AXIS]);
+
+    current_position.y = SWITCHING_TOOLHEAD_Y_POS;
+
+    DEBUG_SYNCHRONIZE();
+    DEBUG_POS("Move Y SwitchPos", current_position);
+
+    _line_to_current(Y_AXIS, 0.2f);
+
+    #if ENABLED(PRIME_BEFORE_REMOVE) && (SWITCHING_TOOLHEAD_PRIME_MM || SWITCHING_TOOLHEAD_RETRACT_MM)
+      #if SWITCHING_TOOLHEAD_PRIME_MM
+        current_position.e += SWITCHING_TOOLHEAD_PRIME_MM;
+        planner.buffer_line(current_position, MMM_TO_MMS(SWITCHING_TOOLHEAD_PRIME_FEEDRATE), new_tool);
+      #endif
+      #if SWITCHING_TOOLHEAD_RETRACT_MM
+        current_position.e -= SWITCHING_TOOLHEAD_RETRACT_MM;
+        planner.buffer_line(current_position, MMM_TO_MMS(SWITCHING_TOOLHEAD_RETRACT_FEEDRATE), new_tool);
+      #endif
+    #else
+      planner.synchronize();
+      safe_delay(100); // Give switch time to settle
+    #endif
+
+    current_position.x = grabxclear;
+    DEBUG_POS("Move to new toolhead X + Security", current_position);
+    _line_to_current(X_AXIS, 0.1f);
+    planner.synchronize();
+    safe_delay(100); // Give switch time to settle
+
+    current_position.y += SWITCHING_TOOLHEAD_Y_CLEAR;
+    DEBUG_POS("Move back Y clear", current_position);
+    fast_line_to_current(Y_AXIS); // move away from docked toolhead
+    planner.synchronize(); // Always sync last tool-change move
+
+    DEBUG_POS("MST Tool-Change done.", current_position);
+  }
+
+#elif ENABLED(ELECTROMAGNETIC_SWITCHING_TOOLHEAD)
+
+  inline void est_activate_solenoid()   { OUT_WRITE(SOL0_PIN, HIGH); }
+  inline void est_deactivate_solenoid() { OUT_WRITE(SOL0_PIN, LOW); }
+  void est_init() { est_activate_solenoid(); }
+
+  inline void em_switching_toolhead_tool_change(const uint8_t new_tool, bool no_move) {
+    if (no_move) return;
+
+    constexpr float toolheadposx[] = SWITCHING_TOOLHEAD_X_POS;
+    const float placexpos = toolheadposx[active_extruder],
+                grabxpos = toolheadposx[new_tool];
+    const xyz_pos_t &hoffs = hotend_offset[active_extruder];
+
+    /**
+     * 1. Raise Z-Axis to give enough clearance
+     * 2. Move to position near active extruder parking
+     * 3. Move gently to park position of active extruder
+     * 4. Disengage magnetic field, wait for delay
+     * 5. Leave extruder and move to position near new extruder parking
+     * 6. Move gently to park position of new extruder
+     * 7. Engage magnetic field for new extruder parking
+     * 8. Unpark extruder
+     * 9. Apply Z hotend offset to current position
+     */
+
+    DEBUG_POS("Start EMST Tool-Change", current_position);
+
+    // 1. Raise Z-Axis to give enough clearance
+
+    current_position.z += SWITCHING_TOOLHEAD_Z_HOP;
+    DEBUG_POS("(1) Raise Z-Axis ", current_position);
+    fast_line_to_current(Z_AXIS);
+
+    // 2. Move to position near active extruder parking
+
+    DEBUG_SYNCHRONIZE();
+    DEBUG_ECHOLNPAIR("(2) Move near active extruder parking", active_extruder);
+    DEBUG_POS("Moving ParkPos", current_position);
+
+    current_position.set(hoffs.x + placexpos,
+                         hoffs.y + SWITCHING_TOOLHEAD_Y_POS + SWITCHING_TOOLHEAD_Y_CLEAR);
+    fast_line_to_current(X_AXIS);
+
+    // 3. Move gently to park position of active extruder
+
+    DEBUG_SYNCHRONIZE();
+    SERIAL_ECHOLNPAIR("(3) Move gently to park position of active extruder", active_extruder);
+    DEBUG_POS("Moving ParkPos", current_position);
+
+    current_position.y -= SWITCHING_TOOLHEAD_Y_CLEAR;
+    slow_line_to_current(Y_AXIS);
+
+    // 4. Disengage magnetic field, wait for delay
+
+    planner.synchronize();
+    DEBUG_ECHOLNPGM("(4) Disengage magnet");
+    est_deactivate_solenoid();
+
+    // 5. Leave extruder and move to position near new extruder parking
+
+    DEBUG_ECHOLNPGM("(5) Move near new extruder parking");
+    DEBUG_POS("Moving ParkPos", current_position);
+
+    current_position.y += SWITCHING_TOOLHEAD_Y_CLEAR;
+    slow_line_to_current(Y_AXIS);
+    current_position.set(hoffs.x + grabxpos,
+                         hoffs.y + SWITCHING_TOOLHEAD_Y_POS + SWITCHING_TOOLHEAD_Y_CLEAR);
+    fast_line_to_current(X_AXIS);
+
+    // 6. Move gently to park position of new extruder
+
+    current_position.y -= SWITCHING_TOOLHEAD_Y_CLEAR;
+    if (DEBUGGING(LEVELING)) {
+      planner.synchronize();
+      DEBUG_ECHOLNPGM("(6) Move near new extruder");
+    }
+    slow_line_to_current(Y_AXIS);
+
+    // 7. Engage magnetic field for new extruder parking
+
+    DEBUG_SYNCHRONIZE();
+    DEBUG_ECHOLNPGM("(7) Engage magnetic field");
+    est_activate_solenoid();
+
+    // 8. Unpark extruder
+
+    current_position.y += SWITCHING_TOOLHEAD_Y_CLEAR;
+    DEBUG_ECHOLNPGM("(8) Unpark extruder");
+    slow_line_to_current(X_AXIS);
+    planner.synchronize(); // Always sync the final move
+
+    // 9. Apply Z hotend offset to current position
+
+    DEBUG_POS("(9) Applying Z-offset", current_position);
+    current_position.z += hoffs.z - hotend_offset[new_tool].z;
+
+    DEBUG_POS("EMST Tool-Change done.", current_position);
+  }
+
+#endif // ELECTROMAGNETIC_SWITCHING_TOOLHEAD
+
+#if EXTRUDERS
+  inline void invalid_extruder_error(const uint8_t e) {
+    SERIAL_ECHO_START();
+    SERIAL_CHAR('T'); SERIAL_ECHO((int)e);
+    SERIAL_CHAR(' '); SERIAL_ECHOLNPGM(STR_INVALID_EXTRUDER);
+  }
+#endif
+
+#if ENABLED(DUAL_X_CARRIAGE)
+
+  /**
+   * @brief Dual X Tool Change
+   * @details Change tools, with extra behavior based on current mode
+   *
+   * @param new_tool Tool index to activate
+   * @param no_move Flag indicating no moves should take place
+   */
+  inline void dualx_tool_change(const uint8_t new_tool, bool &no_move) {
+
+    DEBUG_ECHOPGM("Dual X Carriage Mode ");
+    switch (dual_x_carriage_mode) {
+      case DXC_FULL_CONTROL_MODE: DEBUG_ECHOLNPGM("FULL_CONTROL"); break;
+      case DXC_AUTO_PARK_MODE:    DEBUG_ECHOLNPGM("AUTO_PARK");    break;
+      case DXC_DUPLICATION_MODE:  DEBUG_ECHOLNPGM("DUPLICATION");  break;
+      case DXC_MIRRORED_MODE:     DEBUG_ECHOLNPGM("MIRRORED");     break;
+    }
+
+    // Get the home position of the currently-active tool
+    const float xhome = x_home_pos(active_extruder);
+
+    if (dual_x_carriage_mode == DXC_AUTO_PARK_MODE                  // If Auto-Park mode is enabled
+        && IsRunning() && !no_move                                  // ...and movement is permitted
+        && (delayed_move_time || current_position.x != xhome)       // ...and delayed_move_time is set OR not "already parked"...
+    ) {
+      DEBUG_ECHOLNPAIR("MoveX to ", xhome);
+      current_position.x = xhome;
+      line_to_current_position(planner.settings.max_feedrate_mm_s[X_AXIS]);   // Park the current head
+      planner.synchronize();
+    }
+
+    // Activate the new extruder ahead of calling set_axis_is_at_home!
+    active_extruder = new_tool;
+
+    // This function resets the max/min values - the current position may be overwritten below.
+    set_axis_is_at_home(X_AXIS);
+
+    DEBUG_POS("New Extruder", current_position);
+
+    switch (dual_x_carriage_mode) {
+      case DXC_FULL_CONTROL_MODE:
+        // New current position is the position of the activated extruder
+        current_position.x = inactive_extruder_x;
+        // Save the inactive extruder's position (from the old current_position)
+        inactive_extruder_x = destination.x;
+        DEBUG_ECHOLNPAIR("DXC Full Control curr.x=", current_position.x, " dest.x=", destination.x);
+        break;
+      case DXC_AUTO_PARK_MODE:
+        idex_set_parked();
+        break;
+      default:
+        break;
+    }
+
+    // Ensure X axis DIR pertains to the correct carriage
+    stepper.set_directions();
+
+    DEBUG_ECHOLNPAIR("Active extruder parked: ", active_extruder_parked ? "yes" : "no");
+    DEBUG_POS("New extruder (parked)", current_position);
+  }
+
+#endif // DUAL_X_CARRIAGE
+
+/**
+ * Prime active tool using TOOLCHANGE_FILAMENT_SWAP settings
+ */
+#if ENABLED(TOOLCHANGE_FILAMENT_SWAP)
+
+  void tool_change_prime() {
+    if (toolchange_settings.extra_prime > 0
+      && TERN(PREVENT_COLD_EXTRUSION, !thermalManager.targetTooColdToExtrude(active_extruder), 1)
+    ) {
+      destination = current_position; // Remember the old position
+
+      const bool ok = TERN1(TOOLCHANGE_PARK, all_axes_homed() && toolchange_settings.enable_park);
+
+      #if HAS_FAN && TOOLCHANGE_FS_FAN >= 0
+        // Store and stop fan. Restored on any exit.
+        REMEMBER(fan, thermalManager.fan_speed[TOOLCHANGE_FS_FAN], 0);
+      #endif
+
+      // Z raise
+      if (ok) {
+        // Do a small lift to avoid the workpiece in the move back (below)
+        current_position.z += toolchange_settings.z_raise;
+        #if HAS_SOFTWARE_ENDSTOPS
+          NOMORE(current_position.z, soft_endstop.max.z);
+        #endif
+        fast_line_to_current(Z_AXIS);
+        planner.synchronize();
+      }
+
+      // Park
+      #if ENABLED(TOOLCHANGE_PARK)
+        if (ok) {
+          IF_DISABLED(TOOLCHANGE_PARK_Y_ONLY, current_position.x = toolchange_settings.change_point.x);
+          IF_DISABLED(TOOLCHANGE_PARK_X_ONLY, current_position.y = toolchange_settings.change_point.y);
+          planner.buffer_line(current_position, MMM_TO_MMS(TOOLCHANGE_PARK_XY_FEEDRATE), active_extruder);
+          planner.synchronize();
+        }
+      #endif
+
+      // Prime (All distances are added and slowed down to ensure secure priming in all circumstances)
+      unscaled_e_move(toolchange_settings.swap_length + toolchange_settings.extra_prime, MMM_TO_MMS(toolchange_settings.prime_speed));
+
+      // Cutting retraction
+      #if TOOLCHANGE_FS_WIPE_RETRACT
+        unscaled_e_move(-(TOOLCHANGE_FS_WIPE_RETRACT), MMM_TO_MMS(toolchange_settings.retract_speed));
+      #endif
+
+      // Cool down with fan
+      #if HAS_FAN && TOOLCHANGE_FS_FAN >= 0
+        thermalManager.fan_speed[TOOLCHANGE_FS_FAN] = toolchange_settings.fan_speed;
+        gcode.dwell(SEC_TO_MS(toolchange_settings.fan_time));
+        thermalManager.fan_speed[TOOLCHANGE_FS_FAN] = 0;
+      #endif
+
+      // Move back
+      #if ENABLED(TOOLCHANGE_PARK)
+        if (ok) {
+          #if ENABLED(TOOLCHANGE_NO_RETURN)
+            do_blocking_move_to_z(destination.z, planner.settings.max_feedrate_mm_s[Z_AXIS]);
+          #else
+            do_blocking_move_to(destination, MMM_TO_MMS(TOOLCHANGE_PARK_XY_FEEDRATE));
+          #endif
+        }
+      #endif
+
+      // Cutting recover
+      unscaled_e_move(toolchange_settings.extra_resume + TOOLCHANGE_FS_WIPE_RETRACT, MMM_TO_MMS(toolchange_settings.unretract_speed));
+
+      planner.synchronize();
+      current_position.e = destination.e;
+      sync_plan_position_e(); // Resume at the old E position
+    }
+  }
+
+#endif // TOOLCHANGE_FILAMENT_SWAP
+
+/**
+ * Perform a tool-change, which may result in moving the
+ * previous tool out of the way and the new tool into place.
+ */
+void tool_change(const uint8_t new_tool, bool no_move/*=false*/) {
+
+  if (TERN0(MAGNETIC_SWITCHING_TOOLHEAD, new_tool == active_extruder))
+    return;
+
+  #if ENABLED(MIXING_EXTRUDER)
+
+    UNUSED(no_move);
+
+    if (new_tool >= MIXING_VIRTUAL_TOOLS)
+      return invalid_extruder_error(new_tool);
+
+    #if MIXING_VIRTUAL_TOOLS > 1
+      // T0-Tnnn: Switch virtual tool by changing the index to the mix
+      mixer.T(new_tool);
+    #endif
+
+  #elif HAS_PRUSA_MMU2
+
+    UNUSED(no_move);
+
+    mmu2.tool_change(new_tool);
+
+  #elif EXTRUDERS == 0
+
+    // Nothing to do
+    UNUSED(new_tool); UNUSED(no_move);
+
+  #elif EXTRUDERS < 2
+
+    UNUSED(no_move);
+
+    if (new_tool) invalid_extruder_error(new_tool);
+    return;
+
+  #elif HAS_MULTI_EXTRUDER
+
+    planner.synchronize();
+
+    #if ENABLED(DUAL_X_CARRIAGE)  // Only T0 allowed if the Printer is in DXC_DUPLICATION_MODE or DXC_MIRRORED_MODE
+      if (new_tool != 0 && idex_is_duplicating())
+         return invalid_extruder_error(new_tool);
+    #endif
+
+    if (new_tool >= EXTRUDERS)
+      return invalid_extruder_error(new_tool);
+
+    if (!no_move && homing_needed()) {
+      no_move = true;
+      DEBUG_ECHOLNPGM("No move (not homed)");
+    }
+
+    TERN_(HAS_LCD_MENU, if (!no_move) ui.update());
+
+    #if ENABLED(DUAL_X_CARRIAGE)
+      const bool idex_full_control = dual_x_carriage_mode == DXC_FULL_CONTROL_MODE;
+    #else
+      constexpr bool idex_full_control = false;
+    #endif
+
+    const uint8_t old_tool = active_extruder;
+    const bool can_move_away = !no_move && !idex_full_control;
+
+    #if HAS_LEVELING
+      // Set current position to the physical position
+      TEMPORARY_BED_LEVELING_STATE(false);
+    #endif
+
+    // First tool priming. To prime again, reboot the machine.
+    #if BOTH(TOOLCHANGE_FILAMENT_SWAP, TOOLCHANGE_FS_PRIME_FIRST_USED)
+      static bool first_tool_is_primed = false;
+      if (new_tool == old_tool && !first_tool_is_primed && enable_first_prime) {
+        tool_change_prime();
+        first_tool_is_primed = true;
+        toolchange_extruder_ready[old_tool] = true; // Primed and initialized
+      }
+    #endif
+
+    if (new_tool != old_tool || TERN0(PARKING_EXTRUDER, extruder_parked)) { // PARKING_EXTRUDER may need to attach old_tool when homing
+      destination = current_position;
+
+      #if BOTH(TOOLCHANGE_FILAMENT_SWAP, HAS_FAN) && TOOLCHANGE_FS_FAN >= 0
+        // Store and stop fan. Restored on any exit.
+        REMEMBER(fan, thermalManager.fan_speed[TOOLCHANGE_FS_FAN], 0);
+      #endif
+
+      // Z raise before retraction
+      #if ENABLED(TOOLCHANGE_ZRAISE_BEFORE_RETRACT) && DISABLED(SWITCHING_NOZZLE)
+        if (can_move_away && TERN1(TOOLCHANGE_PARK, toolchange_settings.enable_park)) {
+          // Do a small lift to avoid the workpiece in the move back (below)
+          current_position.z += toolchange_settings.z_raise;
+          #if HAS_SOFTWARE_ENDSTOPS
+            NOMORE(current_position.z, soft_endstop.max.z);
+          #endif
+          fast_line_to_current(Z_AXIS);
+          planner.synchronize();
+        }
+      #endif
+
+      // Unload / Retract
+      #if ENABLED(TOOLCHANGE_FILAMENT_SWAP)
+        const bool should_swap = can_move_away && toolchange_settings.swap_length,
+                   too_cold = TERN0(PREVENT_COLD_EXTRUSION,
+                     !DEBUGGING(DRYRUN) && (thermalManager.targetTooColdToExtrude(old_tool) || thermalManager.targetTooColdToExtrude(new_tool))
+                   );
+        if (should_swap) {
+          if (too_cold) {
+            SERIAL_ECHO_MSG(STR_ERR_HOTEND_TOO_COLD);
+            if (ENABLED(SINGLENOZZLE)) { active_extruder = new_tool; return; }
+          }
+          else {
+            #if ENABLED(TOOLCHANGE_FS_PRIME_FIRST_USED)
+              // For first new tool, change without unloading the old. 'Just prime/init the new'
+              if (first_tool_is_primed)
+                unscaled_e_move(-toolchange_settings.swap_length, MMM_TO_MMS(toolchange_settings.retract_speed));
+              first_tool_is_primed = true; // The first new tool will be primed by toolchanging
+            #endif
+          }
+        }
+      #endif
+
+      TERN_(SWITCHING_NOZZLE_TWO_SERVOS, raise_nozzle(old_tool));
+
+      REMEMBER(fr, feedrate_mm_s, XY_PROBE_FEEDRATE_MM_S);
+
+      #if HAS_SOFTWARE_ENDSTOPS
+        #if HAS_HOTEND_OFFSET
+          #define _EXT_ARGS , old_tool, new_tool
+        #else
+          #define _EXT_ARGS
+        #endif
+        update_software_endstops(X_AXIS _EXT_ARGS);
+        #if DISABLED(DUAL_X_CARRIAGE)
+          update_software_endstops(Y_AXIS _EXT_ARGS);
+          update_software_endstops(Z_AXIS _EXT_ARGS);
+        #endif
+      #endif
+
+      #if DISABLED(TOOLCHANGE_ZRAISE_BEFORE_RETRACT) && DISABLED(SWITCHING_NOZZLE)
+        if (can_move_away && TERN1(TOOLCHANGE_PARK, toolchange_settings.enable_park)) {
+          // Do a small lift to avoid the workpiece in the move back (below)
+          current_position.z += toolchange_settings.z_raise;
+          #if HAS_SOFTWARE_ENDSTOPS
+            NOMORE(current_position.z, soft_endstop.max.z);
+          #endif
+          fast_line_to_current(Z_AXIS);
+        }
+      #endif
+
+      // Toolchange park
+      #if ENABLED(TOOLCHANGE_PARK) && DISABLED(SWITCHING_NOZZLE)
+        if (can_move_away && toolchange_settings.enable_park) {
+          IF_DISABLED(TOOLCHANGE_PARK_Y_ONLY, current_position.x = toolchange_settings.change_point.x);
+          IF_DISABLED(TOOLCHANGE_PARK_X_ONLY, current_position.y = toolchange_settings.change_point.y);
+          planner.buffer_line(current_position, MMM_TO_MMS(TOOLCHANGE_PARK_XY_FEEDRATE), old_tool);
+          planner.synchronize();
+        }
+      #endif
+
+      #if HAS_HOTEND_OFFSET
+        xyz_pos_t diff = hotend_offset[new_tool] - hotend_offset[old_tool];
+        TERN_(DUAL_X_CARRIAGE, diff.x = 0);
+      #else
+        constexpr xyz_pos_t diff{0};
+      #endif
+
+      #if ENABLED(DUAL_X_CARRIAGE)
+        dualx_tool_change(new_tool, no_move);
+      #elif ENABLED(PARKING_EXTRUDER)                                   // Dual Parking extruder
+        parking_extruder_tool_change(new_tool, no_move);
+      #elif ENABLED(MAGNETIC_PARKING_EXTRUDER)                          // Magnetic Parking extruder
+        magnetic_parking_extruder_tool_change(new_tool);
+      #elif ENABLED(SWITCHING_TOOLHEAD)                                 // Switching Toolhead
+        switching_toolhead_tool_change(new_tool, no_move);
+      #elif ENABLED(MAGNETIC_SWITCHING_TOOLHEAD)                        // Magnetic Switching Toolhead
+        magnetic_switching_toolhead_tool_change(new_tool, no_move);
+      #elif ENABLED(ELECTROMAGNETIC_SWITCHING_TOOLHEAD)                 // Magnetic Switching ToolChanger
+        em_switching_toolhead_tool_change(new_tool, no_move);
+      #elif ENABLED(SWITCHING_NOZZLE) && !SWITCHING_NOZZLE_TWO_SERVOS   // Switching Nozzle (single servo)
+        // Raise by a configured distance to avoid workpiece, except with
+        // SWITCHING_NOZZLE_TWO_SERVOS, as both nozzles will lift instead.
+        if (!no_move) {
+          const float newz = current_position.z + _MAX(-diff.z, 0.0);
+
+          // Check if Z has space to compensate at least z_offset, and if not, just abort now
+          const float maxz = _MIN(TERN(HAS_SOFTWARE_ENDSTOPS, soft_endstop.max.z, Z_MAX_POS), Z_MAX_POS);
+          if (newz > maxz) return;
+
+          current_position.z = _MIN(newz + toolchange_settings.z_raise, maxz);
+          fast_line_to_current(Z_AXIS);
+        }
+        move_nozzle_servo(new_tool);
+      #endif
+
+      // Set the new active extruder
+      if (DISABLED(DUAL_X_CARRIAGE)) active_extruder = new_tool;
+
+      // The newly-selected extruder XYZ is actually at...
+      DEBUG_ECHOLNPAIR("Offset Tool XYZ by { ", diff.x, ", ", diff.y, ", ", diff.z, " }");
+      current_position += diff;
+
+      // Tell the planner the new "current position"
+      sync_plan_position();
+
+      #if ENABLED(DELTA)
+        //LOOP_XYZ(i) update_software_endstops(i); // or modify the constrain function
+        const bool safe_to_move = current_position.z < delta_clip_start_height - 1;
+      #else
+        constexpr bool safe_to_move = true;
+      #endif
+
+      // Return to position and lower again
+      const bool should_move = safe_to_move && !no_move && IsRunning();
+      if (should_move) {
+
+        TERN_(SINGLENOZZLE_STANDBY_TEMP, thermalManager.singlenozzle_change(old_tool, new_tool));
+
+        #if ENABLED(TOOLCHANGE_FILAMENT_SWAP)
+          if (should_swap && !too_cold) {
+
+            float fr = toolchange_settings.unretract_speed;
+
+            #if ENABLED(TOOLCHANGE_FS_INIT_BEFORE_SWAP)
+              if (!toolchange_extruder_ready[new_tool]) {
+                toolchange_extruder_ready[new_tool] = true;
+                fr = toolchange_settings.prime_speed;       // Next move is a prime
+                unscaled_e_move(0, MMM_TO_MMS(fr));         // Init planner with 0 length move
+              }
+            #endif
+
+            // Unretract (or Prime)
+            unscaled_e_move(toolchange_settings.swap_length, MMM_TO_MMS(fr));
+
+            // Extra Prime
+            unscaled_e_move(toolchange_settings.extra_prime, MMM_TO_MMS(toolchange_settings.prime_speed));
+
+            // Cutting retraction
+            #if TOOLCHANGE_FS_WIPE_RETRACT
+              unscaled_e_move(-(TOOLCHANGE_FS_WIPE_RETRACT), MMM_TO_MMS(toolchange_settings.retract_speed));
+            #endif
+
+            // Cool down with fan
+            #if HAS_FAN && TOOLCHANGE_FS_FAN >= 0
+              thermalManager.fan_speed[TOOLCHANGE_FS_FAN] = toolchange_settings.fan_speed;
+              gcode.dwell(SEC_TO_MS(toolchange_settings.fan_time));
+              thermalManager.fan_speed[TOOLCHANGE_FS_FAN] = 0;
+            #endif
+          }
+        #endif
+
+        // Prevent a move outside physical bounds
+        #if ENABLED(MAGNETIC_SWITCHING_TOOLHEAD)
+          // If the original position is within tool store area, go to X origin at once
+          if (destination.y < SWITCHING_TOOLHEAD_Y_POS + SWITCHING_TOOLHEAD_Y_CLEAR) {
+            current_position.x = 0;
+            planner.buffer_line(current_position, planner.settings.max_feedrate_mm_s[X_AXIS], new_tool);
+            planner.synchronize();
+          }
+        #else
+          apply_motion_limits(destination);
+        #endif
+
+        // Should the nozzle move back to the old position?
+        if (can_move_away) {
+          #if ENABLED(TOOLCHANGE_NO_RETURN)
+            // Just move back down
+            DEBUG_ECHOLNPGM("Move back Z only");
+
+            if (TERN1(TOOLCHANGE_PARK, toolchange_settings.enable_park))
+              do_blocking_move_to_z(destination.z, planner.settings.max_feedrate_mm_s[Z_AXIS]);
+
+          #else
+            // Move back to the original (or adjusted) position
+            DEBUG_POS("Move back", destination);
+
+            #if ENABLED(TOOLCHANGE_PARK)
+              if (toolchange_settings.enable_park) do_blocking_move_to_xy_z(destination, destination.z, MMM_TO_MMS(TOOLCHANGE_PARK_XY_FEEDRATE));
+            #else
+              do_blocking_move_to_xy(destination, planner.settings.max_feedrate_mm_s[X_AXIS]);
+              do_blocking_move_to_z(destination.z, planner.settings.max_feedrate_mm_s[Z_AXIS]);
+            #endif
+
+          #endif
+        }
+
+        else DEBUG_ECHOLNPGM("Move back skipped");
+
+        #if ENABLED(TOOLCHANGE_FILAMENT_SWAP)
+          if (should_swap && !too_cold) {
+            // Cutting recover
+            unscaled_e_move(toolchange_settings.extra_resume + TOOLCHANGE_FS_WIPE_RETRACT, MMM_TO_MMS(toolchange_settings.unretract_speed));
+            current_position.e = 0;
+            sync_plan_position_e(); // New extruder primed and set to 0
+
+            // Restart Fan
+            #if HAS_FAN && TOOLCHANGE_FS_FAN >= 0
+              RESTORE(fan);
+            #endif
+          }
+        #endif
+
+        TERN_(DUAL_X_CARRIAGE, idex_set_parked(false));
+      }
+
+      #if ENABLED(SWITCHING_NOZZLE)
+        // Move back down. (Including when the new tool is higher.)
+        if (!should_move)
+          do_blocking_move_to_z(destination.z, planner.settings.max_feedrate_mm_s[Z_AXIS]);
+      #endif
+
+      TERN_(SWITCHING_NOZZLE_TWO_SERVOS, lower_nozzle(new_tool));
+
+    } // (new_tool != old_tool)
+
+    planner.synchronize();
+
+    #if ENABLED(EXT_SOLENOID) && DISABLED(PARKING_EXTRUDER)
+      disable_all_solenoids();
+      enable_solenoid_on_active_extruder();
+    #endif
+
+    #if HAS_PRUSA_MMU1
+      if (new_tool >= E_STEPPERS) return invalid_extruder_error(new_tool);
+      select_multiplexed_stepper(new_tool);
+    #endif
+
+    #if DO_SWITCH_EXTRUDER
+      planner.synchronize();
+      move_extruder_servo(active_extruder);
+    #endif
+
+    TERN_(HAS_FANMUX, fanmux_switch(active_extruder));
+
+    #ifdef EVENT_GCODE_AFTER_TOOLCHANGE
+      if (!no_move && TERN1(DUAL_X_CARRIAGE, dual_x_carriage_mode == DXC_AUTO_PARK_MODE))
+        gcode.process_subcommands_now_P(PSTR(EVENT_GCODE_AFTER_TOOLCHANGE));
+    #endif
+
+    SERIAL_ECHO_START();
+    SERIAL_ECHOLNPAIR(STR_ACTIVE_EXTRUDER, int(active_extruder));
+
+  #endif // HAS_MULTI_EXTRUDER
+}
+
+#if ENABLED(TOOLCHANGE_MIGRATION_FEATURE)
+
+  #define DEBUG_OUT ENABLED(DEBUG_TOOLCHANGE_MIGRATION_FEATURE)
+  #include "../core/debug_out.h"
+
+  bool extruder_migration() {
+
+    #if ENABLED(PREVENT_COLD_EXTRUSION)
+      if (thermalManager.targetTooColdToExtrude(active_extruder)) {
+        DEBUG_ECHOLNPGM("Migration Source Too Cold");
+        return false;
+      }
+    #endif
+
+    // No auto-migration or specified target?
+    if (!migration.target && active_extruder >= migration.last) {
+      DEBUG_ECHO_MSG("No Migration Target");
+      DEBUG_ECHO_MSG("Target: ", migration.target, " Last: ", migration.last, " Active: ", active_extruder);
+      migration.automode = false;
+      return false;
+    }
+
+    // Migrate to a target or the next extruder
+
+    uint8_t migration_extruder = active_extruder;
+
+    if (migration.target) {
+      DEBUG_ECHOLNPGM("Migration using fixed target");
+      // Specified target ok?
+      const int16_t t = migration.target - 1;
+      if (t != active_extruder) migration_extruder = t;
+    }
+    else if (migration.automode && migration_extruder < migration.last && migration_extruder < EXTRUDERS - 1)
+      migration_extruder++;
+
+    if (migration_extruder == active_extruder) {
+      DEBUG_ECHOLNPGM("Migration source matches active");
+      return false;
+    }
+
+    // Migration begins
+    DEBUG_ECHOLNPGM("Beginning migration");
+
+    migration.in_progress = true; // Prevent runout script
+    planner.synchronize();
+
+    // Remember position before migration
+    const float resume_current_e = current_position.e;
+
+    // Migrate the flow
+    planner.set_flow(migration_extruder, planner.flow_percentage[active_extruder]);
+
+    // Migrate the retracted state
+    #if ENABLED(FWRETRACT)
+      fwretract.retracted[migration_extruder] = fwretract.retracted[active_extruder];
+    #endif
+
+    // Migrate the temperature to the new hotend
+    #if HAS_MULTI_HOTEND
+      thermalManager.setTargetHotend(thermalManager.temp_hotend[active_extruder].target, migration_extruder);
+      TERN_(AUTOTEMP, planner.autotemp_update());
+      TERN_(HAS_DISPLAY, thermalManager.set_heating_message(0));
+      thermalManager.wait_for_hotend(active_extruder);
+    #endif
+
+    // Migrate Linear Advance K factor to the new extruder
+    TERN_(LIN_ADVANCE, planner.extruder_advance_K[active_extruder] = planner.extruder_advance_K[migration_extruder]);
+
+    // Perform the tool change
+    tool_change(migration_extruder);
+
+    // Retract if previously retracted
+    #if ENABLED(FWRETRACT)
+      if (fwretract.retracted[active_extruder])
+        unscaled_e_move(-fwretract.settings.retract_length, fwretract.settings.retract_feedrate_mm_s);
+    #endif
+
+    // If no available extruder
+    if (EXTRUDERS < 2 || active_extruder >= EXTRUDERS - 2 || active_extruder == migration.last)
+      migration.automode = false;
+
+    migration.in_progress = false;
+
+    current_position.e = resume_current_e;
+
+    planner.synchronize();
+    planner.set_e_position_mm(current_position.e); // New extruder primed and ready
+    DEBUG_ECHOLNPGM("Migration Complete");
+    return true;
+  }
+
+#endif // TOOLCHANGE_MIGRATION_FEATURE
diff --git a/Marlin/src/module/tool_change.h b/Marlin/src/module/tool_change.h
new file mode 100644
index 0000000..4f88ca7
--- /dev/null
+++ b/Marlin/src/module/tool_change.h
@@ -0,0 +1,125 @@
+/**
+ * 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 "../core/types.h"
+
+//#define DEBUG_TOOLCHANGE_MIGRATION_FEATURE
+
+#if HAS_MULTI_EXTRUDER
+
+  typedef struct {
+    #if ENABLED(TOOLCHANGE_FILAMENT_SWAP)
+      float swap_length, extra_prime, extra_resume;
+      int16_t prime_speed, retract_speed, unretract_speed, fan, fan_speed, fan_time;
+    #endif
+    #if ENABLED(TOOLCHANGE_PARK)
+      bool enable_park;
+      xy_pos_t change_point;
+    #endif
+    float z_raise;
+  } toolchange_settings_t;
+
+  extern toolchange_settings_t toolchange_settings;
+
+  #if ENABLED(TOOLCHANGE_FILAMENT_SWAP)
+    extern void tool_change_prime();
+  #endif
+
+  #if ENABLED(TOOLCHANGE_FS_PRIME_FIRST_USED)
+    extern bool enable_first_prime;
+  #endif
+
+  #if ENABLED(TOOLCHANGE_FS_INIT_BEFORE_SWAP)
+    extern bool toolchange_extruder_ready[EXTRUDERS];
+  #endif
+
+  #if ENABLED(TOOLCHANGE_MIGRATION_FEATURE)
+    typedef struct {
+      uint8_t target, last;
+      bool automode, in_progress;
+    } migration_settings_t;
+    constexpr migration_settings_t migration_defaults = { 0, 0, false, false };
+    extern migration_settings_t migration;
+    bool extruder_migration();
+  #endif
+#endif
+
+#if DO_SWITCH_EXTRUDER
+  void move_extruder_servo(const uint8_t e);
+#endif
+
+#if ENABLED(SWITCHING_NOZZLE)
+  #if SWITCHING_NOZZLE_TWO_SERVOS
+    void lower_nozzle(const uint8_t e);
+    void raise_nozzle(const uint8_t e);
+  #else
+    void move_nozzle_servo(const uint8_t angle_index);
+  #endif
+#endif
+
+#if ENABLED(PARKING_EXTRUDER)
+
+  #if ENABLED(PARKING_EXTRUDER_SOLENOIDS_INVERT)
+    #define PE_MAGNET_ON_STATE !PARKING_EXTRUDER_SOLENOIDS_PINS_ACTIVE
+  #else
+    #define PE_MAGNET_ON_STATE PARKING_EXTRUDER_SOLENOIDS_PINS_ACTIVE
+  #endif
+
+  void pe_solenoid_set_pin_state(const uint8_t extruder_num, const uint8_t state);
+
+  inline void pe_solenoid_magnet_on(const uint8_t extruder_num)  { pe_solenoid_set_pin_state(extruder_num,  PE_MAGNET_ON_STATE); }
+  inline void pe_solenoid_magnet_off(const uint8_t extruder_num) { pe_solenoid_set_pin_state(extruder_num, !PE_MAGNET_ON_STATE); }
+
+  void pe_solenoid_init();
+
+  extern bool extruder_parked;
+  inline void parking_extruder_set_parked(const bool parked) { extruder_parked = parked; }
+  bool parking_extruder_unpark_after_homing(const uint8_t final_tool, bool homed_towards_final_tool);
+
+#elif ENABLED(MAGNETIC_PARKING_EXTRUDER)
+
+  typedef struct MPESettings {
+      float parking_xpos[2],      // M951 L R
+            grab_distance;        // M951 I
+ feedRate_t slow_feedrate,        // M951 J
+            fast_feedrate;        // M951 H
+      float travel_distance,      // M951 D
+            compensation_factor;  // M951 C
+  } mpe_settings_t;
+
+  extern mpe_settings_t mpe_settings;
+
+  void mpe_settings_init();
+
+#endif
+
+TERN_(ELECTROMAGNETIC_SWITCHING_TOOLHEAD, void est_init());
+
+TERN_(SWITCHING_TOOLHEAD, void swt_init());
+
+/**
+ * Perform a tool-change, which may result in moving the
+ * previous tool out of the way and the new tool into place.
+ */
+void tool_change(const uint8_t tmp_extruder, bool no_move=false);