Initial commit

111 files changed, 20586 insertions, 0 deletions

diff --git a/Marlin/src/feature/babystep.cpp b/Marlin/src/feature/babystep.cpp
new file mode 100644
index 0000000..b076881
--- /dev/null
+++ b/Marlin/src/feature/babystep.cpp
@@ -0,0 +1,67 @@
+/**
+ * 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 ENABLED(BABYSTEPPING)
+
+#include "babystep.h"
+#include "../MarlinCore.h"
+#include "../module/motion.h"   // for axes_should_home()
+#include "../module/planner.h"  // for axis_steps_per_mm[]
+#include "../module/stepper.h"
+
+#if ENABLED(BABYSTEP_ALWAYS_AVAILABLE)
+  #include "../gcode/gcode.h"
+#endif
+
+Babystep babystep;
+
+volatile int16_t Babystep::steps[BS_AXIS_IND(Z_AXIS) + 1];
+#if ENABLED(BABYSTEP_DISPLAY_TOTAL)
+  int16_t Babystep::axis_total[BS_TOTAL_IND(Z_AXIS) + 1];
+#endif
+int16_t Babystep::accum;
+
+void Babystep::step_axis(const AxisEnum axis) {
+  const int16_t curTodo = steps[BS_AXIS_IND(axis)]; // get rid of volatile for performance
+  if (curTodo) {
+    stepper.do_babystep((AxisEnum)axis, curTodo > 0);
+    if (curTodo > 0) steps[BS_AXIS_IND(axis)]--; else steps[BS_AXIS_IND(axis)]++;
+  }
+}
+
+void Babystep::add_mm(const AxisEnum axis, const float &mm) {
+  add_steps(axis, mm * planner.settings.axis_steps_per_mm[axis]);
+}
+
+void Babystep::add_steps(const AxisEnum axis, const int16_t distance) {
+  if (DISABLED(BABYSTEP_WITHOUT_HOMING) && axes_should_home(_BV(axis))) return;
+
+  accum += distance; // Count up babysteps for the UI
+  steps[BS_AXIS_IND(axis)] += distance;
+  TERN_(BABYSTEP_DISPLAY_TOTAL, axis_total[BS_TOTAL_IND(axis)] += distance);
+  TERN_(BABYSTEP_ALWAYS_AVAILABLE, gcode.reset_stepper_timeout());
+  TERN_(INTEGRATED_BABYSTEPPING, if (has_steps()) stepper.initiateBabystepping());
+}
+
+#endif // BABYSTEPPING
diff --git a/Marlin/src/feature/babystep.h b/Marlin/src/feature/babystep.h
new file mode 100644
index 0000000..f85e590
--- /dev/null
+++ b/Marlin/src/feature/babystep.h
@@ -0,0 +1,82 @@
+/**
+ * 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"
+
+#if ENABLED(INTEGRATED_BABYSTEPPING)
+  #define BABYSTEPS_PER_SEC 1000UL
+  #define BABYSTEP_TICKS ((STEPPER_TIMER_RATE) / (BABYSTEPS_PER_SEC))
+#else
+  #define BABYSTEPS_PER_SEC 976UL
+  #define BABYSTEP_TICKS ((TEMP_TIMER_RATE) / (BABYSTEPS_PER_SEC))
+#endif
+
+#if IS_CORE || EITHER(BABYSTEP_XY, I2C_POSITION_ENCODERS)
+  #define BS_AXIS_IND(A) A
+  #define BS_AXIS(I) AxisEnum(I)
+#else
+  #define BS_AXIS_IND(A) 0
+  #define BS_AXIS(I) Z_AXIS
+#endif
+
+#if ENABLED(BABYSTEP_DISPLAY_TOTAL)
+  #if ENABLED(BABYSTEP_XY)
+    #define BS_TOTAL_IND(A) A
+  #else
+    #define BS_TOTAL_IND(A) 0
+  #endif
+#endif
+
+class Babystep {
+public:
+  static volatile int16_t steps[BS_AXIS_IND(Z_AXIS) + 1];
+  static int16_t accum;                                     // Total babysteps in current edit
+
+  #if ENABLED(BABYSTEP_DISPLAY_TOTAL)
+    static int16_t axis_total[BS_TOTAL_IND(Z_AXIS) + 1];   // Total babysteps since G28
+    static inline void reset_total(const AxisEnum axis) {
+      if (TERN1(BABYSTEP_XY, axis == Z_AXIS))
+        axis_total[BS_TOTAL_IND(axis)] = 0;
+    }
+  #endif
+
+  static void add_steps(const AxisEnum axis, const int16_t distance);
+  static void add_mm(const AxisEnum axis, const float &mm);
+
+  static inline bool has_steps() {
+    return steps[BS_AXIS_IND(X_AXIS)] || steps[BS_AXIS_IND(Y_AXIS)] || steps[BS_AXIS_IND(Z_AXIS)];
+  }
+
+  //
+  // Called by the Temperature or Stepper ISR to
+  // apply accumulated babysteps to the axes.
+  //
+  static inline void task() {
+    LOOP_LE_N(i, BS_AXIS_IND(Z_AXIS)) step_axis(BS_AXIS(i));
+  }
+
+private:
+  static void step_axis(const AxisEnum axis);
+};
+
+extern Babystep babystep;
diff --git a/Marlin/src/feature/backlash.cpp b/Marlin/src/feature/backlash.cpp
new file mode 100644
index 0000000..b848214
--- /dev/null
+++ b/Marlin/src/feature/backlash.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/>.
+ *
+ */
+
+#include "../inc/MarlinConfigPre.h"
+
+#if ENABLED(BACKLASH_COMPENSATION)
+
+#include "backlash.h"
+
+#include "../module/motion.h"
+#include "../module/planner.h"
+
+#ifdef BACKLASH_DISTANCE_MM
+  #if ENABLED(BACKLASH_GCODE)
+    xyz_float_t Backlash::distance_mm = BACKLASH_DISTANCE_MM;
+  #else
+    const xyz_float_t Backlash::distance_mm = BACKLASH_DISTANCE_MM;
+  #endif
+#endif
+
+#if ENABLED(BACKLASH_GCODE)
+  uint8_t Backlash::correction = (BACKLASH_CORRECTION) * 0xFF;
+  #ifdef BACKLASH_SMOOTHING_MM
+    float Backlash::smoothing_mm = BACKLASH_SMOOTHING_MM;
+  #endif
+#endif
+
+#if ENABLED(MEASURE_BACKLASH_WHEN_PROBING)
+  xyz_float_t Backlash::measured_mm{0};
+  xyz_uint8_t Backlash::measured_count{0};
+#endif
+
+Backlash backlash;
+
+/**
+ * To minimize seams in the printed part, backlash correction only adds
+ * steps to the current segment (instead of creating a new segment, which
+ * causes discontinuities and print artifacts).
+ *
+ * With a non-zero BACKLASH_SMOOTHING_MM value the backlash correction is
+ * spread over multiple segments, smoothing out artifacts even more.
+ */
+
+void Backlash::add_correction_steps(const int32_t &da, const int32_t &db, const int32_t &dc, const uint8_t dm, block_t * const block) {
+  static uint8_t last_direction_bits;
+  uint8_t changed_dir = last_direction_bits ^ dm;
+  // Ignore direction change if no steps are taken in that direction
+  if (da == 0) CBI(changed_dir, X_AXIS);
+  if (db == 0) CBI(changed_dir, Y_AXIS);
+  if (dc == 0) CBI(changed_dir, Z_AXIS);
+  last_direction_bits ^= changed_dir;
+
+  if (correction == 0) return;
+
+  #ifdef BACKLASH_SMOOTHING_MM
+    // The segment proportion is a value greater than 0.0 indicating how much residual_error
+    // is corrected for in this segment. The contribution is based on segment length and the
+    // smoothing distance. Since the computation of this proportion involves a floating point
+    // division, defer computation until needed.
+    float segment_proportion = 0;
+
+    // Residual error carried forward across multiple segments, so correction can be applied
+    // to segments where there is no direction change.
+    static xyz_long_t residual_error{0};
+  #else
+    // No direction change, no correction.
+    if (!changed_dir) return;
+    // No leftover residual error from segment to segment
+    xyz_long_t residual_error{0};
+  #endif
+
+  const float f_corr = float(correction) / 255.0f;
+
+  LOOP_XYZ(axis) {
+    if (distance_mm[axis]) {
+      const bool reversing = TEST(dm,axis);
+
+      // When an axis changes direction, add axis backlash to the residual error
+      if (TEST(changed_dir, axis))
+        residual_error[axis] += (reversing ? -f_corr : f_corr) * distance_mm[axis] * planner.settings.axis_steps_per_mm[axis];
+
+      // Decide how much of the residual error to correct in this segment
+      int32_t error_correction = residual_error[axis];
+      #ifdef BACKLASH_SMOOTHING_MM
+        if (error_correction && smoothing_mm != 0) {
+          // Take up a portion of the residual_error in this segment, but only when
+          // the current segment travels in the same direction as the correction
+          if (reversing == (error_correction < 0)) {
+            if (segment_proportion == 0)
+              segment_proportion = _MIN(1.0f, block->millimeters / smoothing_mm);
+            error_correction = CEIL(segment_proportion * error_correction);
+          }
+          else
+            error_correction = 0; // Don't take up any backlash in this segment, as it would subtract steps
+        }
+      #endif
+      // Making a correction reduces the residual error and adds block steps
+      if (error_correction) {
+        block->steps[axis] += ABS(error_correction);
+        residual_error[axis] -= error_correction;
+      }
+    }
+  }
+}
+
+#if ENABLED(MEASURE_BACKLASH_WHEN_PROBING)
+
+  #include "../module/probe.h"
+
+  // Measure Z backlash by raising nozzle in increments until probe deactivates
+  void Backlash::measure_with_probe() {
+    if (measured_count.z == 255) return;
+
+    const float start_height = current_position.z;
+    while (current_position.z < (start_height + BACKLASH_MEASUREMENT_LIMIT) && PROBE_TRIGGERED())
+      do_blocking_move_to_z(current_position.z + BACKLASH_MEASUREMENT_RESOLUTION, MMM_TO_MMS(BACKLASH_MEASUREMENT_FEEDRATE));
+
+    // The backlash from all probe points is averaged, so count the number of measurements
+    measured_mm.z += current_position.z - start_height;
+    measured_count.z++;
+  }
+
+#endif
+
+#endif // BACKLASH_COMPENSATION
diff --git a/Marlin/src/feature/backlash.h b/Marlin/src/feature/backlash.h
new file mode 100644
index 0000000..49857f1
--- /dev/null
+++ b/Marlin/src/feature/backlash.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
+
+#include "../inc/MarlinConfigPre.h"
+#include "../module/planner.h"
+
+constexpr uint8_t all_on = 0xFF, all_off = 0x00;
+
+class Backlash {
+public:
+  #if ENABLED(BACKLASH_GCODE)
+    static xyz_float_t distance_mm;
+    static uint8_t correction;
+    #ifdef BACKLASH_SMOOTHING_MM
+      static float smoothing_mm;
+    #endif
+
+    static inline void set_correction(const float &v) { correction = _MAX(0, _MIN(1.0, v)) * all_on; }
+    static inline float get_correction() { return float(ui8_to_percent(correction)) / 100.0f; }
+  #else
+    static constexpr uint8_t correction = (BACKLASH_CORRECTION) * 0xFF;
+    static const xyz_float_t distance_mm;
+    #ifdef BACKLASH_SMOOTHING_MM
+      static constexpr float smoothing_mm = BACKLASH_SMOOTHING_MM;
+    #endif
+  #endif
+
+  #if ENABLED(MEASURE_BACKLASH_WHEN_PROBING)
+    private:
+      static xyz_float_t measured_mm;
+      static xyz_uint8_t measured_count;
+    public:
+      static void measure_with_probe();
+  #endif
+
+  static inline float get_measurement(const AxisEnum a) {
+    UNUSED(a);
+    // Return the measurement averaged over all readings
+    return TERN(MEASURE_BACKLASH_WHEN_PROBING
+      , measured_count[a] > 0 ? measured_mm[a] / measured_count[a] : 0
+      , 0
+    );
+  }
+
+  static inline bool has_measurement(const AxisEnum a) {
+    UNUSED(a);
+    return TERN0(MEASURE_BACKLASH_WHEN_PROBING, measured_count[a] > 0);
+  }
+
+  static inline bool has_any_measurement() {
+    return has_measurement(X_AXIS) || has_measurement(Y_AXIS) || has_measurement(Z_AXIS);
+  }
+
+  void add_correction_steps(const int32_t &da, const int32_t &db, const int32_t &dc, const uint8_t dm, block_t * const block);
+};
+
+extern Backlash backlash;
diff --git a/Marlin/src/feature/baricuda.cpp b/Marlin/src/feature/baricuda.cpp
new file mode 100644
index 0000000..5968917
--- /dev/null
+++ b/Marlin/src/feature/baricuda.cpp
@@ -0,0 +1,32 @@
+/**
+ * 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"
+
+#if ENABLED(BARICUDA)
+
+#include "baricuda.h"
+
+uint8_t baricuda_valve_pressure = 0,
+        baricuda_e_to_p_pressure = 0;
+
+#endif // BARICUDA
diff --git a/Marlin/src/feature/baricuda.h b/Marlin/src/feature/baricuda.h
new file mode 100644
index 0000000..f28d07d
--- /dev/null
+++ b/Marlin/src/feature/baricuda.h
@@ -0,0 +1,25 @@
+/**
+ * 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
+
+extern uint8_t baricuda_valve_pressure,
+               baricuda_e_to_p_pressure;
diff --git a/Marlin/src/feature/bedlevel/abl/abl.cpp b/Marlin/src/feature/bedlevel/abl/abl.cpp
new file mode 100644
index 0000000..3fb0cfc
--- /dev/null
+++ b/Marlin/src/feature/bedlevel/abl/abl.cpp
@@ -0,0 +1,421 @@
+/**
+ * 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 ENABLED(AUTO_BED_LEVELING_BILINEAR)
+
+#include "../bedlevel.h"
+
+#include "../../../module/motion.h"
+
+#define DEBUG_OUT ENABLED(DEBUG_LEVELING_FEATURE)
+#include "../../../core/debug_out.h"
+
+#if ENABLED(EXTENSIBLE_UI)
+  #include "../../../lcd/extui/ui_api.h"
+#endif
+
+xy_pos_t bilinear_grid_spacing, bilinear_start;
+xy_float_t bilinear_grid_factor;
+bed_mesh_t z_values;
+
+/**
+ * Extrapolate a single point from its neighbors
+ */
+static void extrapolate_one_point(const uint8_t x, const uint8_t y, const int8_t xdir, const int8_t ydir) {
+  if (!isnan(z_values[x][y])) return;
+  if (DEBUGGING(LEVELING)) {
+    DEBUG_ECHOPGM("Extrapolate [");
+    if (x < 10) DEBUG_CHAR(' ');
+    DEBUG_ECHO((int)x);
+    DEBUG_CHAR(xdir ? (xdir > 0 ? '+' : '-') : ' ');
+    DEBUG_CHAR(' ');
+    if (y < 10) DEBUG_CHAR(' ');
+    DEBUG_ECHO((int)y);
+    DEBUG_CHAR(ydir ? (ydir > 0 ? '+' : '-') : ' ');
+    DEBUG_ECHOLNPGM("]");
+  }
+
+  // Get X neighbors, Y neighbors, and XY neighbors
+  const uint8_t x1 = x + xdir, y1 = y + ydir, x2 = x1 + xdir, y2 = y1 + ydir;
+  float a1 = z_values[x1][y ], a2 = z_values[x2][y ],
+        b1 = z_values[x ][y1], b2 = z_values[x ][y2],
+        c1 = z_values[x1][y1], c2 = z_values[x2][y2];
+
+  // Treat far unprobed points as zero, near as equal to far
+  if (isnan(a2)) a2 = 0.0;
+  if (isnan(a1)) a1 = a2;
+  if (isnan(b2)) b2 = 0.0;
+  if (isnan(b1)) b1 = b2;
+  if (isnan(c2)) c2 = 0.0;
+  if (isnan(c1)) c1 = c2;
+
+  const float a = 2 * a1 - a2, b = 2 * b1 - b2, c = 2 * c1 - c2;
+
+  // Take the average instead of the median
+  z_values[x][y] = (a + b + c) / 3.0;
+  TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(x, y, z_values[x][y]));
+
+  // Median is robust (ignores outliers).
+  // z_values[x][y] = (a < b) ? ((b < c) ? b : (c < a) ? a : c)
+  //                                : ((c < b) ? b : (a < c) ? a : c);
+}
+
+//Enable this if your SCARA uses 180° of total area
+//#define EXTRAPOLATE_FROM_EDGE
+
+#if ENABLED(EXTRAPOLATE_FROM_EDGE)
+  #if GRID_MAX_POINTS_X < GRID_MAX_POINTS_Y
+    #define HALF_IN_X
+  #elif GRID_MAX_POINTS_Y < GRID_MAX_POINTS_X
+    #define HALF_IN_Y
+  #endif
+#endif
+
+/**
+ * Fill in the unprobed points (corners of circular print surface)
+ * using linear extrapolation, away from the center.
+ */
+void extrapolate_unprobed_bed_level() {
+  #ifdef HALF_IN_X
+    constexpr uint8_t ctrx2 = 0, xlen = GRID_MAX_POINTS_X - 1;
+  #else
+    constexpr uint8_t ctrx1 = (GRID_MAX_POINTS_X - 1) / 2, // left-of-center
+                      ctrx2 = (GRID_MAX_POINTS_X) / 2,     // right-of-center
+                      xlen = ctrx1;
+  #endif
+
+  #ifdef HALF_IN_Y
+    constexpr uint8_t ctry2 = 0, ylen = GRID_MAX_POINTS_Y - 1;
+  #else
+    constexpr uint8_t ctry1 = (GRID_MAX_POINTS_Y - 1) / 2, // top-of-center
+                      ctry2 = (GRID_MAX_POINTS_Y) / 2,     // bottom-of-center
+                      ylen = ctry1;
+  #endif
+
+  LOOP_LE_N(xo, xlen)
+    LOOP_LE_N(yo, ylen) {
+      uint8_t x2 = ctrx2 + xo, y2 = ctry2 + yo;
+      #ifndef HALF_IN_X
+        const uint8_t x1 = ctrx1 - xo;
+      #endif
+      #ifndef HALF_IN_Y
+        const uint8_t y1 = ctry1 - yo;
+        #ifndef HALF_IN_X
+          extrapolate_one_point(x1, y1, +1, +1);   //  left-below + +
+        #endif
+        extrapolate_one_point(x2, y1, -1, +1);     // right-below - +
+      #endif
+      #ifndef HALF_IN_X
+        extrapolate_one_point(x1, y2, +1, -1);     //  left-above + -
+      #endif
+      extrapolate_one_point(x2, y2, -1, -1);       // right-above - -
+    }
+
+}
+
+void print_bilinear_leveling_grid() {
+  SERIAL_ECHOLNPGM("Bilinear Leveling Grid:");
+  print_2d_array(GRID_MAX_POINTS_X, GRID_MAX_POINTS_Y, 3,
+    [](const uint8_t ix, const uint8_t iy) { return z_values[ix][iy]; }
+  );
+}
+
+#if ENABLED(ABL_BILINEAR_SUBDIVISION)
+
+  #define ABL_GRID_POINTS_VIRT_X (GRID_MAX_POINTS_X - 1) * (BILINEAR_SUBDIVISIONS) + 1
+  #define ABL_GRID_POINTS_VIRT_Y (GRID_MAX_POINTS_Y - 1) * (BILINEAR_SUBDIVISIONS) + 1
+  #define ABL_TEMP_POINTS_X (GRID_MAX_POINTS_X + 2)
+  #define ABL_TEMP_POINTS_Y (GRID_MAX_POINTS_Y + 2)
+  float z_values_virt[ABL_GRID_POINTS_VIRT_X][ABL_GRID_POINTS_VIRT_Y];
+  xy_pos_t bilinear_grid_spacing_virt;
+  xy_float_t bilinear_grid_factor_virt;
+
+  void print_bilinear_leveling_grid_virt() {
+    SERIAL_ECHOLNPGM("Subdivided with CATMULL ROM Leveling Grid:");
+    print_2d_array(ABL_GRID_POINTS_VIRT_X, ABL_GRID_POINTS_VIRT_Y, 5,
+      [](const uint8_t ix, const uint8_t iy) { return z_values_virt[ix][iy]; }
+    );
+  }
+
+  #define LINEAR_EXTRAPOLATION(E, I) ((E) * 2 - (I))
+  float bed_level_virt_coord(const uint8_t x, const uint8_t y) {
+    uint8_t ep = 0, ip = 1;
+    if (x > GRID_MAX_POINTS_X + 1 || y > GRID_MAX_POINTS_Y + 1) {
+      // The requested point requires extrapolating two points beyond the mesh.
+      // These values are only requested for the edges of the mesh, which are always an actual mesh point,
+      // and do not require interpolation. When interpolation is not needed, this "Mesh + 2" point is
+      // cancelled out in bed_level_virt_cmr and does not impact the result. Return 0.0 rather than
+      // making this function more complex by extrapolating two points.
+      return 0.0;
+    }
+    if (!x || x == ABL_TEMP_POINTS_X - 1) {
+      if (x) {
+        ep = GRID_MAX_POINTS_X - 1;
+        ip = GRID_MAX_POINTS_X - 2;
+      }
+      if (WITHIN(y, 1, ABL_TEMP_POINTS_Y - 2))
+        return LINEAR_EXTRAPOLATION(
+          z_values[ep][y - 1],
+          z_values[ip][y - 1]
+        );
+      else
+        return LINEAR_EXTRAPOLATION(
+          bed_level_virt_coord(ep + 1, y),
+          bed_level_virt_coord(ip + 1, y)
+        );
+    }
+    if (!y || y == ABL_TEMP_POINTS_Y - 1) {
+      if (y) {
+        ep = GRID_MAX_POINTS_Y - 1;
+        ip = GRID_MAX_POINTS_Y - 2;
+      }
+      if (WITHIN(x, 1, ABL_TEMP_POINTS_X - 2))
+        return LINEAR_EXTRAPOLATION(
+          z_values[x - 1][ep],
+          z_values[x - 1][ip]
+        );
+      else
+        return LINEAR_EXTRAPOLATION(
+          bed_level_virt_coord(x, ep + 1),
+          bed_level_virt_coord(x, ip + 1)
+        );
+    }
+    return z_values[x - 1][y - 1];
+  }
+
+  static float bed_level_virt_cmr(const float p[4], const uint8_t i, const float t) {
+    return (
+        p[i-1] * -t * sq(1 - t)
+      + p[i]   * (2 - 5 * sq(t) + 3 * t * sq(t))
+      + p[i+1] * t * (1 + 4 * t - 3 * sq(t))
+      - p[i+2] * sq(t) * (1 - t)
+    ) * 0.5f;
+  }
+
+  static float bed_level_virt_2cmr(const uint8_t x, const uint8_t y, const float &tx, const float &ty) {
+    float row[4], column[4];
+    LOOP_L_N(i, 4) {
+      LOOP_L_N(j, 4) {
+        column[j] = bed_level_virt_coord(i + x - 1, j + y - 1);
+      }
+      row[i] = bed_level_virt_cmr(column, 1, ty);
+    }
+    return bed_level_virt_cmr(row, 1, tx);
+  }
+
+  void bed_level_virt_interpolate() {
+    bilinear_grid_spacing_virt = bilinear_grid_spacing / (BILINEAR_SUBDIVISIONS);
+    bilinear_grid_factor_virt = bilinear_grid_spacing_virt.reciprocal();
+    LOOP_L_N(y, GRID_MAX_POINTS_Y)
+      LOOP_L_N(x, GRID_MAX_POINTS_X)
+        LOOP_L_N(ty, BILINEAR_SUBDIVISIONS)
+          LOOP_L_N(tx, BILINEAR_SUBDIVISIONS) {
+            if ((ty && y == (GRID_MAX_POINTS_Y) - 1) || (tx && x == (GRID_MAX_POINTS_X) - 1))
+              continue;
+            z_values_virt[x * (BILINEAR_SUBDIVISIONS) + tx][y * (BILINEAR_SUBDIVISIONS) + ty] =
+              bed_level_virt_2cmr(
+                x + 1,
+                y + 1,
+                (float)tx / (BILINEAR_SUBDIVISIONS),
+                (float)ty / (BILINEAR_SUBDIVISIONS)
+              );
+          }
+  }
+#endif // ABL_BILINEAR_SUBDIVISION
+
+// Refresh after other values have been updated
+void refresh_bed_level() {
+  bilinear_grid_factor = bilinear_grid_spacing.reciprocal();
+  TERN_(ABL_BILINEAR_SUBDIVISION, bed_level_virt_interpolate());
+}
+
+#if ENABLED(ABL_BILINEAR_SUBDIVISION)
+  #define ABL_BG_SPACING(A) bilinear_grid_spacing_virt.A
+  #define ABL_BG_FACTOR(A)  bilinear_grid_factor_virt.A
+  #define ABL_BG_POINTS_X   ABL_GRID_POINTS_VIRT_X
+  #define ABL_BG_POINTS_Y   ABL_GRID_POINTS_VIRT_Y
+  #define ABL_BG_GRID(X,Y)  z_values_virt[X][Y]
+#else
+  #define ABL_BG_SPACING(A) bilinear_grid_spacing.A
+  #define ABL_BG_FACTOR(A)  bilinear_grid_factor.A
+  #define ABL_BG_POINTS_X   GRID_MAX_POINTS_X
+  #define ABL_BG_POINTS_Y   GRID_MAX_POINTS_Y
+  #define ABL_BG_GRID(X,Y)  z_values[X][Y]
+#endif
+
+// Get the Z adjustment for non-linear bed leveling
+float bilinear_z_offset(const xy_pos_t &raw) {
+
+  static float z1, d2, z3, d4, L, D;
+
+  static xy_pos_t prev { -999.999, -999.999 }, ratio;
+
+  // Whole units for the grid line indices. Constrained within bounds.
+  static xy_int8_t thisg, nextg, lastg { -99, -99 };
+
+  // XY relative to the probed area
+  xy_pos_t rel = raw - bilinear_start.asFloat();
+
+  #if ENABLED(EXTRAPOLATE_BEYOND_GRID)
+    #define FAR_EDGE_OR_BOX 2   // Keep using the last grid box
+  #else
+    #define FAR_EDGE_OR_BOX 1   // Just use the grid far edge
+  #endif
+
+  if (prev.x != rel.x) {
+    prev.x = rel.x;
+    ratio.x = rel.x * ABL_BG_FACTOR(x);
+    const float gx = constrain(FLOOR(ratio.x), 0, ABL_BG_POINTS_X - (FAR_EDGE_OR_BOX));
+    ratio.x -= gx;      // Subtract whole to get the ratio within the grid box
+
+    #if DISABLED(EXTRAPOLATE_BEYOND_GRID)
+      // Beyond the grid maintain height at grid edges
+      NOLESS(ratio.x, 0); // Never <0 (>1 is ok when nextg.x==thisg.x)
+    #endif
+
+    thisg.x = gx;
+    nextg.x = _MIN(thisg.x + 1, ABL_BG_POINTS_X - 1);
+  }
+
+  if (prev.y != rel.y || lastg.x != thisg.x) {
+
+    if (prev.y != rel.y) {
+      prev.y = rel.y;
+      ratio.y = rel.y * ABL_BG_FACTOR(y);
+      const float gy = constrain(FLOOR(ratio.y), 0, ABL_BG_POINTS_Y - (FAR_EDGE_OR_BOX));
+      ratio.y -= gy;
+
+      #if DISABLED(EXTRAPOLATE_BEYOND_GRID)
+        // Beyond the grid maintain height at grid edges
+        NOLESS(ratio.y, 0); // Never < 0.0. (> 1.0 is ok when nextg.y==thisg.y.)
+      #endif
+
+      thisg.y = gy;
+      nextg.y = _MIN(thisg.y + 1, ABL_BG_POINTS_Y - 1);
+    }
+
+    if (lastg != thisg) {
+      lastg = thisg;
+      // Z at the box corners
+      z1 = ABL_BG_GRID(thisg.x, thisg.y);       // left-front
+      d2 = ABL_BG_GRID(thisg.x, nextg.y) - z1;  // left-back (delta)
+      z3 = ABL_BG_GRID(nextg.x, thisg.y);       // right-front
+      d4 = ABL_BG_GRID(nextg.x, nextg.y) - z3;  // right-back (delta)
+    }
+
+    // Bilinear interpolate. Needed since rel.y or thisg.x has changed.
+                L = z1 + d2 * ratio.y;   // Linear interp. LF -> LB
+    const float R = z3 + d4 * ratio.y;   // Linear interp. RF -> RB
+
+    D = R - L;
+  }
+
+  const float offset = L + ratio.x * D;   // the offset almost always changes
+
+  /*
+  static float last_offset = 0;
+  if (ABS(last_offset - offset) > 0.2) {
+    SERIAL_ECHOLNPAIR("Sudden Shift at x=", rel.x, " / ", bilinear_grid_spacing.x, " -> thisg.x=", thisg.x);
+    SERIAL_ECHOLNPAIR(" y=", rel.y, " / ", bilinear_grid_spacing.y, " -> thisg.y=", thisg.y);
+    SERIAL_ECHOLNPAIR(" ratio.x=", ratio.x, " ratio.y=", ratio.y);
+    SERIAL_ECHOLNPAIR(" z1=", z1, " z2=", z2, " z3=", z3, " z4=", z4);
+    SERIAL_ECHOLNPAIR(" L=", L, " R=", R, " offset=", offset);
+  }
+  last_offset = offset;
+  //*/
+
+  return offset;
+}
+
+#if IS_CARTESIAN && DISABLED(SEGMENT_LEVELED_MOVES)
+
+  #define CELL_INDEX(A,V) ((V - bilinear_start.A) * ABL_BG_FACTOR(A))
+
+  /**
+   * Prepare a bilinear-leveled linear move on Cartesian,
+   * splitting the move where it crosses grid borders.
+   */
+  void bilinear_line_to_destination(const feedRate_t &scaled_fr_mm_s, uint16_t x_splits, uint16_t y_splits) {
+    // Get current and destination cells for this line
+    xy_int_t c1 { CELL_INDEX(x, current_position.x), CELL_INDEX(y, current_position.y) },
+             c2 { CELL_INDEX(x, destination.x), CELL_INDEX(y, destination.y) };
+    LIMIT(c1.x, 0, ABL_BG_POINTS_X - 2);
+    LIMIT(c1.y, 0, ABL_BG_POINTS_Y - 2);
+    LIMIT(c2.x, 0, ABL_BG_POINTS_X - 2);
+    LIMIT(c2.y, 0, ABL_BG_POINTS_Y - 2);
+
+    // Start and end in the same cell? No split needed.
+    if (c1 == c2) {
+      current_position = destination;
+      line_to_current_position(scaled_fr_mm_s);
+      return;
+    }
+
+    #define LINE_SEGMENT_END(A) (current_position.A + (destination.A - current_position.A) * normalized_dist)
+
+    float normalized_dist;
+    xyze_pos_t end;
+    const xy_int8_t gc { _MAX(c1.x, c2.x), _MAX(c1.y, c2.y) };
+
+    // Crosses on the X and not already split on this X?
+    // The x_splits flags are insurance against rounding errors.
+    if (c2.x != c1.x && TEST(x_splits, gc.x)) {
+      // Split on the X grid line
+      CBI(x_splits, gc.x);
+      end = destination;
+      destination.x = bilinear_start.x + ABL_BG_SPACING(x) * gc.x;
+      normalized_dist = (destination.x - current_position.x) / (end.x - current_position.x);
+      destination.y = LINE_SEGMENT_END(y);
+    }
+    // Crosses on the Y and not already split on this Y?
+    else if (c2.y != c1.y && TEST(y_splits, gc.y)) {
+      // Split on the Y grid line
+      CBI(y_splits, gc.y);
+      end = destination;
+      destination.y = bilinear_start.y + ABL_BG_SPACING(y) * gc.y;
+      normalized_dist = (destination.y - current_position.y) / (end.y - current_position.y);
+      destination.x = LINE_SEGMENT_END(x);
+    }
+    else {
+      // Must already have been split on these border(s)
+      // This should be a rare case.
+      current_position = destination;
+      line_to_current_position(scaled_fr_mm_s);
+      return;
+    }
+
+    destination.z = LINE_SEGMENT_END(z);
+    destination.e = LINE_SEGMENT_END(e);
+
+    // Do the split and look for more borders
+    bilinear_line_to_destination(scaled_fr_mm_s, x_splits, y_splits);
+
+    // Restore destination from stack
+    destination = end;
+    bilinear_line_to_destination(scaled_fr_mm_s, x_splits, y_splits);
+  }
+
+#endif // IS_CARTESIAN && !SEGMENT_LEVELED_MOVES
+
+#endif // AUTO_BED_LEVELING_BILINEAR
diff --git a/Marlin/src/feature/bedlevel/abl/abl.h b/Marlin/src/feature/bedlevel/abl/abl.h
new file mode 100644
index 0000000..bbe2411
--- /dev/null
+++ b/Marlin/src/feature/bedlevel/abl/abl.h
@@ -0,0 +1,45 @@
+/**
+ * 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"
+
+extern xy_pos_t bilinear_grid_spacing, bilinear_start;
+extern xy_float_t bilinear_grid_factor;
+extern bed_mesh_t z_values;
+float bilinear_z_offset(const xy_pos_t &raw);
+
+void extrapolate_unprobed_bed_level();
+void print_bilinear_leveling_grid();
+void refresh_bed_level();
+#if ENABLED(ABL_BILINEAR_SUBDIVISION)
+  void print_bilinear_leveling_grid_virt();
+  void bed_level_virt_interpolate();
+#endif
+
+#if IS_CARTESIAN && DISABLED(SEGMENT_LEVELED_MOVES)
+  void bilinear_line_to_destination(const feedRate_t &scaled_fr_mm_s, uint16_t x_splits=0xFFFF, uint16_t y_splits=0xFFFF);
+#endif
+
+#define _GET_MESH_X(I) float(bilinear_start.x + (I) * bilinear_grid_spacing.x)
+#define _GET_MESH_Y(J) float(bilinear_start.y + (J) * bilinear_grid_spacing.y)
+#define Z_VALUES_ARR  z_values
diff --git a/Marlin/src/feature/bedlevel/bedlevel.cpp b/Marlin/src/feature/bedlevel/bedlevel.cpp
new file mode 100644
index 0000000..0e0b87e
--- /dev/null
+++ b/Marlin/src/feature/bedlevel/bedlevel.cpp
@@ -0,0 +1,239 @@
+/**
+ * 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 HAS_LEVELING
+
+#include "bedlevel.h"
+#include "../../module/planner.h"
+
+#if EITHER(MESH_BED_LEVELING, PROBE_MANUALLY)
+  #include "../../module/motion.h"
+#endif
+
+#if ENABLED(PROBE_MANUALLY)
+  bool g29_in_progress = false;
+#endif
+
+#if ENABLED(LCD_BED_LEVELING)
+  #include "../../lcd/marlinui.h"
+#endif
+
+#define DEBUG_OUT ENABLED(DEBUG_LEVELING_FEATURE)
+#include "../../core/debug_out.h"
+
+#if ENABLED(EXTENSIBLE_UI)
+  #include "../../lcd/extui/ui_api.h"
+#endif
+
+bool leveling_is_valid() {
+  return TERN1(MESH_BED_LEVELING,          mbl.has_mesh())
+      && TERN1(AUTO_BED_LEVELING_BILINEAR, !!bilinear_grid_spacing.x)
+      && TERN1(AUTO_BED_LEVELING_UBL,      ubl.mesh_is_valid());
+}
+
+/**
+ * Turn bed leveling on or off, fixing the current
+ * position as-needed.
+ *
+ * Disable: Current position = physical position
+ *  Enable: Current position = "unleveled" physical position
+ */
+void set_bed_leveling_enabled(const bool enable/*=true*/) {
+
+  const bool can_change = TERN1(AUTO_BED_LEVELING_BILINEAR, !enable || leveling_is_valid());
+
+  if (can_change && enable != planner.leveling_active) {
+
+    planner.synchronize();
+
+    #if ENABLED(AUTO_BED_LEVELING_BILINEAR)
+      // Force bilinear_z_offset to re-calculate next time
+      const xyz_pos_t reset { -9999.999, -9999.999, 0 };
+      (void)bilinear_z_offset(reset);
+    #endif
+
+    if (planner.leveling_active) {      // leveling from on to off
+      if (DEBUGGING(LEVELING)) DEBUG_POS("Leveling ON", current_position);
+      // change unleveled current_position to physical current_position without moving steppers.
+      planner.apply_leveling(current_position);
+      planner.leveling_active = false;  // disable only AFTER calling apply_leveling
+      if (DEBUGGING(LEVELING)) DEBUG_POS("...Now OFF", current_position);
+    }
+    else {                              // leveling from off to on
+      if (DEBUGGING(LEVELING)) DEBUG_POS("Leveling OFF", current_position);
+      planner.leveling_active = true;   // enable BEFORE calling unapply_leveling, otherwise ignored
+      // change physical current_position to unleveled current_position without moving steppers.
+      planner.unapply_leveling(current_position);
+      if (DEBUGGING(LEVELING)) DEBUG_POS("...Now ON", current_position);
+    }
+
+    sync_plan_position();
+  }
+}
+
+TemporaryBedLevelingState::TemporaryBedLevelingState(const bool enable) : saved(planner.leveling_active) {
+  set_bed_leveling_enabled(enable);
+}
+
+#if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
+
+  void set_z_fade_height(const float zfh, const bool do_report/*=true*/) {
+
+    if (planner.z_fade_height == zfh) return;
+
+    const bool leveling_was_active = planner.leveling_active;
+    set_bed_leveling_enabled(false);
+
+    planner.set_z_fade_height(zfh);
+
+    if (leveling_was_active) {
+      const xyz_pos_t oldpos = current_position;
+      set_bed_leveling_enabled(true);
+      if (do_report && oldpos != current_position)
+        report_current_position();
+    }
+  }
+
+#endif // ENABLE_LEVELING_FADE_HEIGHT
+
+/**
+ * Reset calibration results to zero.
+ */
+void reset_bed_level() {
+  if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("reset_bed_level");
+  #if ENABLED(AUTO_BED_LEVELING_UBL)
+    ubl.reset();
+  #else
+    set_bed_leveling_enabled(false);
+    #if ENABLED(MESH_BED_LEVELING)
+      mbl.reset();
+    #elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
+      bilinear_start.reset();
+      bilinear_grid_spacing.reset();
+      GRID_LOOP(x, y) {
+        z_values[x][y] = NAN;
+        TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(x, y, 0));
+      }
+    #elif ABL_PLANAR
+      planner.bed_level_matrix.set_to_identity();
+    #endif
+  #endif
+}
+
+#if EITHER(AUTO_BED_LEVELING_BILINEAR, MESH_BED_LEVELING)
+
+  /**
+   * Enable to produce output in JSON format suitable
+   * for SCAD or JavaScript mesh visualizers.
+   *
+   * Visualize meshes in OpenSCAD using the included script.
+   *
+   *   buildroot/shared/scripts/MarlinMesh.scad
+   */
+  //#define SCAD_MESH_OUTPUT
+
+  /**
+   * Print calibration results for plotting or manual frame adjustment.
+   */
+  void print_2d_array(const uint8_t sx, const uint8_t sy, const uint8_t precision, element_2d_fn fn) {
+    #ifndef SCAD_MESH_OUTPUT
+      LOOP_L_N(x, sx) {
+        serial_spaces(precision + (x < 10 ? 3 : 2));
+        SERIAL_ECHO(int(x));
+      }
+      SERIAL_EOL();
+    #endif
+    #ifdef SCAD_MESH_OUTPUT
+      SERIAL_ECHOLNPGM("measured_z = ["); // open 2D array
+    #endif
+    LOOP_L_N(y, sy) {
+      #ifdef SCAD_MESH_OUTPUT
+        SERIAL_ECHOPGM(" [");             // open sub-array
+      #else
+        if (y < 10) SERIAL_CHAR(' ');
+        SERIAL_ECHO(int(y));
+      #endif
+      LOOP_L_N(x, sx) {
+        SERIAL_CHAR(' ');
+        const float offset = fn(x, y);
+        if (!isnan(offset)) {
+          if (offset >= 0) SERIAL_CHAR('+');
+          SERIAL_ECHO_F(offset, int(precision));
+        }
+        else {
+          #ifdef SCAD_MESH_OUTPUT
+            for (uint8_t i = 3; i < precision + 3; i++)
+              SERIAL_CHAR(' ');
+            SERIAL_ECHOPGM("NAN");
+          #else
+            LOOP_L_N(i, precision + 3)
+              SERIAL_CHAR(i ? '=' : ' ');
+          #endif
+        }
+        #ifdef SCAD_MESH_OUTPUT
+          if (x < sx - 1) SERIAL_CHAR(',');
+        #endif
+      }
+      #ifdef SCAD_MESH_OUTPUT
+        SERIAL_CHAR(' ', ']');            // close sub-array
+        if (y < sy - 1) SERIAL_CHAR(',');
+      #endif
+      SERIAL_EOL();
+    }
+    #ifdef SCAD_MESH_OUTPUT
+      SERIAL_ECHOPGM("];");               // close 2D array
+    #endif
+    SERIAL_EOL();
+  }
+
+#endif // AUTO_BED_LEVELING_BILINEAR || MESH_BED_LEVELING
+
+#if EITHER(MESH_BED_LEVELING, PROBE_MANUALLY)
+
+  void _manual_goto_xy(const xy_pos_t &pos) {
+
+    #ifdef MANUAL_PROBE_START_Z
+      constexpr float startz = _MAX(0, MANUAL_PROBE_START_Z);
+      #if MANUAL_PROBE_HEIGHT > 0
+        do_blocking_move_to_xy_z(pos, MANUAL_PROBE_HEIGHT);
+        do_blocking_move_to_z(startz);
+      #else
+        do_blocking_move_to_xy_z(pos, startz);
+      #endif
+    #elif MANUAL_PROBE_HEIGHT > 0
+      const float prev_z = current_position.z;
+      do_blocking_move_to_xy_z(pos, MANUAL_PROBE_HEIGHT);
+      do_blocking_move_to_z(prev_z);
+    #else
+      do_blocking_move_to_xy(pos);
+    #endif
+
+    current_position = pos;
+
+    TERN_(LCD_BED_LEVELING, ui.wait_for_move = false);
+  }
+
+#endif
+
+#endif // HAS_LEVELING
diff --git a/Marlin/src/feature/bedlevel/bedlevel.h b/Marlin/src/feature/bedlevel/bedlevel.h
new file mode 100644
index 0000000..a33f08a
--- /dev/null
+++ b/Marlin/src/feature/bedlevel/bedlevel.h
@@ -0,0 +1,102 @@
+/**
+ * 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"
+
+#if EITHER(RESTORE_LEVELING_AFTER_G28, ENABLE_LEVELING_AFTER_G28)
+  #define G28_L0_ENSURES_LEVELING_OFF 1
+#endif
+
+#if ENABLED(PROBE_MANUALLY)
+  extern bool g29_in_progress;
+#else
+  constexpr bool g29_in_progress = false;
+#endif
+
+bool leveling_is_valid();
+void set_bed_leveling_enabled(const bool enable=true);
+void reset_bed_level();
+
+#if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
+  void set_z_fade_height(const float zfh, const bool do_report=true);
+#endif
+
+#if EITHER(MESH_BED_LEVELING, PROBE_MANUALLY)
+  void _manual_goto_xy(const xy_pos_t &pos);
+#endif
+
+/**
+ * A class to save and change the bed leveling state,
+ * then restore it when it goes out of scope.
+ */
+class TemporaryBedLevelingState {
+  bool saved;
+  public:
+    TemporaryBedLevelingState(const bool enable);
+    ~TemporaryBedLevelingState() { set_bed_leveling_enabled(saved); }
+};
+#define TEMPORARY_BED_LEVELING_STATE(enable) const TemporaryBedLevelingState tbls(enable)
+
+#if HAS_MESH
+
+  typedef float bed_mesh_t[GRID_MAX_POINTS_X][GRID_MAX_POINTS_Y];
+
+  #if ENABLED(AUTO_BED_LEVELING_BILINEAR)
+    #include "abl/abl.h"
+  #elif ENABLED(AUTO_BED_LEVELING_UBL)
+    #include "ubl/ubl.h"
+  #elif ENABLED(MESH_BED_LEVELING)
+    #include "mbl/mesh_bed_leveling.h"
+  #endif
+
+  #define Z_VALUES(X,Y) Z_VALUES_ARR[X][Y]
+  #define _GET_MESH_POS(M) { _GET_MESH_X(M.a), _GET_MESH_Y(M.b) }
+
+  #if EITHER(AUTO_BED_LEVELING_BILINEAR, MESH_BED_LEVELING)
+
+    #include <stdint.h>
+
+    typedef float (*element_2d_fn)(const uint8_t, const uint8_t);
+
+    /**
+     * Print calibration results for plotting or manual frame adjustment.
+     */
+    void print_2d_array(const uint8_t sx, const uint8_t sy, const uint8_t precision, element_2d_fn fn);
+
+  #endif
+
+  struct mesh_index_pair {
+    xy_int8_t pos;
+    float distance;   // When populated, the distance from the search location
+    void invalidate() { pos = -1; }
+    bool valid() const { return pos.x >= 0 && pos.y >= 0; }
+    #if ENABLED(AUTO_BED_LEVELING_UBL)
+      xy_pos_t meshpos() {
+        return { ubl.mesh_index_to_xpos(pos.x), ubl.mesh_index_to_ypos(pos.y) };
+      }
+    #endif
+    operator xy_int8_t&() { return pos; }
+    operator const xy_int8_t&() const { return pos; }
+  };
+
+#endif
diff --git a/Marlin/src/feature/bedlevel/mbl/mesh_bed_leveling.cpp b/Marlin/src/feature/bedlevel/mbl/mesh_bed_leveling.cpp
new file mode 100644
index 0000000..ec5b95c
--- /dev/null
+++ b/Marlin/src/feature/bedlevel/mbl/mesh_bed_leveling.cpp
@@ -0,0 +1,133 @@
+/**
+ * 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 ENABLED(MESH_BED_LEVELING)
+
+  #include "../bedlevel.h"
+
+  #include "../../../module/motion.h"
+
+  #if ENABLED(EXTENSIBLE_UI)
+    #include "../../../lcd/extui/ui_api.h"
+  #endif
+
+  mesh_bed_leveling mbl;
+
+  float mesh_bed_leveling::z_offset,
+        mesh_bed_leveling::z_values[GRID_MAX_POINTS_X][GRID_MAX_POINTS_Y],
+        mesh_bed_leveling::index_to_xpos[GRID_MAX_POINTS_X],
+        mesh_bed_leveling::index_to_ypos[GRID_MAX_POINTS_Y];
+
+  mesh_bed_leveling::mesh_bed_leveling() {
+    LOOP_L_N(i, GRID_MAX_POINTS_X)
+      index_to_xpos[i] = MESH_MIN_X + i * (MESH_X_DIST);
+    LOOP_L_N(i, GRID_MAX_POINTS_Y)
+      index_to_ypos[i] = MESH_MIN_Y + i * (MESH_Y_DIST);
+    reset();
+  }
+
+  void mesh_bed_leveling::reset() {
+    z_offset = 0;
+    ZERO(z_values);
+    #if ENABLED(EXTENSIBLE_UI)
+      GRID_LOOP(x, y) ExtUI::onMeshUpdate(x, y, 0);
+    #endif
+  }
+
+  #if IS_CARTESIAN && DISABLED(SEGMENT_LEVELED_MOVES)
+
+    /**
+     * Prepare a mesh-leveled linear move in a Cartesian setup,
+     * splitting the move where it crosses mesh borders.
+     */
+    void mesh_bed_leveling::line_to_destination(const feedRate_t &scaled_fr_mm_s, uint8_t x_splits, uint8_t y_splits) {
+      // Get current and destination cells for this line
+      xy_int8_t scel = cell_indexes(current_position), ecel = cell_indexes(destination);
+      NOMORE(scel.x, GRID_MAX_POINTS_X - 2);
+      NOMORE(scel.y, GRID_MAX_POINTS_Y - 2);
+      NOMORE(ecel.x, GRID_MAX_POINTS_X - 2);
+      NOMORE(ecel.y, GRID_MAX_POINTS_Y - 2);
+
+      // Start and end in the same cell? No split needed.
+      if (scel == ecel) {
+        current_position = destination;
+        line_to_current_position(scaled_fr_mm_s);
+        return;
+      }
+
+      #define MBL_SEGMENT_END(A) (current_position.A + (destination.A - current_position.A) * normalized_dist)
+
+      float normalized_dist;
+      xyze_pos_t dest;
+      const int8_t gcx = _MAX(scel.x, ecel.x), gcy = _MAX(scel.y, ecel.y);
+
+      // Crosses on the X and not already split on this X?
+      // The x_splits flags are insurance against rounding errors.
+      if (ecel.x != scel.x && TEST(x_splits, gcx)) {
+        // Split on the X grid line
+        CBI(x_splits, gcx);
+        dest = destination;
+        destination.x = index_to_xpos[gcx];
+        normalized_dist = (destination.x - current_position.x) / (dest.x - current_position.x);
+        destination.y = MBL_SEGMENT_END(y);
+      }
+      // Crosses on the Y and not already split on this Y?
+      else if (ecel.y != scel.y && TEST(y_splits, gcy)) {
+        // Split on the Y grid line
+        CBI(y_splits, gcy);
+        dest = destination;
+        destination.y = index_to_ypos[gcy];
+        normalized_dist = (destination.y - current_position.y) / (dest.y - current_position.y);
+        destination.x = MBL_SEGMENT_END(x);
+      }
+      else {
+        // Must already have been split on these border(s)
+        // This should be a rare case.
+        current_position = destination;
+        line_to_current_position(scaled_fr_mm_s);
+        return;
+      }
+
+      destination.z = MBL_SEGMENT_END(z);
+      destination.e = MBL_SEGMENT_END(e);
+
+      // Do the split and look for more borders
+      line_to_destination(scaled_fr_mm_s, x_splits, y_splits);
+
+      // Restore destination from stack
+      destination = dest;
+      line_to_destination(scaled_fr_mm_s, x_splits, y_splits);
+    }
+
+  #endif // IS_CARTESIAN && !SEGMENT_LEVELED_MOVES
+
+  void mesh_bed_leveling::report_mesh() {
+    SERIAL_ECHOPAIR_F(STRINGIFY(GRID_MAX_POINTS_X) "x" STRINGIFY(GRID_MAX_POINTS_Y) " mesh. Z offset: ", z_offset, 5);
+    SERIAL_ECHOLNPGM("\nMeasured points:");
+    print_2d_array(GRID_MAX_POINTS_X, GRID_MAX_POINTS_Y, 5,
+      [](const uint8_t ix, const uint8_t iy) { return z_values[ix][iy]; }
+    );
+  }
+
+#endif // MESH_BED_LEVELING
diff --git a/Marlin/src/feature/bedlevel/mbl/mesh_bed_leveling.h b/Marlin/src/feature/bedlevel/mbl/mesh_bed_leveling.h
new file mode 100644
index 0000000..ade7a93
--- /dev/null
+++ b/Marlin/src/feature/bedlevel/mbl/mesh_bed_leveling.h
@@ -0,0 +1,127 @@
+/**
+ * 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"
+
+enum MeshLevelingState : char {
+  MeshReport,     // G29 S0
+  MeshStart,      // G29 S1
+  MeshNext,       // G29 S2
+  MeshSet,        // G29 S3
+  MeshSetZOffset, // G29 S4
+  MeshReset       // G29 S5
+};
+
+#define MESH_X_DIST (float(MESH_MAX_X - (MESH_MIN_X)) / float(GRID_MAX_POINTS_X - 1))
+#define MESH_Y_DIST (float(MESH_MAX_Y - (MESH_MIN_Y)) / float(GRID_MAX_POINTS_Y - 1))
+#define _GET_MESH_X(I) mbl.index_to_xpos[I]
+#define _GET_MESH_Y(J) mbl.index_to_ypos[J]
+#define Z_VALUES_ARR mbl.z_values
+
+class mesh_bed_leveling {
+public:
+  static float z_offset,
+               z_values[GRID_MAX_POINTS_X][GRID_MAX_POINTS_Y],
+               index_to_xpos[GRID_MAX_POINTS_X],
+               index_to_ypos[GRID_MAX_POINTS_Y];
+
+  mesh_bed_leveling();
+
+  static void report_mesh();
+
+  static void reset();
+
+  FORCE_INLINE static bool has_mesh() {
+    GRID_LOOP(x, y) if (z_values[x][y]) return true;
+    return false;
+  }
+
+  static void set_z(const int8_t px, const int8_t py, const float &z) { z_values[px][py] = z; }
+
+  static inline void zigzag(const int8_t index, int8_t &px, int8_t &py) {
+    px = index % (GRID_MAX_POINTS_X);
+    py = index / (GRID_MAX_POINTS_X);
+    if (py & 1) px = (GRID_MAX_POINTS_X - 1) - px; // Zig zag
+  }
+
+  static void set_zigzag_z(const int8_t index, const float &z) {
+    int8_t px, py;
+    zigzag(index, px, py);
+    set_z(px, py, z);
+  }
+
+  static int8_t cell_index_x(const float &x) {
+    int8_t cx = (x - (MESH_MIN_X)) * RECIPROCAL(MESH_X_DIST);
+    return constrain(cx, 0, (GRID_MAX_POINTS_X) - 2);
+  }
+  static int8_t cell_index_y(const float &y) {
+    int8_t cy = (y - (MESH_MIN_Y)) * RECIPROCAL(MESH_Y_DIST);
+    return constrain(cy, 0, (GRID_MAX_POINTS_Y) - 2);
+  }
+  static inline xy_int8_t cell_indexes(const float &x, const float &y) {
+    return { cell_index_x(x), cell_index_y(y) };
+  }
+  static inline xy_int8_t cell_indexes(const xy_pos_t &xy) { return cell_indexes(xy.x, xy.y); }
+
+  static int8_t probe_index_x(const float &x) {
+    int8_t px = (x - (MESH_MIN_X) + 0.5f * (MESH_X_DIST)) * RECIPROCAL(MESH_X_DIST);
+    return WITHIN(px, 0, GRID_MAX_POINTS_X - 1) ? px : -1;
+  }
+  static int8_t probe_index_y(const float &y) {
+    int8_t py = (y - (MESH_MIN_Y) + 0.5f * (MESH_Y_DIST)) * RECIPROCAL(MESH_Y_DIST);
+    return WITHIN(py, 0, GRID_MAX_POINTS_Y - 1) ? py : -1;
+  }
+  static inline xy_int8_t probe_indexes(const float &x, const float &y) {
+    return { probe_index_x(x), probe_index_y(y) };
+  }
+  static inline xy_int8_t probe_indexes(const xy_pos_t &xy) { return probe_indexes(xy.x, xy.y); }
+
+  static float calc_z0(const float &a0, const float &a1, const float &z1, const float &a2, const float &z2) {
+    const float delta_z = (z2 - z1) / (a2 - a1),
+                delta_a = a0 - a1;
+    return z1 + delta_a * delta_z;
+  }
+
+  static float get_z(const xy_pos_t &pos
+    #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
+      , const float &factor=1.0f
+    #endif
+  ) {
+    #if DISABLED(ENABLE_LEVELING_FADE_HEIGHT)
+      constexpr float factor = 1.0f;
+    #endif
+    const xy_int8_t ind = cell_indexes(pos);
+    const float x1 = index_to_xpos[ind.x], x2 = index_to_xpos[ind.x+1],
+                y1 = index_to_xpos[ind.y], y2 = index_to_xpos[ind.y+1],
+                z1 = calc_z0(pos.x, x1, z_values[ind.x][ind.y  ], x2, z_values[ind.x+1][ind.y  ]),
+                z2 = calc_z0(pos.x, x1, z_values[ind.x][ind.y+1], x2, z_values[ind.x+1][ind.y+1]);
+
+    return z_offset + calc_z0(pos.y, y1, z1, y2, z2) * factor;
+  }
+
+  #if IS_CARTESIAN && DISABLED(SEGMENT_LEVELED_MOVES)
+    static void line_to_destination(const feedRate_t &scaled_fr_mm_s, uint8_t x_splits=0xFF, uint8_t y_splits=0xFF);
+  #endif
+};
+
+extern mesh_bed_leveling mbl;
diff --git a/Marlin/src/feature/bedlevel/ubl/ubl.cpp b/Marlin/src/feature/bedlevel/ubl/ubl.cpp
new file mode 100644
index 0000000..b0640e5
--- /dev/null
+++ b/Marlin/src/feature/bedlevel/ubl/ubl.cpp
@@ -0,0 +1,257 @@
+/**
+ * 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 ENABLED(AUTO_BED_LEVELING_UBL)
+
+  #include "../bedlevel.h"
+
+  unified_bed_leveling ubl;
+
+  #include "../../../MarlinCore.h"
+  #include "../../../gcode/gcode.h"
+
+  #include "../../../module/settings.h"
+  #include "../../../module/planner.h"
+  #include "../../../module/motion.h"
+  #include "../../../module/probe.h"
+
+  #if ENABLED(EXTENSIBLE_UI)
+    #include "../../../lcd/extui/ui_api.h"
+  #endif
+
+  #include "math.h"
+
+  void unified_bed_leveling::echo_name() { SERIAL_ECHOPGM("Unified Bed Leveling"); }
+
+  void unified_bed_leveling::report_current_mesh() {
+    if (!leveling_is_valid()) return;
+    SERIAL_ECHO_MSG("  G29 I999");
+    GRID_LOOP(x, y)
+      if (!isnan(z_values[x][y])) {
+        SERIAL_ECHO_START();
+        SERIAL_ECHOPAIR("  M421 I", int(x), " J", int(y));
+        SERIAL_ECHOLNPAIR_F_P(SP_Z_STR, z_values[x][y], 4);
+        serial_delay(75); // Prevent Printrun from exploding
+      }
+  }
+
+  void unified_bed_leveling::report_state() {
+    echo_name();
+    SERIAL_ECHO_TERNARY(planner.leveling_active, " System v" UBL_VERSION " ", "", "in", "active\n");
+    serial_delay(50);
+  }
+
+  int8_t unified_bed_leveling::storage_slot;
+
+  float unified_bed_leveling::z_values[GRID_MAX_POINTS_X][GRID_MAX_POINTS_Y];
+
+  #define _GRIDPOS(A,N) (MESH_MIN_##A + N * (MESH_##A##_DIST))
+
+  const float
+  unified_bed_leveling::_mesh_index_to_xpos[GRID_MAX_POINTS_X] PROGMEM = ARRAY_N(GRID_MAX_POINTS_X,
+    _GRIDPOS(X,  0), _GRIDPOS(X,  1), _GRIDPOS(X,  2), _GRIDPOS(X,  3),
+    _GRIDPOS(X,  4), _GRIDPOS(X,  5), _GRIDPOS(X,  6), _GRIDPOS(X,  7),
+    _GRIDPOS(X,  8), _GRIDPOS(X,  9), _GRIDPOS(X, 10), _GRIDPOS(X, 11),
+    _GRIDPOS(X, 12), _GRIDPOS(X, 13), _GRIDPOS(X, 14), _GRIDPOS(X, 15)
+  ),
+  unified_bed_leveling::_mesh_index_to_ypos[GRID_MAX_POINTS_Y] PROGMEM = ARRAY_N(GRID_MAX_POINTS_Y,
+    _GRIDPOS(Y,  0), _GRIDPOS(Y,  1), _GRIDPOS(Y,  2), _GRIDPOS(Y,  3),
+    _GRIDPOS(Y,  4), _GRIDPOS(Y,  5), _GRIDPOS(Y,  6), _GRIDPOS(Y,  7),
+    _GRIDPOS(Y,  8), _GRIDPOS(Y,  9), _GRIDPOS(Y, 10), _GRIDPOS(Y, 11),
+    _GRIDPOS(Y, 12), _GRIDPOS(Y, 13), _GRIDPOS(Y, 14), _GRIDPOS(Y, 15)
+  );
+
+  volatile int16_t unified_bed_leveling::encoder_diff;
+
+  unified_bed_leveling::unified_bed_leveling() { reset(); }
+
+  void unified_bed_leveling::reset() {
+    const bool was_enabled = planner.leveling_active;
+    set_bed_leveling_enabled(false);
+    storage_slot = -1;
+    ZERO(z_values);
+    #if ENABLED(EXTENSIBLE_UI)
+      GRID_LOOP(x, y) ExtUI::onMeshUpdate(x, y, 0);
+    #endif
+    if (was_enabled) report_current_position();
+  }
+
+  void unified_bed_leveling::invalidate() {
+    set_bed_leveling_enabled(false);
+    set_all_mesh_points_to_value(NAN);
+  }
+
+  void unified_bed_leveling::set_all_mesh_points_to_value(const float value) {
+    GRID_LOOP(x, y) {
+      z_values[x][y] = value;
+      TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(x, y, value));
+    }
+  }
+
+  #if ENABLED(OPTIMIZED_MESH_STORAGE)
+
+    constexpr float mesh_store_scaling = 1000;
+    constexpr int16_t Z_STEPS_NAN = INT16_MAX;
+
+    void unified_bed_leveling::set_store_from_mesh(const bed_mesh_t &in_values, mesh_store_t &stored_values) {
+      auto z_to_store = [](const float &z) {
+        if (isnan(z)) return Z_STEPS_NAN;
+        const int32_t z_scaled = TRUNC(z * mesh_store_scaling);
+        if (z_scaled == Z_STEPS_NAN || !WITHIN(z_scaled, INT16_MIN, INT16_MAX))
+          return Z_STEPS_NAN; // If Z is out of range, return our custom 'NaN'
+        return int16_t(z_scaled);
+      };
+      GRID_LOOP(x, y) stored_values[x][y] = z_to_store(in_values[x][y]);
+    }
+
+    void unified_bed_leveling::set_mesh_from_store(const mesh_store_t &stored_values, bed_mesh_t &out_values) {
+      auto store_to_z = [](const int16_t z_scaled) {
+        return z_scaled == Z_STEPS_NAN ? NAN : z_scaled / mesh_store_scaling;
+      };
+      GRID_LOOP(x, y) out_values[x][y] = store_to_z(stored_values[x][y]);
+    }
+
+  #endif // OPTIMIZED_MESH_STORAGE
+
+  static void serial_echo_xy(const uint8_t sp, const int16_t x, const int16_t y) {
+    SERIAL_ECHO_SP(sp);
+    SERIAL_CHAR('(');
+    if (x < 100) { SERIAL_CHAR(' '); if (x < 10) SERIAL_CHAR(' '); }
+    SERIAL_ECHO(x);
+    SERIAL_CHAR(',');
+    if (y < 100) { SERIAL_CHAR(' '); if (y < 10) SERIAL_CHAR(' '); }
+    SERIAL_ECHO(y);
+    SERIAL_CHAR(')');
+    serial_delay(5);
+  }
+
+  static void serial_echo_column_labels(const uint8_t sp) {
+    SERIAL_ECHO_SP(7);
+    LOOP_L_N(i, GRID_MAX_POINTS_X) {
+      if (i < 10) SERIAL_CHAR(' ');
+      SERIAL_ECHO((int)i);
+      SERIAL_ECHO_SP(sp);
+    }
+    serial_delay(10);
+  }
+
+  /**
+   * Produce one of these mesh maps:
+   *   0: Human-readable
+   *   1: CSV format for spreadsheet import
+   *   2: TODO: Display on Graphical LCD
+   *   4: Compact Human-Readable
+   */
+  void unified_bed_leveling::display_map(const int map_type) {
+    const bool was = gcode.set_autoreport_paused(true);
+
+    constexpr uint8_t eachsp = 1 + 6 + 1,                           // [-3.567]
+                      twixt = eachsp * (GRID_MAX_POINTS_X) - 9 * 2; // Leading 4sp, Coordinates 9sp each
+
+    const bool human = !(map_type & 0x3), csv = map_type == 1, lcd = map_type == 2, comp = map_type & 0x4;
+
+    SERIAL_ECHOPGM("\nBed Topography Report");
+    if (human) {
+      SERIAL_ECHOLNPGM(":\n");
+      serial_echo_xy(4, MESH_MIN_X, MESH_MAX_Y);
+      serial_echo_xy(twixt, MESH_MAX_X, MESH_MAX_Y);
+      SERIAL_EOL();
+      serial_echo_column_labels(eachsp - 2);
+    }
+    else {
+      SERIAL_ECHOPGM(" for ");
+      serialprintPGM(csv ? PSTR("CSV:\n") : PSTR("LCD:\n"));
+    }
+
+    // Add XY probe offset from extruder because probe.probe_at_point() subtracts them when
+    // moving to the XY position to be measured. This ensures better agreement between
+    // the current Z position after G28 and the mesh values.
+    const xy_int8_t curr = closest_indexes(xy_pos_t(current_position) + probe.offset_xy);
+
+    if (!lcd) SERIAL_EOL();
+    for (int8_t j = GRID_MAX_POINTS_Y - 1; j >= 0; j--) {
+
+      // Row Label (J index)
+      if (human) {
+        if (j < 10) SERIAL_CHAR(' ');
+        SERIAL_ECHO(j);
+        SERIAL_ECHOPGM(" |");
+      }
+
+      // Row Values (I indexes)
+      LOOP_L_N(i, GRID_MAX_POINTS_X) {
+
+        // Opening Brace or Space
+        const bool is_current = i == curr.x && j == curr.y;
+        if (human) SERIAL_CHAR(is_current ? '[' : ' ');
+
+        // Z Value at current I, J
+        const float f = z_values[i][j];
+        if (lcd) {
+          // TODO: Display on Graphical LCD
+        }
+        else if (isnan(f))
+          serialprintPGM(human ? PSTR("  .   ") : PSTR("NAN"));
+        else if (human || csv) {
+          if (human && f >= 0.0) SERIAL_CHAR(f > 0 ? '+' : ' ');  // Space for positive ('-' for negative)
+          SERIAL_ECHO_F(f, 3);                                    // Positive: 5 digits, Negative: 6 digits
+        }
+        if (csv && i < GRID_MAX_POINTS_X - 1) SERIAL_CHAR('\t');
+
+        // Closing Brace or Space
+        if (human) SERIAL_CHAR(is_current ? ']' : ' ');
+
+        SERIAL_FLUSHTX();
+        idle_no_sleep();
+      }
+      if (!lcd) SERIAL_EOL();
+
+      // A blank line between rows (unless compact)
+      if (j && human && !comp) SERIAL_ECHOLNPGM("   |");
+    }
+
+    if (human) {
+      serial_echo_column_labels(eachsp - 2);
+      SERIAL_EOL();
+      serial_echo_xy(4, MESH_MIN_X, MESH_MIN_Y);
+      serial_echo_xy(twixt, MESH_MAX_X, MESH_MIN_Y);
+      SERIAL_EOL();
+      SERIAL_EOL();
+    }
+
+    gcode.set_autoreport_paused(was);
+  }
+
+  bool unified_bed_leveling::sanity_check() {
+    uint8_t error_flag = 0;
+
+    if (settings.calc_num_meshes() < 1) {
+      SERIAL_ECHOLNPGM("?Mesh too big for EEPROM.");
+      error_flag++;
+    }
+
+    return !!error_flag;
+  }
+
+#endif // AUTO_BED_LEVELING_UBL
diff --git a/Marlin/src/feature/bedlevel/ubl/ubl.h b/Marlin/src/feature/bedlevel/ubl/ubl.h
new file mode 100644
index 0000000..876063c
--- /dev/null
+++ b/Marlin/src/feature/bedlevel/ubl/ubl.h
@@ -0,0 +1,328 @@
+/**
+ * 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 UBL_DEVEL_DEBUGGING
+
+#include "../../../module/motion.h"
+
+#define DEBUG_OUT ENABLED(DEBUG_LEVELING_FEATURE)
+#include "../../../core/debug_out.h"
+
+#define UBL_VERSION "1.01"
+#define UBL_OK false
+#define UBL_ERR true
+
+enum MeshPointType : char { INVALID, REAL, SET_IN_BITMAP };
+
+// External references
+
+struct mesh_index_pair;
+
+#define MESH_X_DIST (float(MESH_MAX_X - (MESH_MIN_X)) / float(GRID_MAX_POINTS_X - 1))
+#define MESH_Y_DIST (float(MESH_MAX_Y - (MESH_MIN_Y)) / float(GRID_MAX_POINTS_Y - 1))
+
+#if ENABLED(OPTIMIZED_MESH_STORAGE)
+  typedef int16_t mesh_store_t[GRID_MAX_POINTS_X][GRID_MAX_POINTS_Y];
+#endif
+
+class unified_bed_leveling {
+  private:
+
+    static int    g29_verbose_level,
+                  g29_phase_value,
+                  g29_repetition_cnt,
+                  g29_storage_slot,
+                  g29_map_type;
+    static bool   g29_c_flag;
+    static float  g29_card_thickness,
+                  g29_constant;
+    static xy_pos_t g29_pos;
+    static xy_bool_t xy_seen;
+
+    #if HAS_BED_PROBE
+      static int  g29_grid_size;
+    #endif
+
+    #if IS_NEWPANEL
+      static void move_z_with_encoder(const float &multiplier);
+      static float measure_point_with_encoder();
+      static float measure_business_card_thickness();
+      static void manually_probe_remaining_mesh(const xy_pos_t&, const float&, const float&, const bool) _O0;
+      static void fine_tune_mesh(const xy_pos_t &pos, const bool do_ubl_mesh_map) _O0;
+    #endif
+
+    static bool g29_parameter_parsing() _O0;
+    static void shift_mesh_height();
+    static void probe_entire_mesh(const xy_pos_t &near, const bool do_ubl_mesh_map, const bool stow_probe, const bool do_furthest) _O0;
+    static void tilt_mesh_based_on_3pts(const float &z1, const float &z2, const float &z3);
+    static void tilt_mesh_based_on_probed_grid(const bool do_ubl_mesh_map);
+    static bool smart_fill_one(const uint8_t x, const uint8_t y, const int8_t xdir, const int8_t ydir);
+    static inline bool smart_fill_one(const xy_uint8_t &pos, const xy_uint8_t &dir) {
+      return smart_fill_one(pos.x, pos.y, dir.x, dir.y);
+    }
+    static void smart_fill_mesh();
+
+    #if ENABLED(UBL_DEVEL_DEBUGGING)
+      static void g29_what_command();
+      static void g29_eeprom_dump();
+      static void g29_compare_current_mesh_to_stored_mesh();
+    #endif
+
+  public:
+
+    static void echo_name();
+    static void report_current_mesh();
+    static void report_state();
+    static void save_ubl_active_state_and_disable();
+    static void restore_ubl_active_state_and_leave();
+    static void display_map(const int) _O0;
+    static mesh_index_pair find_closest_mesh_point_of_type(const MeshPointType, const xy_pos_t&, const bool=false, MeshFlags *done_flags=nullptr) _O0;
+    static mesh_index_pair find_furthest_invalid_mesh_point() _O0;
+    static void reset();
+    static void invalidate();
+    static void set_all_mesh_points_to_value(const float value);
+    static void adjust_mesh_to_mean(const bool cflag, const float value);
+    static bool sanity_check();
+
+    static void G29() _O0;                          // O0 for no optimization
+    static void smart_fill_wlsf(const float &) _O2; // O2 gives smaller code than Os on A2560
+
+    static int8_t storage_slot;
+
+    static bed_mesh_t z_values;
+    #if ENABLED(OPTIMIZED_MESH_STORAGE)
+      static void set_store_from_mesh(const bed_mesh_t &in_values, mesh_store_t &stored_values);
+      static void set_mesh_from_store(const mesh_store_t &stored_values, bed_mesh_t &out_values);
+    #endif
+    static const float _mesh_index_to_xpos[GRID_MAX_POINTS_X],
+                       _mesh_index_to_ypos[GRID_MAX_POINTS_Y];
+
+    #if HAS_LCD_MENU
+      static bool lcd_map_control;
+      static void steppers_were_disabled();
+    #else
+      static inline void steppers_were_disabled() {}
+    #endif
+
+    static volatile int16_t encoder_diff; // Volatile because buttons may changed it at interrupt time
+
+    unified_bed_leveling();
+
+    FORCE_INLINE static void set_z(const int8_t px, const int8_t py, const float &z) { z_values[px][py] = z; }
+
+    static int8_t cell_index_x_raw(const float &x) {
+      return FLOOR((x - (MESH_MIN_X)) * RECIPROCAL(MESH_X_DIST));
+    }
+
+    static int8_t cell_index_y_raw(const float &y) {
+      return FLOOR((y - (MESH_MIN_Y)) * RECIPROCAL(MESH_Y_DIST));
+    }
+
+    static int8_t cell_index_x_valid(const float &x) {
+      return WITHIN(cell_index_x_raw(x), 0, (GRID_MAX_POINTS_X - 2));
+    }
+
+    static int8_t cell_index_y_valid(const float &y) {
+      return WITHIN(cell_index_y_raw(y), 0, (GRID_MAX_POINTS_Y - 2));
+    }
+
+    static int8_t cell_index_x(const float &x) {
+      return constrain(cell_index_x_raw(x), 0, (GRID_MAX_POINTS_X) - 2);
+    }
+
+    static int8_t cell_index_y(const float &y) {
+      return constrain(cell_index_y_raw(y), 0, (GRID_MAX_POINTS_Y) - 2);
+    }
+
+    static inline xy_int8_t cell_indexes(const float &x, const float &y) {
+      return { cell_index_x(x), cell_index_y(y) };
+    }
+    static inline xy_int8_t cell_indexes(const xy_pos_t &xy) { return cell_indexes(xy.x, xy.y); }
+
+    static int8_t closest_x_index(const float &x) {
+      const int8_t px = (x - (MESH_MIN_X) + (MESH_X_DIST) * 0.5) * RECIPROCAL(MESH_X_DIST);
+      return WITHIN(px, 0, GRID_MAX_POINTS_X - 1) ? px : -1;
+    }
+    static int8_t closest_y_index(const float &y) {
+      const int8_t py = (y - (MESH_MIN_Y) + (MESH_Y_DIST) * 0.5) * RECIPROCAL(MESH_Y_DIST);
+      return WITHIN(py, 0, GRID_MAX_POINTS_Y - 1) ? py : -1;
+    }
+    static inline xy_int8_t closest_indexes(const xy_pos_t &xy) {
+      return { closest_x_index(xy.x), closest_y_index(xy.y) };
+    }
+
+    /**
+     *                           z2   --|
+     *                 z0        |      |
+     *                  |        |      + (z2-z1)
+     *   z1             |        |      |
+     * ---+-------------+--------+--  --|
+     *   a1            a0        a2
+     *    |<---delta_a---------->|
+     *
+     *  calc_z0 is the basis for all the Mesh Based correction. It is used to
+     *  find the expected Z Height at a position between two known Z-Height locations.
+     *
+     *  It is fairly expensive with its 4 floating point additions and 2 floating point
+     *  multiplications.
+     */
+    FORCE_INLINE static float calc_z0(const float &a0, const float &a1, const float &z1, const float &a2, const float &z2) {
+      return z1 + (z2 - z1) * (a0 - a1) / (a2 - a1);
+    }
+
+    #ifdef UBL_Z_RAISE_WHEN_OFF_MESH
+      #define _UBL_OUTER_Z_RAISE UBL_Z_RAISE_WHEN_OFF_MESH
+    #else
+      #define _UBL_OUTER_Z_RAISE NAN
+    #endif
+
+    /**
+     * z_correction_for_x_on_horizontal_mesh_line is an optimization for
+     * the case where the printer is making a vertical line that only crosses horizontal mesh lines.
+     */
+    static inline float z_correction_for_x_on_horizontal_mesh_line(const float &rx0, const int x1_i, const int yi) {
+      if (!WITHIN(x1_i, 0, GRID_MAX_POINTS_X - 1) || !WITHIN(yi, 0, GRID_MAX_POINTS_Y - 1)) {
+
+        if (DEBUGGING(LEVELING)) {
+          if (WITHIN(x1_i, 0, GRID_MAX_POINTS_X - 1)) DEBUG_ECHOPGM("yi"); else DEBUG_ECHOPGM("x1_i");
+          DEBUG_ECHOLNPAIR(" out of bounds in z_correction_for_x_on_horizontal_mesh_line(rx0=", rx0, ",x1_i=", x1_i, ",yi=", yi, ")");
+        }
+
+        // The requested location is off the mesh. Return UBL_Z_RAISE_WHEN_OFF_MESH or NAN.
+        return _UBL_OUTER_Z_RAISE;
+      }
+
+      const float xratio = (rx0 - mesh_index_to_xpos(x1_i)) * RECIPROCAL(MESH_X_DIST),
+                  z1 = z_values[x1_i][yi];
+
+      return z1 + xratio * (z_values[_MIN(x1_i, GRID_MAX_POINTS_X - 2) + 1][yi] - z1); // Don't allow x1_i+1 to be past the end of the array
+                                                                                      // If it is, it is clamped to the last element of the
+                                                                                      // z_values[][] array and no correction is applied.
+    }
+
+    //
+    // See comments above for z_correction_for_x_on_horizontal_mesh_line
+    //
+    static inline float z_correction_for_y_on_vertical_mesh_line(const float &ry0, const int xi, const int y1_i) {
+      if (!WITHIN(xi, 0, GRID_MAX_POINTS_X - 1) || !WITHIN(y1_i, 0, GRID_MAX_POINTS_Y - 1)) {
+
+        if (DEBUGGING(LEVELING)) {
+          if (WITHIN(xi, 0, GRID_MAX_POINTS_X - 1)) DEBUG_ECHOPGM("y1_i"); else DEBUG_ECHOPGM("xi");
+          DEBUG_ECHOLNPAIR(" out of bounds in z_correction_for_y_on_vertical_mesh_line(ry0=", ry0, ", xi=", xi, ", y1_i=", y1_i, ")");
+        }
+
+        // The requested location is off the mesh. Return UBL_Z_RAISE_WHEN_OFF_MESH or NAN.
+        return _UBL_OUTER_Z_RAISE;
+      }
+
+      const float yratio = (ry0 - mesh_index_to_ypos(y1_i)) * RECIPROCAL(MESH_Y_DIST),
+                  z1 = z_values[xi][y1_i];
+
+      return z1 + yratio * (z_values[xi][_MIN(y1_i, GRID_MAX_POINTS_Y - 2) + 1] - z1); // Don't allow y1_i+1 to be past the end of the array
+                                                                                      // If it is, it is clamped to the last element of the
+                                                                                      // z_values[][] array and no correction is applied.
+    }
+
+    /**
+     * This is the generic Z-Correction. It works anywhere within a Mesh Cell. It first
+     * does a linear interpolation along both of the bounding X-Mesh-Lines to find the
+     * Z-Height at both ends. Then it does a linear interpolation of these heights based
+     * on the Y position within the cell.
+     */
+    static float get_z_correction(const float &rx0, const float &ry0) {
+      const int8_t cx = cell_index_x(rx0), cy = cell_index_y(ry0); // return values are clamped
+
+      /**
+       * Check if the requested location is off the mesh.  If so, and
+       * UBL_Z_RAISE_WHEN_OFF_MESH is specified, that value is returned.
+       */
+      #ifdef UBL_Z_RAISE_WHEN_OFF_MESH
+        if (!WITHIN(rx0, MESH_MIN_X, MESH_MAX_X) || !WITHIN(ry0, MESH_MIN_Y, MESH_MAX_Y))
+          return UBL_Z_RAISE_WHEN_OFF_MESH;
+      #endif
+
+      const float z1 = calc_z0(rx0,
+                               mesh_index_to_xpos(cx), z_values[cx][cy],
+                               mesh_index_to_xpos(cx + 1), z_values[_MIN(cx, GRID_MAX_POINTS_X - 2) + 1][cy]);
+
+      const float z2 = calc_z0(rx0,
+                               mesh_index_to_xpos(cx), z_values[cx][_MIN(cy, GRID_MAX_POINTS_Y - 2) + 1],
+                               mesh_index_to_xpos(cx + 1), z_values[_MIN(cx, GRID_MAX_POINTS_X - 2) + 1][_MIN(cy, GRID_MAX_POINTS_Y - 2) + 1]);
+
+      float z0 = calc_z0(ry0,
+                         mesh_index_to_ypos(cy), z1,
+                         mesh_index_to_ypos(cy + 1), z2);
+
+      if (DEBUGGING(MESH_ADJUST)) {
+        DEBUG_ECHOPAIR(" raw get_z_correction(", rx0);
+        DEBUG_CHAR(','); DEBUG_ECHO(ry0);
+        DEBUG_ECHOPAIR_F(") = ", z0, 6);
+        DEBUG_ECHOLNPAIR_F(" >>>---> ", z0, 6);
+      }
+
+      if (isnan(z0)) { // if part of the Mesh is undefined, it will show up as NAN
+        z0 = 0.0;      // in ubl.z_values[][] and propagate through the
+                       // calculations. If our correction is NAN, we throw it out
+                       // because part of the Mesh is undefined and we don't have the
+                       // information we need to complete the height correction.
+
+        if (DEBUGGING(MESH_ADJUST)) {
+          DEBUG_ECHOPAIR("??? Yikes!  NAN in get_z_correction(", rx0);
+          DEBUG_CHAR(',');
+          DEBUG_ECHO(ry0);
+          DEBUG_CHAR(')');
+          DEBUG_EOL();
+        }
+      }
+      return z0;
+    }
+    static inline float get_z_correction(const xy_pos_t &pos) { return get_z_correction(pos.x, pos.y); }
+
+    static inline float mesh_index_to_xpos(const uint8_t i) {
+      return i < GRID_MAX_POINTS_X ? pgm_read_float(&_mesh_index_to_xpos[i]) : MESH_MIN_X + i * (MESH_X_DIST);
+    }
+    static inline float mesh_index_to_ypos(const uint8_t i) {
+      return i < GRID_MAX_POINTS_Y ? pgm_read_float(&_mesh_index_to_ypos[i]) : MESH_MIN_Y + i * (MESH_Y_DIST);
+    }
+
+    #if UBL_SEGMENTED
+      static bool line_to_destination_segmented(const feedRate_t &scaled_fr_mm_s);
+    #else
+      static void line_to_destination_cartesian(const feedRate_t &scaled_fr_mm_s, const uint8_t e);
+    #endif
+
+    static inline bool mesh_is_valid() {
+      GRID_LOOP(x, y) if (isnan(z_values[x][y])) return false;
+      return true;
+    }
+
+}; // class unified_bed_leveling
+
+extern unified_bed_leveling ubl;
+
+#define _GET_MESH_X(I) ubl.mesh_index_to_xpos(I)
+#define _GET_MESH_Y(J) ubl.mesh_index_to_ypos(J)
+#define Z_VALUES_ARR ubl.z_values
+
+// Prevent debugging propagating to other files
+#include "../../../core/debug_out.h"
diff --git a/Marlin/src/feature/bedlevel/ubl/ubl_G29.cpp b/Marlin/src/feature/bedlevel/ubl/ubl_G29.cpp
new file mode 100644
index 0000000..41d2a36
--- /dev/null
+++ b/Marlin/src/feature/bedlevel/ubl/ubl_G29.cpp
@@ -0,0 +1,1783 @@
+/**
+ * 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 ENABLED(AUTO_BED_LEVELING_UBL)
+
+  #include "../bedlevel.h"
+
+  #include "../../../MarlinCore.h"
+  #include "../../../HAL/shared/eeprom_api.h"
+  #include "../../../libs/hex_print.h"
+  #include "../../../module/settings.h"
+  #include "../../../lcd/marlinui.h"
+  #include "../../../module/stepper.h"
+  #include "../../../module/planner.h"
+  #include "../../../module/motion.h"
+  #include "../../../module/probe.h"
+  #include "../../../gcode/gcode.h"
+  #include "../../../libs/least_squares_fit.h"
+
+  #if HAS_MULTI_HOTEND
+    #include "../../../module/tool_change.h"
+  #endif
+
+  #define DEBUG_OUT ENABLED(DEBUG_LEVELING_FEATURE)
+  #include "../../../core/debug_out.h"
+
+  #if ENABLED(EXTENSIBLE_UI)
+    #include "../../../lcd/extui/ui_api.h"
+  #endif
+
+  #include <math.h>
+
+  #define UBL_G29_P31
+
+  #if HAS_LCD_MENU
+
+    bool unified_bed_leveling::lcd_map_control = false;
+
+    void unified_bed_leveling::steppers_were_disabled() {
+      if (lcd_map_control) {
+        lcd_map_control = false;
+        ui.defer_status_screen(false);
+      }
+    }
+
+    void ubl_map_screen();
+
+  #endif
+
+  #define SIZE_OF_LITTLE_RAISE 1
+  #define BIG_RAISE_NOT_NEEDED 0
+
+  int    unified_bed_leveling::g29_verbose_level,
+         unified_bed_leveling::g29_phase_value,
+         unified_bed_leveling::g29_repetition_cnt,
+         unified_bed_leveling::g29_storage_slot = 0,
+         unified_bed_leveling::g29_map_type;
+  bool   unified_bed_leveling::g29_c_flag;
+  float  unified_bed_leveling::g29_card_thickness = 0,
+         unified_bed_leveling::g29_constant = 0;
+  xy_bool_t unified_bed_leveling::xy_seen;
+  xy_pos_t unified_bed_leveling::g29_pos;
+
+  #if HAS_BED_PROBE
+    int  unified_bed_leveling::g29_grid_size;
+  #endif
+
+  /**
+   *   G29: Unified Bed Leveling by Roxy
+   *
+   *   Parameters understood by this leveling system:
+   *
+   *   A     Activate   Activate the Unified Bed Leveling system.
+   *
+   *   B #   Business   Use the 'Business Card' mode of the Manual Probe subsystem with P2.
+   *                    Note: A non-compressible Spark Gap feeler gauge is recommended over a business card.
+   *                    In this mode of G29 P2, a business or index card is used as a shim that the nozzle can
+   *                    grab onto as it is lowered. In principle, the nozzle-bed distance is the same when the
+   *                    same resistance is felt in the shim. You can omit the numerical value on first invocation
+   *                    of G29 P2 B to measure shim thickness. Subsequent use of 'B' will apply the previously-
+   *                    measured thickness by default.
+   *
+   *   C     Continue   G29 P1 C continues the generation of a partially-constructed Mesh without invalidating
+   *                    previous measurements.
+   *
+   *   C                G29 P2 C tells the Manual Probe subsystem to not use the current nozzle
+   *                    location in its search for the closest unmeasured Mesh Point. Instead, attempt to
+   *                    start at one end of the uprobed points and Continue sequentially.
+   *
+   *                    G29 P3 C specifies the Constant for the fill. Otherwise, uses a "reasonable" value.
+   *
+   *   C     Current    G29 Z C uses the Current location (instead of bed center or nearest edge).
+   *
+   *   D     Disable    Disable the Unified Bed Leveling system.
+   *
+   *   E     Stow_probe Stow the probe after each sampled point.
+   *
+   *   F #   Fade       Fade the amount of Mesh Based Compensation over a specified height. At the
+   *                    specified height, no correction is applied and natural printer kenimatics take over. If no
+   *                    number is specified for the command, 10mm is assumed to be reasonable.
+   *
+   *   H #   Height     With P2, 'H' specifies the Height to raise the nozzle after each manual probe of the bed.
+   *                    If omitted, the nozzle will raise by Z_CLEARANCE_BETWEEN_PROBES.
+   *
+   *   H #   Offset     With P4, 'H' specifies the Offset above the mesh height to place the nozzle.
+   *                    If omitted, Z_CLEARANCE_BETWEEN_PROBES will be used.
+   *
+   *   I #   Invalidate Invalidate the specified number of Mesh Points near the given 'X' 'Y'. If X or Y are omitted,
+   *                    the nozzle location is used. If no 'I' value is given, only the point nearest to the location
+   *                    is invalidated. Use 'T' to produce a map afterward. This command is useful to invalidate a
+   *                    portion of the Mesh so it can be adjusted using other UBL tools. When attempting to invalidate
+   *                    an isolated bad mesh point, the 'T' option shows the nozzle position in the Mesh with (#). You
+   *                    can move the nozzle around and use this feature to select the center of the area (or cell) to
+   *                    invalidate.
+   *
+   *   J #   Grid       Perform a Grid Based Leveling of the current Mesh using a grid with n points on a side.
+   *                    Not specifying a grid size will invoke the 3-Point leveling function.
+   *
+   *   L     Load       Load Mesh from the previously activated location in the EEPROM.
+   *
+   *   L #   Load       Load Mesh from the specified location in the EEPROM. Set this location as activated
+   *                    for subsequent Load and Store operations.
+   *
+   *   The P or Phase commands are used for the bulk of the work to setup a Mesh. In general, your Mesh will
+   *   start off being initialized with a G29 P0 or a G29 P1. Further refinement of the Mesh happens with
+   *   each additional Phase that processes it.
+   *
+   *   P0    Phase 0    Zero Mesh Data and turn off the Mesh Compensation System. This reverts the
+   *                    3D Printer to the same state it was in before the Unified Bed Leveling Compensation
+   *                    was turned on. Setting the entire Mesh to Zero is a special case that allows
+   *                    a subsequent G or T leveling operation for backward compatibility.
+   *
+   *   P1    Phase 1    Invalidate entire Mesh and continue with automatic generation of the Mesh data using
+   *                    the Z-Probe. Usually the probe can't reach all areas that the nozzle can reach. For delta
+   *                    printers only the areas where the probe and nozzle can both reach will be automatically probed.
+   *
+   *                    Unreachable points will be handled in Phase 2 and Phase 3.
+   *
+   *                    Use 'C' to leave the previous mesh intact and automatically probe needed points. This allows you
+   *                    to invalidate parts of the Mesh but still use Automatic Probing.
+   *
+   *                    The 'X' and 'Y' parameters prioritize where to try and measure points. If omitted, the current
+   *                    probe position is used.
+   *
+   *                    Use 'T' (Topology) to generate a report of mesh generation.
+   *
+   *                    P1 will suspend Mesh generation if the controller button is held down. Note that you may need
+   *                    to press and hold the switch for several seconds if moves are underway.
+   *
+   *   P2    Phase 2    Probe unreachable points.
+   *
+   *                    Use 'H' to set the height between Mesh points. If omitted, Z_CLEARANCE_BETWEEN_PROBES is used.
+   *                    Smaller values will be quicker. Move the nozzle down till it barely touches the bed. Make sure the
+   *                    nozzle is clean and unobstructed. Use caution and move slowly. This can damage your printer!
+   *                    (Uses SIZE_OF_LITTLE_RAISE mm if the nozzle is moving less than BIG_RAISE_NOT_NEEDED mm.)
+   *
+   *                    The 'H' value can be negative if the Mesh dips in a large area. Press and hold the
+   *                    controller button to terminate the current Phase 2 command. You can then re-issue "G29 P 2"
+   *                    with an 'H' parameter more suitable for the area you're manually probing. Note that the command
+   *                    tries to start in a corner of the bed where movement will be predictable. Override the distance
+   *                    calculation location with the X and Y parameters. You can print a Mesh Map (G29 T) to see where
+   *                    the mesh is invalidated and where the nozzle needs to move to complete the command. Use 'C' to
+   *                    indicate that the search should be based on the current position.
+   *
+   *                    The 'B' parameter for this command is described above. It places the manual probe subsystem into
+   *                    Business Card mode where the thickness of a business card is measured and then used to accurately
+   *                    set the nozzle height in all manual probing for the duration of the command. A Business card can
+   *                    be used, but you'll get better results with a flexible Shim that doesn't compress. This makes it
+   *                    easier to produce similar amounts of force and get more accurate measurements. Google if you're
+   *                    not sure how to use a shim.
+   *
+   *                    The 'T' (Map) parameter helps track Mesh building progress.
+   *
+   *                    NOTE: P2 requires an LCD controller!
+   *
+   *   P3    Phase 3    Fill the unpopulated regions of the Mesh with a fixed value. There are two different paths to
+   *                    go down:
+   *
+   *                    - If a 'C' constant is specified, the closest invalid mesh points to the nozzle will be filled,
+   *                      and a repeat count can then also be specified with 'R'.
+   *
+   *                    - Leaving out 'C' invokes Smart Fill, which scans the mesh from the edges inward looking for
+   *                      invalid mesh points. Adjacent points are used to determine the bed slope. If the bed is sloped
+   *                      upward from the invalid point, it takes the value of the nearest point. If sloped downward, it's
+   *                      replaced by a value that puts all three points in a line. This version of G29 P3 is a quick, easy
+   *                      and (usually) safe way to populate unprobed mesh regions before continuing to G26 Mesh Validation
+   *                      Pattern. Note that this populates the mesh with unverified values. Pay attention and use caution.
+   *
+   *   P4    Phase 4    Fine tune the Mesh. The Delta Mesh Compensation System assumes the existence of
+   *                    an LCD Panel. It is possible to fine tune the mesh without an LCD Panel using
+   *                    G42 and M421. See the UBL documentation for further details.
+   *
+   *                    Phase 4 is meant to be used with G26 Mesh Validation to fine tune the mesh by direct editing
+   *                    of Mesh Points. Raise and lower points to fine tune the mesh until it gives consistently reliable
+   *                    adhesion.
+   *
+   *                    P4 moves to the closest Mesh Point (and/or the given X Y), raises the nozzle above the mesh height
+   *                    by the given 'H' offset (or default 0), and waits while the controller is used to adjust the nozzle
+   *                    height. On click the displayed height is saved in the mesh.
+   *
+   *                    Start Phase 4 at a specific location with X and Y. Adjust a specific number of Mesh Points with
+   *                    the 'R' (Repeat) parameter. (If 'R' is left out, the whole matrix is assumed.) This command can be
+   *                    terminated early (e.g., after editing the area of interest) by pressing and holding the encoder button.
+   *
+   *                    The general form is G29 P4 [R points] [X position] [Y position]
+   *
+   *                    The H [offset] parameter is useful if a shim is used to fine-tune the mesh. For a 0.4mm shim the
+   *                    command would be G29 P4 H0.4. The nozzle is moved to the shim height, you adjust height to the shim,
+   *                    and on click the height minus the shim thickness will be saved in the mesh.
+   *
+   *                    !!Use with caution, as a very poor mesh could cause the nozzle to crash into the bed!!
+   *
+   *                    NOTE:  P4 is not available unless you have LCD support enabled!
+   *
+   *   P5    Phase 5    Find Mean Mesh Height and Standard Deviation. Typically, it is easier to use and
+   *                    work with the Mesh if it is Mean Adjusted. You can specify a C parameter to
+   *                    Correct the Mesh to a 0.00 Mean Height. Adding a C parameter will automatically
+   *                    execute a G29 P6 C <mean height>.
+   *
+   *   P6    Phase 6    Shift Mesh height. The entire Mesh's height is adjusted by the height specified
+   *                    with the C parameter. Being able to adjust the height of a Mesh is useful tool. It
+   *                    can be used to compensate for poorly calibrated Z-Probes and other errors. Ideally,
+   *                    you should have the Mesh adjusted for a Mean Height of 0.00 and the Z-Probe measuring
+   *                    0.000 at the Z Home location.
+   *
+   *   Q     Test       Load specified Test Pattern to assist in checking correct operation of system. This
+   *                    command is not anticipated to be of much value to the typical user. It is intended
+   *                    for developers to help them verify correct operation of the Unified Bed Leveling System.
+   *
+   *   R #   Repeat     Repeat this command the specified number of times. If no number is specified the
+   *                    command will be repeated GRID_MAX_POINTS_X * GRID_MAX_POINTS_Y times.
+   *
+   *   S     Store      Store the current Mesh in the Activated area of the EEPROM. It will also store the
+   *                    current state of the Unified Bed Leveling system in the EEPROM.
+   *
+   *   S #   Store      Store the current Mesh at the specified location in EEPROM. Activate this location
+   *                    for subsequent Load and Store operations. Valid storage slot numbers begin at 0 and
+   *                    extend to a limit related to the available EEPROM storage.
+   *
+   *   S -1  Store      Print the current Mesh as G-code that can be used to restore the mesh anytime.
+   *
+   *   T     Topology   Display the Mesh Map Topology.
+   *                    'T' can be used alone (e.g., G29 T) or in combination with most of the other commands.
+   *                    This option works with all Phase commands (e.g., G29 P4 R 5 T X 50 Y100 C -.1 O)
+   *                    This parameter can also specify a Map Type. T0 (the default) is user-readable. T1
+   *                    is suitable to paste into a spreadsheet for a 3D graph of the mesh.
+   *
+   *   U     Unlevel    Perform a probe of the outer perimeter to assist in physically leveling unlevel beds.
+   *                    Only used for G29 P1 T U. This speeds up the probing of the edge of the bed. Useful
+   *                    when the entire bed doesn't need to be probed because it will be adjusted.
+   *
+   *   V #   Verbosity  Set the verbosity level (0-4) for extra details. (Default 0)
+   *
+   *   X #              X Location for this command
+   *
+   *   Y #              Y Location for this command
+   *
+   * With UBL_DEVEL_DEBUGGING:
+   *
+   *   K #  Kompare     Kompare current Mesh with stored Mesh #, replacing current Mesh with the result.
+   *                    This command literally performs a diff between two Meshes.
+   *
+   *   Q-1  Dump EEPROM Dump the UBL contents stored in EEPROM as HEX format. Useful for developers to help
+   *                    verify correct operation of the UBL.
+   *
+   *   W    What?       Display valuable UBL data.
+   *
+   *
+   *   Release Notes:
+   *   You MUST do M502, M500 to initialize the storage. Failure to do this will cause all
+   *   kinds of problems. Enabling EEPROM Storage is required.
+   *
+   *   When you do a G28 and G29 P1 to automatically build your first mesh, you are going to notice that
+   *   UBL probes points increasingly further from the starting location. (The starting location defaults
+   *   to the center of the bed.) In contrast, ABL and MBL follow a zigzag pattern. The spiral pattern is
+   *   especially better for Delta printers, since it populates the center of the mesh first, allowing for
+   *   a quicker test print to verify settings. You don't need to populate the entire mesh to use it.
+   *   After all, you don't want to spend a lot of time generating a mesh only to realize the resolution
+   *   or probe offsets are incorrect. Mesh-generation gathers points starting closest to the nozzle unless
+   *   an (X,Y) coordinate pair is given.
+   *
+   *   Unified Bed Leveling uses a lot of EEPROM storage to hold its data, and it takes some effort to get
+   *   the mesh just right. To prevent this valuable data from being destroyed as the EEPROM structure
+   *   evolves, UBL stores all mesh data at the end of EEPROM.
+   *
+   *   UBL is founded on Edward Patel's Mesh Bed Leveling code. A big 'Thanks!' to him and the creators of
+   *   3-Point and Grid Based leveling. Combining their contributions we now have the functionality and
+   *   features of all three systems combined.
+   */
+
+  void unified_bed_leveling::G29() {
+
+    bool probe_deployed = false;
+    if (g29_parameter_parsing()) return; // Abort on parameter error
+
+    const int8_t p_val = parser.intval('P', -1);
+    const bool may_move = p_val == 1 || p_val == 2 || p_val == 4 || parser.seen('J');
+    TERN_(HAS_MULTI_HOTEND, const uint8_t old_tool_index = active_extruder);
+
+    // Check for commands that require the printer to be homed
+    if (may_move) {
+      planner.synchronize();
+      // Send 'N' to force homing before G29 (internal only)
+      if (axes_should_home() || parser.seen('N')) gcode.home_all_axes();
+      TERN_(HAS_MULTI_HOTEND, if (active_extruder) tool_change(0));
+    }
+
+    // Invalidate Mesh Points. This command is a little bit asymmetrical because
+    // it directly specifies the repetition count and does not use the 'R' parameter.
+    if (parser.seen('I')) {
+      uint8_t cnt = 0;
+      g29_repetition_cnt = parser.has_value() ? parser.value_int() : 1;
+      if (g29_repetition_cnt >= GRID_MAX_POINTS) {
+        set_all_mesh_points_to_value(NAN);
+      }
+      else {
+        while (g29_repetition_cnt--) {
+          if (cnt > 20) { cnt = 0; idle(); }
+          const mesh_index_pair closest = find_closest_mesh_point_of_type(REAL, g29_pos);
+          const xy_int8_t &cpos = closest.pos;
+          if (cpos.x < 0) {
+            // No more REAL mesh points to invalidate, so we ASSUME the user
+            // meant to invalidate the ENTIRE mesh, which cannot be done with
+            // find_closest_mesh_point loop which only returns REAL points.
+            set_all_mesh_points_to_value(NAN);
+            SERIAL_ECHOLNPGM("Entire Mesh invalidated.\n");
+            break;            // No more invalid Mesh Points to populate
+          }
+          z_values[cpos.x][cpos.y] = NAN;
+          TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(cpos, 0.0f));
+          cnt++;
+        }
+      }
+      SERIAL_ECHOLNPGM("Locations invalidated.\n");
+    }
+
+    if (parser.seen('Q')) {
+      const int test_pattern = parser.has_value() ? parser.value_int() : -99;
+      if (!WITHIN(test_pattern, -1, 2)) {
+        SERIAL_ECHOLNPGM("Invalid test_pattern value. (-1 to 2)\n");
+        return;
+      }
+      SERIAL_ECHOLNPGM("Loading test_pattern values.\n");
+      switch (test_pattern) {
+
+        #if ENABLED(UBL_DEVEL_DEBUGGING)
+          case -1:
+            g29_eeprom_dump();
+            break;
+        #endif
+
+        case 0:
+          GRID_LOOP(x, y) {                                     // Create a bowl shape similar to a poorly-calibrated Delta
+            const float p1 = 0.5f * (GRID_MAX_POINTS_X) - x,
+                        p2 = 0.5f * (GRID_MAX_POINTS_Y) - y;
+            z_values[x][y] += 2.0f * HYPOT(p1, p2);
+            TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(x, y, z_values[x][y]));
+          }
+          break;
+
+        case 1:
+          LOOP_L_N(x, GRID_MAX_POINTS_X) {                     // Create a diagonal line several Mesh cells thick that is raised
+            z_values[x][x] += 9.999f;
+            z_values[x][x + (x < (GRID_MAX_POINTS_Y) - 1) ? 1 : -1] += 9.999f; // We want the altered line several mesh points thick
+            #if ENABLED(EXTENSIBLE_UI)
+              ExtUI::onMeshUpdate(x, x, z_values[x][x]);
+              ExtUI::onMeshUpdate(x, (x + (x < (GRID_MAX_POINTS_Y) - 1) ? 1 : -1), z_values[x][x + (x < (GRID_MAX_POINTS_Y) - 1) ? 1 : -1]);
+            #endif
+
+          }
+          break;
+
+        case 2:
+          // Allow the user to specify the height because 10mm is a little extreme in some cases.
+          for (uint8_t x = (GRID_MAX_POINTS_X) / 3; x < 2 * (GRID_MAX_POINTS_X) / 3; x++)     // Create a rectangular raised area in
+            for (uint8_t y = (GRID_MAX_POINTS_Y) / 3; y < 2 * (GRID_MAX_POINTS_Y) / 3; y++) { // the center of the bed
+              z_values[x][y] += parser.seen('C') ? g29_constant : 9.99f;
+              TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(x, y, z_values[x][y]));
+            }
+          break;
+      }
+    }
+
+    #if HAS_BED_PROBE
+
+      if (parser.seen('J')) {
+        save_ubl_active_state_and_disable();
+        tilt_mesh_based_on_probed_grid(g29_grid_size == 0); // Zero size does 3-Point
+        restore_ubl_active_state_and_leave();
+        #if ENABLED(UBL_G29_J_RECENTER)
+          do_blocking_move_to_xy(0.5f * ((MESH_MIN_X) + (MESH_MAX_X)), 0.5f * ((MESH_MIN_Y) + (MESH_MAX_Y)));
+        #endif
+        report_current_position();
+        probe_deployed = true;
+      }
+
+    #endif // HAS_BED_PROBE
+
+    if (parser.seen('P')) {
+      if (WITHIN(g29_phase_value, 0, 1) && storage_slot == -1) {
+        storage_slot = 0;
+        SERIAL_ECHOLNPGM("Default storage slot 0 selected.");
+      }
+
+      switch (g29_phase_value) {
+        case 0:
+          //
+          // Zero Mesh Data
+          //
+          reset();
+          SERIAL_ECHOLNPGM("Mesh zeroed.");
+          break;
+
+        #if HAS_BED_PROBE
+
+          case 1: {
+            //
+            // Invalidate Entire Mesh and Automatically Probe Mesh in areas that can be reached by the probe
+            //
+            if (!parser.seen('C')) {
+              invalidate();
+              SERIAL_ECHOLNPGM("Mesh invalidated. Probing mesh.");
+            }
+            if (g29_verbose_level > 1) {
+              SERIAL_ECHOPAIR("Probing around (", g29_pos.x);
+              SERIAL_CHAR(',');
+              SERIAL_DECIMAL(g29_pos.y);
+              SERIAL_ECHOLNPGM(").\n");
+            }
+            const xy_pos_t near_probe_xy = g29_pos + probe.offset_xy;
+            probe_entire_mesh(near_probe_xy, parser.seen('T'), parser.seen('E'), parser.seen('U'));
+
+            report_current_position();
+            probe_deployed = true;
+          } break;
+
+        #endif // HAS_BED_PROBE
+
+        case 2: {
+          #if HAS_LCD_MENU
+            //
+            // Manually Probe Mesh in areas that can't be reached by the probe
+            //
+            SERIAL_ECHOLNPGM("Manually probing unreachable points.");
+            do_z_clearance(Z_CLEARANCE_BETWEEN_PROBES);
+
+            if (parser.seen('C') && !xy_seen) {
+
+              /**
+               * Use a good default location for the path.
+               * The flipped > and < operators in these comparisons is intentional.
+               * It should cause the probed points to follow a nice path on Cartesian printers.
+               * It may make sense to have Delta printers default to the center of the bed.
+               * Until that is decided, this can be forced with the X and Y parameters.
+               */
+              g29_pos.set(
+                #if IS_KINEMATIC
+                  X_HOME_POS, Y_HOME_POS
+                #else
+                  probe.offset_xy.x > 0 ? X_BED_SIZE : 0,
+                  probe.offset_xy.y < 0 ? Y_BED_SIZE : 0
+                #endif
+              );
+            }
+
+            if (parser.seen('B')) {
+              g29_card_thickness = parser.has_value() ? parser.value_float() : measure_business_card_thickness();
+              if (ABS(g29_card_thickness) > 1.5f) {
+                SERIAL_ECHOLNPGM("?Error in Business Card measurement.");
+                return;
+              }
+              probe_deployed = true;
+            }
+
+            if (!position_is_reachable(g29_pos)) {
+              SERIAL_ECHOLNPGM("XY outside printable radius.");
+              return;
+            }
+
+            const float height = parser.floatval('H', Z_CLEARANCE_BETWEEN_PROBES);
+            manually_probe_remaining_mesh(g29_pos, height, g29_card_thickness, parser.seen('T'));
+
+            SERIAL_ECHOLNPGM("G29 P2 finished.");
+
+            report_current_position();
+
+          #else
+
+            SERIAL_ECHOLNPGM("?P2 is only available when an LCD is present.");
+            return;
+
+          #endif
+        } break;
+
+        case 3: {
+          /**
+           * Populate invalid mesh areas. Proceed with caution.
+           * Two choices are available:
+           *   - Specify a constant with the 'C' parameter.
+           *   - Allow 'G29 P3' to choose a 'reasonable' constant.
+           */
+
+          if (g29_c_flag) {
+            if (g29_repetition_cnt >= GRID_MAX_POINTS) {
+              set_all_mesh_points_to_value(g29_constant);
+            }
+            else {
+              while (g29_repetition_cnt--) {  // this only populates reachable mesh points near
+                const mesh_index_pair closest = find_closest_mesh_point_of_type(INVALID, g29_pos);
+                const xy_int8_t &cpos = closest.pos;
+                if (cpos.x < 0) {
+                  // No more REAL INVALID mesh points to populate, so we ASSUME
+                  // user meant to populate ALL INVALID mesh points to value
+                  GRID_LOOP(x, y) if (isnan(z_values[x][y])) z_values[x][y] = g29_constant;
+                  break; // No more invalid Mesh Points to populate
+                }
+                else {
+                  z_values[cpos.x][cpos.y] = g29_constant;
+                  TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(cpos, g29_constant));
+                }
+              }
+            }
+          }
+          else {
+            const float cvf = parser.value_float();
+            switch ((int)TRUNC(cvf * 10.0f) - 30) {   // 3.1 -> 1
+              #if ENABLED(UBL_G29_P31)
+                case 1: {
+
+                  // P3.1  use least squares fit to fill missing mesh values
+                  // P3.10 zero weighting for distance, all grid points equal, best fit tilted plane
+                  // P3.11 10X weighting for nearest grid points versus farthest grid points
+                  // P3.12 100X distance weighting
+                  // P3.13 1000X distance weighting, approaches simple average of nearest points
+
+                  const float weight_power  = (cvf - 3.10f) * 100.0f,  // 3.12345 -> 2.345
+                              weight_factor = weight_power ? POW(10.0f, weight_power) : 0;
+                  smart_fill_wlsf(weight_factor);
+                }
+                break;
+              #endif
+              case 0:   // P3 or P3.0
+              default:  // and anything P3.x that's not P3.1
+                smart_fill_mesh();  // Do a 'Smart' fill using nearby known values
+                break;
+            }
+          }
+          break;
+        }
+
+        case 4: // Fine Tune (i.e., Edit) the Mesh
+          #if HAS_LCD_MENU
+            fine_tune_mesh(g29_pos, parser.seen('T'));
+          #else
+            SERIAL_ECHOLNPGM("?P4 is only available when an LCD is present.");
+            return;
+          #endif
+          break;
+
+        case 5: adjust_mesh_to_mean(g29_c_flag, g29_constant); break;
+
+        case 6: shift_mesh_height(); break;
+      }
+    }
+
+    #if ENABLED(UBL_DEVEL_DEBUGGING)
+
+      //
+      // Much of the 'What?' command can be eliminated. But until we are fully debugged, it is
+      // good to have the extra information. Soon... we prune this to just a few items
+      //
+      if (parser.seen('W')) g29_what_command();
+
+      //
+      // When we are fully debugged, this may go away. But there are some valid
+      // use cases for the users. So we can wait and see what to do with it.
+      //
+
+      if (parser.seen('K')) // Kompare Current Mesh Data to Specified Stored Mesh
+        g29_compare_current_mesh_to_stored_mesh();
+
+    #endif // UBL_DEVEL_DEBUGGING
+
+
+    //
+    // Load a Mesh from the EEPROM
+    //
+
+    if (parser.seen('L')) {     // Load Current Mesh Data
+      g29_storage_slot = parser.has_value() ? parser.value_int() : storage_slot;
+
+      int16_t a = settings.calc_num_meshes();
+
+      if (!a) {
+        SERIAL_ECHOLNPGM("?EEPROM storage not available.");
+        return;
+      }
+
+      if (!WITHIN(g29_storage_slot, 0, a - 1)) {
+        SERIAL_ECHOLNPAIR("?Invalid storage slot.\n?Use 0 to ", a - 1);
+        return;
+      }
+
+      settings.load_mesh(g29_storage_slot);
+      storage_slot = g29_storage_slot;
+
+      SERIAL_ECHOLNPGM("Done.");
+    }
+
+    //
+    // Store a Mesh in the EEPROM
+    //
+
+    if (parser.seen('S')) {     // Store (or Save) Current Mesh Data
+      g29_storage_slot = parser.has_value() ? parser.value_int() : storage_slot;
+
+      if (g29_storage_slot == -1)                     // Special case, the user wants to 'Export' the mesh to the
+        return report_current_mesh();                 // host program to be saved on the user's computer
+
+      int16_t a = settings.calc_num_meshes();
+
+      if (!a) {
+        SERIAL_ECHOLNPGM("?EEPROM storage not available.");
+        goto LEAVE;
+      }
+
+      if (!WITHIN(g29_storage_slot, 0, a - 1)) {
+        SERIAL_ECHOLNPAIR("?Invalid storage slot.\n?Use 0 to ", a - 1);
+        goto LEAVE;
+      }
+
+      settings.store_mesh(g29_storage_slot);
+      storage_slot = g29_storage_slot;
+
+      SERIAL_ECHOLNPGM("Done.");
+    }
+
+    if (parser.seen('T'))
+      display_map(g29_map_type);
+
+    LEAVE:
+
+    #if HAS_LCD_MENU
+      ui.reset_alert_level();
+      ui.quick_feedback();
+      ui.reset_status();
+      ui.release();
+    #endif
+
+    #ifdef Z_PROBE_END_SCRIPT
+      if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("Z Probe End Script: ", Z_PROBE_END_SCRIPT);
+      if (probe_deployed) {
+        planner.synchronize();
+        gcode.process_subcommands_now_P(PSTR(Z_PROBE_END_SCRIPT));
+      }
+    #else
+      UNUSED(probe_deployed);
+    #endif
+
+    TERN_(HAS_MULTI_HOTEND, tool_change(old_tool_index));
+    return;
+  }
+
+  void unified_bed_leveling::adjust_mesh_to_mean(const bool cflag, const float value) {
+    float sum = 0;
+    int n = 0;
+    GRID_LOOP(x, y)
+      if (!isnan(z_values[x][y])) {
+        sum += z_values[x][y];
+        n++;
+      }
+
+    const float mean = sum / n;
+
+    //
+    // Sum the squares of difference from mean
+    //
+    float sum_of_diff_squared = 0;
+    GRID_LOOP(x, y)
+      if (!isnan(z_values[x][y]))
+        sum_of_diff_squared += sq(z_values[x][y] - mean);
+
+    SERIAL_ECHOLNPAIR("# of samples: ", n);
+    SERIAL_ECHOLNPAIR_F("Mean Mesh Height: ", mean, 6);
+
+    const float sigma = SQRT(sum_of_diff_squared / (n + 1));
+    SERIAL_ECHOLNPAIR_F("Standard Deviation: ", sigma, 6);
+
+    if (cflag)
+      GRID_LOOP(x, y)
+        if (!isnan(z_values[x][y])) {
+          z_values[x][y] -= mean + value;
+          TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(x, y, z_values[x][y]));
+        }
+  }
+
+  void unified_bed_leveling::shift_mesh_height() {
+    GRID_LOOP(x, y)
+      if (!isnan(z_values[x][y])) {
+        z_values[x][y] += g29_constant;
+        TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(x, y, z_values[x][y]));
+      }
+  }
+
+  #if HAS_BED_PROBE
+    /**
+     * Probe all invalidated locations of the mesh that can be reached by the probe.
+     * This attempts to fill in locations closest to the nozzle's start location first.
+     */
+    void unified_bed_leveling::probe_entire_mesh(const xy_pos_t &nearby, const bool do_ubl_mesh_map, const bool stow_probe, const bool do_furthest) {
+      probe.deploy(); // Deploy before ui.capture() to allow for PAUSE_BEFORE_DEPLOY_STOW
+
+      TERN_(HAS_LCD_MENU, ui.capture());
+
+      save_ubl_active_state_and_disable();  // No bed level correction so only raw data is obtained
+      uint8_t count = GRID_MAX_POINTS;
+
+      mesh_index_pair best;
+      TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(best.pos, ExtUI::MESH_START));
+      do {
+        if (do_ubl_mesh_map) display_map(g29_map_type);
+
+        const int point_num = (GRID_MAX_POINTS) - count + 1;
+        SERIAL_ECHOLNPAIR("Probing mesh point ", point_num, "/", int(GRID_MAX_POINTS), ".");
+        TERN_(HAS_DISPLAY, ui.status_printf_P(0, PSTR(S_FMT " %i/%i"), GET_TEXT(MSG_PROBING_MESH), point_num, int(GRID_MAX_POINTS)));
+
+        #if HAS_LCD_MENU
+          if (ui.button_pressed()) {
+            ui.quick_feedback(false); // Preserve button state for click-and-hold
+            SERIAL_ECHOLNPGM("\nMesh only partially populated.\n");
+            ui.wait_for_release();
+            ui.quick_feedback();
+            ui.release();
+            probe.stow(); // Release UI before stow to allow for PAUSE_BEFORE_DEPLOY_STOW
+            return restore_ubl_active_state_and_leave();
+          }
+        #endif
+
+        best = do_furthest
+          ? find_furthest_invalid_mesh_point()
+          : find_closest_mesh_point_of_type(INVALID, nearby, true);
+
+        if (best.pos.x >= 0) {    // mesh point found and is reachable by probe
+          TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(best.pos, ExtUI::PROBE_START));
+          const float measured_z = probe.probe_at_point(
+                        best.meshpos(),
+                        stow_probe ? PROBE_PT_STOW : PROBE_PT_RAISE, g29_verbose_level
+                      );
+          z_values[best.pos.x][best.pos.y] = measured_z;
+          #if ENABLED(EXTENSIBLE_UI)
+            ExtUI::onMeshUpdate(best.pos, ExtUI::PROBE_FINISH);
+            ExtUI::onMeshUpdate(best.pos, measured_z);
+          #endif
+        }
+        SERIAL_FLUSH(); // Prevent host M105 buffer overrun.
+
+      } while (best.pos.x >= 0 && --count);
+
+      TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(best.pos, ExtUI::MESH_FINISH));
+
+      // Release UI during stow to allow for PAUSE_BEFORE_DEPLOY_STOW
+      TERN_(HAS_LCD_MENU, ui.release());
+      probe.stow();
+      TERN_(HAS_LCD_MENU, ui.capture());
+
+      probe.move_z_after_probing();
+
+      restore_ubl_active_state_and_leave();
+
+      do_blocking_move_to_xy(
+        constrain(nearby.x - probe.offset_xy.x, MESH_MIN_X, MESH_MAX_X),
+        constrain(nearby.y - probe.offset_xy.y, MESH_MIN_Y, MESH_MAX_Y)
+      );
+    }
+
+  #endif // HAS_BED_PROBE
+
+  #if HAS_LCD_MENU
+
+    typedef void (*clickFunc_t)();
+
+    bool click_and_hold(const clickFunc_t func=nullptr) {
+      if (ui.button_pressed()) {
+        ui.quick_feedback(false);         // Preserve button state for click-and-hold
+        const millis_t nxt = millis() + 1500UL;
+        while (ui.button_pressed()) {     // Loop while the encoder is pressed. Uses hardware flag!
+          idle();                         // idle, of course
+          if (ELAPSED(millis(), nxt)) {   // After 1.5 seconds
+            ui.quick_feedback();
+            if (func) (*func)();
+            ui.wait_for_release();
+            return true;
+          }
+        }
+      }
+      serial_delay(15);
+      return false;
+    }
+
+    void unified_bed_leveling::move_z_with_encoder(const float &multiplier) {
+      ui.wait_for_release();
+      while (!ui.button_pressed()) {
+        idle();
+        gcode.reset_stepper_timeout(); // Keep steppers powered
+        if (encoder_diff) {
+          do_blocking_move_to_z(current_position.z + float(encoder_diff) * multiplier);
+          encoder_diff = 0;
+        }
+      }
+    }
+
+    float unified_bed_leveling::measure_point_with_encoder() {
+      KEEPALIVE_STATE(PAUSED_FOR_USER);
+      move_z_with_encoder(0.01f);
+      return current_position.z;
+    }
+
+    static void echo_and_take_a_measurement() { SERIAL_ECHOLNPGM(" and take a measurement."); }
+
+    float unified_bed_leveling::measure_business_card_thickness() {
+      ui.capture();
+      save_ubl_active_state_and_disable();   // Disable bed level correction for probing
+
+      do_blocking_move_to(0.5f * (MESH_MAX_X - (MESH_MIN_X)), 0.5f * (MESH_MAX_Y - (MESH_MIN_Y)), MANUAL_PROBE_START_Z);
+        //, _MIN(planner.settings.max_feedrate_mm_s[X_AXIS], planner.settings.max_feedrate_mm_s[Y_AXIS]) * 0.5f);
+      planner.synchronize();
+
+      SERIAL_ECHOPGM("Place shim under nozzle");
+      LCD_MESSAGEPGM(MSG_UBL_BC_INSERT);
+      ui.return_to_status();
+      echo_and_take_a_measurement();
+
+      const float z1 = measure_point_with_encoder();
+      do_blocking_move_to_z(current_position.z + SIZE_OF_LITTLE_RAISE);
+      planner.synchronize();
+
+      SERIAL_ECHOPGM("Remove shim");
+      LCD_MESSAGEPGM(MSG_UBL_BC_REMOVE);
+      echo_and_take_a_measurement();
+
+      const float z2 = measure_point_with_encoder();
+      do_blocking_move_to_z(current_position.z + Z_CLEARANCE_BETWEEN_PROBES);
+
+      const float thickness = ABS(z1 - z2);
+
+      if (g29_verbose_level > 1) {
+        SERIAL_ECHOPAIR_F("Business Card is ", thickness, 4);
+        SERIAL_ECHOLNPGM("mm thick.");
+      }
+
+      restore_ubl_active_state_and_leave();
+
+      return thickness;
+    }
+
+    void unified_bed_leveling::manually_probe_remaining_mesh(const xy_pos_t &pos, const float &z_clearance, const float &thick, const bool do_ubl_mesh_map) {
+      ui.capture();
+
+      save_ubl_active_state_and_disable();  // No bed level correction so only raw data is obtained
+      do_blocking_move_to_xy_z(current_position, z_clearance);
+
+      ui.return_to_status();
+
+      mesh_index_pair location;
+      const xy_int8_t &lpos = location.pos;
+      do {
+        location = find_closest_mesh_point_of_type(INVALID, pos);
+        // It doesn't matter if the probe can't reach the NAN location. This is a manual probe.
+        if (!location.valid()) continue;
+
+        const xyz_pos_t ppos = {
+          mesh_index_to_xpos(lpos.x),
+          mesh_index_to_ypos(lpos.y),
+          Z_CLEARANCE_BETWEEN_PROBES
+        };
+
+        if (!position_is_reachable(ppos)) break; // SHOULD NOT OCCUR (find_closest_mesh_point only returns reachable points)
+
+        LCD_MESSAGEPGM(MSG_UBL_MOVING_TO_NEXT);
+
+        do_blocking_move_to(ppos);
+        do_z_clearance(z_clearance);
+
+        KEEPALIVE_STATE(PAUSED_FOR_USER);
+        ui.capture();
+
+        if (do_ubl_mesh_map) display_map(g29_map_type);  // show user where we're probing
+
+        serialprintPGM(parser.seen('B') ? GET_TEXT(MSG_UBL_BC_INSERT) : GET_TEXT(MSG_UBL_BC_INSERT2));
+
+        const float z_step = 0.01f;                         // existing behavior: 0.01mm per click, occasionally step
+        //const float z_step = planner.steps_to_mm[Z_AXIS]; // approx one step each click
+
+        move_z_with_encoder(z_step);
+
+        if (click_and_hold()) {
+          SERIAL_ECHOLNPGM("\nMesh only partially populated.");
+          do_z_clearance(Z_CLEARANCE_DEPLOY_PROBE);
+          return restore_ubl_active_state_and_leave();
+        }
+
+        z_values[lpos.x][lpos.y] = current_position.z - thick;
+        TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(location, z_values[lpos.x][lpos.y]));
+
+        if (g29_verbose_level > 2)
+          SERIAL_ECHOLNPAIR_F("Mesh Point Measured at: ", z_values[lpos.x][lpos.y], 6);
+        SERIAL_FLUSH(); // Prevent host M105 buffer overrun.
+      } while (location.valid());
+
+      if (do_ubl_mesh_map) display_map(g29_map_type);  // show user where we're probing
+
+      restore_ubl_active_state_and_leave();
+      do_blocking_move_to_xy_z(pos, Z_CLEARANCE_DEPLOY_PROBE);
+    }
+
+    inline void set_message_with_feedback(PGM_P const msg_P) {
+      ui.set_status_P(msg_P);
+      ui.quick_feedback();
+    }
+
+    void abort_fine_tune() {
+      ui.return_to_status();
+      do_z_clearance(Z_CLEARANCE_BETWEEN_PROBES);
+      set_message_with_feedback(GET_TEXT(MSG_EDITING_STOPPED));
+    }
+
+    void unified_bed_leveling::fine_tune_mesh(const xy_pos_t &pos, const bool do_ubl_mesh_map) {
+      if (!parser.seen('R'))      // fine_tune_mesh() is special. If no repetition count flag is specified
+        g29_repetition_cnt = 1;   // do exactly one mesh location. Otherwise use what the parser decided.
+
+      #if ENABLED(UBL_MESH_EDIT_MOVES_Z)
+        const float h_offset = parser.seenval('H') ? parser.value_linear_units() : MANUAL_PROBE_START_Z;
+        if (!WITHIN(h_offset, 0, 10)) {
+          SERIAL_ECHOLNPGM("Offset out of bounds. (0 to 10mm)\n");
+          return;
+        }
+      #endif
+
+      mesh_index_pair location;
+
+      if (!position_is_reachable(pos)) {
+        SERIAL_ECHOLNPGM("(X,Y) outside printable radius.");
+        return;
+      }
+
+      save_ubl_active_state_and_disable();
+
+      LCD_MESSAGEPGM(MSG_UBL_FINE_TUNE_MESH);
+      ui.capture();                                               // Take over control of the LCD encoder
+
+      do_blocking_move_to_xy_z(pos, Z_CLEARANCE_BETWEEN_PROBES);  // Move to the given XY with probe clearance
+
+      MeshFlags done_flags{0};
+      const xy_int8_t &lpos = location.pos;
+
+      #if IS_TFTGLCD_PANEL
+        lcd_mesh_edit_setup(0);                             // Change current screen before calling ui.ubl_plot
+        safe_delay(50);
+      #endif
+
+      do {
+        location = find_closest_mesh_point_of_type(SET_IN_BITMAP, pos, false, &done_flags);
+
+        if (lpos.x < 0) break;                              // Stop when there are no more reachable points
+
+        done_flags.mark(lpos);                              // Mark this location as 'adjusted' so a new
+                                                            // location is used on the next loop
+        const xyz_pos_t raw = {
+          mesh_index_to_xpos(lpos.x),
+          mesh_index_to_ypos(lpos.y),
+          Z_CLEARANCE_BETWEEN_PROBES
+        };
+
+        if (!position_is_reachable(raw)) break;             // SHOULD NOT OCCUR (find_closest_mesh_point_of_type only returns reachable)
+
+        do_blocking_move_to(raw);                           // Move the nozzle to the edit point with probe clearance
+
+        TERN_(UBL_MESH_EDIT_MOVES_Z, do_blocking_move_to_z(h_offset)); // Move Z to the given 'H' offset before editing
+
+        KEEPALIVE_STATE(PAUSED_FOR_USER);
+
+        if (do_ubl_mesh_map) display_map(g29_map_type);     // Display the current point
+
+        #if IS_TFTGLCD_PANEL
+          ui.ubl_plot(lpos.x, lpos.y);   // update plot screen
+        #endif
+
+        ui.refresh();
+
+        float new_z = z_values[lpos.x][lpos.y];
+        if (isnan(new_z)) new_z = 0;                        // Invalid points begin at 0
+        new_z = FLOOR(new_z * 1000) * 0.001f;               // Chop off digits after the 1000ths place
+
+        lcd_mesh_edit_setup(new_z);
+
+        SET_SOFT_ENDSTOP_LOOSE(true);
+
+        do {
+          idle();
+          new_z = lcd_mesh_edit();
+          TERN_(UBL_MESH_EDIT_MOVES_Z, do_blocking_move_to_z(h_offset + new_z)); // Move the nozzle as the point is edited
+          SERIAL_FLUSH();                                   // Prevent host M105 buffer overrun.
+        } while (!ui.button_pressed());
+
+        SET_SOFT_ENDSTOP_LOOSE(false);
+
+        if (!lcd_map_control) ui.return_to_status();        // Just editing a single point? Return to status
+
+        if (click_and_hold(abort_fine_tune)) break;         // Button held down? Abort editing
+
+        z_values[lpos.x][lpos.y] = new_z;                   // Save the updated Z value
+        TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(location, new_z));
+
+        serial_delay(20);                                   // No switch noise
+        ui.refresh();
+
+      } while (lpos.x >= 0 && --g29_repetition_cnt > 0);
+
+      if (do_ubl_mesh_map) display_map(g29_map_type);
+      restore_ubl_active_state_and_leave();
+
+      do_blocking_move_to_xy_z(pos, Z_CLEARANCE_BETWEEN_PROBES);
+
+      LCD_MESSAGEPGM(MSG_UBL_DONE_EDITING_MESH);
+      SERIAL_ECHOLNPGM("Done Editing Mesh");
+
+      if (lcd_map_control)
+        ui.goto_screen(ubl_map_screen);
+      else
+        ui.return_to_status();
+    }
+
+  #endif // HAS_LCD_MENU
+
+  bool unified_bed_leveling::g29_parameter_parsing() {
+    bool err_flag = false;
+
+    TERN_(HAS_LCD_MENU, set_message_with_feedback(GET_TEXT(MSG_UBL_DOING_G29)));
+
+    g29_constant = 0;
+    g29_repetition_cnt = 0;
+
+    if (parser.seen('R')) {
+      g29_repetition_cnt = parser.has_value() ? parser.value_int() : GRID_MAX_POINTS;
+      NOMORE(g29_repetition_cnt, GRID_MAX_POINTS);
+      if (g29_repetition_cnt < 1) {
+        SERIAL_ECHOLNPGM("?(R)epetition count invalid (1+).\n");
+        return UBL_ERR;
+      }
+    }
+
+    g29_verbose_level = parser.seen('V') ? parser.value_int() : 0;
+    if (!WITHIN(g29_verbose_level, 0, 4)) {
+      SERIAL_ECHOLNPGM("?(V)erbose level implausible (0-4).\n");
+      err_flag = true;
+    }
+
+    if (parser.seen('P')) {
+      const int pv = parser.value_int();
+      #if !HAS_BED_PROBE
+        if (pv == 1) {
+          SERIAL_ECHOLNPGM("G29 P1 requires a probe.\n");
+          err_flag = true;
+        }
+        else
+      #endif
+        {
+          g29_phase_value = pv;
+          if (!WITHIN(g29_phase_value, 0, 6)) {
+            SERIAL_ECHOLNPGM("?(P)hase value invalid (0-6).\n");
+            err_flag = true;
+          }
+        }
+    }
+
+    if (parser.seen('J')) {
+      #if HAS_BED_PROBE
+        g29_grid_size = parser.has_value() ? parser.value_int() : 0;
+        if (g29_grid_size && !WITHIN(g29_grid_size, 2, 9)) {
+          SERIAL_ECHOLNPGM("?Invalid grid size (J) specified (2-9).\n");
+          err_flag = true;
+        }
+      #else
+        SERIAL_ECHOLNPGM("G29 J action requires a probe.\n");
+        err_flag = true;
+      #endif
+    }
+
+    xy_seen.x = parser.seenval('X');
+    float sx = xy_seen.x ? parser.value_float() : current_position.x;
+    xy_seen.y = parser.seenval('Y');
+    float sy = xy_seen.y ? parser.value_float() : current_position.y;
+
+    if (xy_seen.x != xy_seen.y) {
+      SERIAL_ECHOLNPGM("Both X & Y locations must be specified.\n");
+      err_flag = true;
+    }
+
+    // If X or Y are not valid, use center of the bed values
+    if (!WITHIN(sx, X_MIN_BED, X_MAX_BED)) sx = X_CENTER;
+    if (!WITHIN(sy, Y_MIN_BED, Y_MAX_BED)) sy = Y_CENTER;
+
+    if (err_flag) return UBL_ERR;
+
+    g29_pos.set(sx, sy);
+
+    /**
+     * Activate or deactivate UBL
+     * Note: UBL's G29 restores the state set here when done.
+     *       Leveling is being enabled here with old data, possibly
+     *       none. Error handling should disable for safety...
+     */
+    if (parser.seen('A')) {
+      if (parser.seen('D')) {
+        SERIAL_ECHOLNPGM("?Can't activate and deactivate at the same time.\n");
+        return UBL_ERR;
+      }
+      set_bed_leveling_enabled(true);
+      report_state();
+    }
+    else if (parser.seen('D')) {
+      set_bed_leveling_enabled(false);
+      report_state();
+    }
+
+    // Set global 'C' flag and its value
+    if ((g29_c_flag = parser.seen('C')))
+      g29_constant = parser.value_float();
+
+    #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
+      if (parser.seenval('F')) {
+        const float fh = parser.value_float();
+        if (!WITHIN(fh, 0, 100)) {
+          SERIAL_ECHOLNPGM("?(F)ade height for Bed Level Correction not plausible.\n");
+          return UBL_ERR;
+        }
+        set_z_fade_height(fh);
+      }
+    #endif
+
+    g29_map_type = parser.intval('T');
+    if (!WITHIN(g29_map_type, 0, 2)) {
+      SERIAL_ECHOLNPGM("Invalid map type.\n");
+      return UBL_ERR;
+    }
+    return UBL_OK;
+  }
+
+  static uint8_t ubl_state_at_invocation = 0;
+
+  #if ENABLED(UBL_DEVEL_DEBUGGING)
+    static uint8_t ubl_state_recursion_chk = 0;
+  #endif
+
+  void unified_bed_leveling::save_ubl_active_state_and_disable() {
+    #if ENABLED(UBL_DEVEL_DEBUGGING)
+      ubl_state_recursion_chk++;
+      if (ubl_state_recursion_chk != 1) {
+        SERIAL_ECHOLNPGM("save_ubl_active_state_and_disabled() called multiple times in a row.");
+        TERN_(HAS_LCD_MENU, set_message_with_feedback(GET_TEXT(MSG_UBL_SAVE_ERROR)));
+        return;
+      }
+    #endif
+    ubl_state_at_invocation = planner.leveling_active;
+    set_bed_leveling_enabled(false);
+  }
+
+  void unified_bed_leveling::restore_ubl_active_state_and_leave() {
+    TERN_(HAS_LCD_MENU, ui.release());
+    #if ENABLED(UBL_DEVEL_DEBUGGING)
+      if (--ubl_state_recursion_chk) {
+        SERIAL_ECHOLNPGM("restore_ubl_active_state_and_leave() called too many times.");
+        TERN_(HAS_LCD_MENU, set_message_with_feedback(GET_TEXT(MSG_UBL_RESTORE_ERROR)));
+        return;
+      }
+    #endif
+    set_bed_leveling_enabled(ubl_state_at_invocation);
+  }
+
+  mesh_index_pair unified_bed_leveling::find_furthest_invalid_mesh_point() {
+
+    bool found_a_NAN = false, found_a_real = false;
+
+    mesh_index_pair farthest { -1, -1, -99999.99 };
+
+    GRID_LOOP(i, j) {
+      if (!isnan(z_values[i][j])) continue;  // Skip valid mesh points
+
+      // Skip unreachable points
+      if (!probe.can_reach(mesh_index_to_xpos(i), mesh_index_to_ypos(j)))
+        continue;
+
+      found_a_NAN = true;
+
+      xy_int8_t nearby { -1, -1 };
+      float d1, d2 = 99999.9f;
+      GRID_LOOP(k, l) {
+        if (isnan(z_values[k][l])) continue;
+
+        found_a_real = true;
+
+        // Add in a random weighting factor that scrambles the probing of the
+        // last half of the mesh (when every unprobed mesh point is one index
+        // from a probed location).
+
+        d1 = HYPOT(i - k, j - l) + (1.0f / ((millis() % 47) + 13));
+
+        if (d1 < d2) {    // Invalid mesh point (i,j) is closer to the defined point (k,l)
+          d2 = d1;
+          nearby.set(i, j);
+        }
+      }
+
+      //
+      // At this point d2 should have the near defined mesh point to invalid mesh point (i,j)
+      //
+
+      if (found_a_real && nearby.x >= 0 && d2 > farthest.distance) {
+        farthest.pos = nearby; // Found an invalid location farther from the defined mesh point
+        farthest.distance = d2;
+      }
+    } // GRID_LOOP
+
+    if (!found_a_real && found_a_NAN) {        // if the mesh is totally unpopulated, start the probing
+      farthest.pos.set((GRID_MAX_POINTS_X) / 2, (GRID_MAX_POINTS_Y) / 2);
+      farthest.distance = 1;
+    }
+    return farthest;
+  }
+
+  mesh_index_pair unified_bed_leveling::find_closest_mesh_point_of_type(const MeshPointType type, const xy_pos_t &pos, const bool probe_relative/*=false*/, MeshFlags *done_flags/*=nullptr*/) {
+    mesh_index_pair closest;
+    closest.invalidate();
+    closest.distance = -99999.9f;
+
+    // Get the reference position, either nozzle or probe
+    const xy_pos_t ref = probe_relative ? pos + probe.offset_xy : pos;
+
+    float best_so_far = 99999.99f;
+
+    GRID_LOOP(i, j) {
+      if ( (type == (isnan(z_values[i][j]) ? INVALID : REAL))
+        || (type == SET_IN_BITMAP && !done_flags->marked(i, j))
+      ) {
+        // Found a Mesh Point of the specified type!
+        const xy_pos_t mpos = { mesh_index_to_xpos(i), mesh_index_to_ypos(j) };
+
+        // If using the probe as the reference there are some unreachable locations.
+        // Also for round beds, there are grid points outside the bed the nozzle can't reach.
+        // Prune them from the list and ignore them till the next Phase (manual nozzle probing).
+
+        if (!(probe_relative ? probe.can_reach(mpos) : position_is_reachable(mpos)))
+          continue;
+
+        // Reachable. Check if it's the best_so_far location to the nozzle.
+
+        const xy_pos_t diff = current_position - mpos;
+        const float distance = (ref - mpos).magnitude() + diff.magnitude() * 0.1f;
+
+        // factor in the distance from the current location for the normal case
+        // so the nozzle isn't running all over the bed.
+        if (distance < best_so_far) {
+          best_so_far = distance;   // Found a closer location with the desired value type.
+          closest.pos.set(i, j);
+          closest.distance = best_so_far;
+        }
+      }
+    } // GRID_LOOP
+
+    return closest;
+  }
+
+  /**
+   * 'Smart Fill': Scan from the outward edges of the mesh towards the center.
+   * If an invalid location is found, use the next two points (if valid) to
+   * calculate a 'reasonable' value for the unprobed mesh point.
+   */
+
+  bool unified_bed_leveling::smart_fill_one(const uint8_t x, const uint8_t y, const int8_t xdir, const int8_t ydir) {
+    const float v = z_values[x][y];
+    if (isnan(v)) {                           // A NAN...
+      const int8_t dx = x + xdir, dy = y + ydir;
+      const float v1 = z_values[dx][dy];
+      if (!isnan(v1)) {                       // ...next to a pair of real values?
+        const float v2 = z_values[dx + xdir][dy + ydir];
+        if (!isnan(v2)) {
+          z_values[x][y] = v1 < v2 ? v1 : v1 + v1 - v2;
+          TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(x, y, z_values[x][y]));
+          return true;
+        }
+      }
+    }
+    return false;
+  }
+
+  typedef struct { uint8_t sx, ex, sy, ey; bool yfirst; } smart_fill_info;
+
+  void unified_bed_leveling::smart_fill_mesh() {
+    static const smart_fill_info
+      info0 PROGMEM = { 0, GRID_MAX_POINTS_X,      0, GRID_MAX_POINTS_Y - 2,  false },  // Bottom of the mesh looking up
+      info1 PROGMEM = { 0, GRID_MAX_POINTS_X,      GRID_MAX_POINTS_Y - 1, 0,  false },  // Top of the mesh looking down
+      info2 PROGMEM = { 0, GRID_MAX_POINTS_X - 2,  0, GRID_MAX_POINTS_Y,      true  },  // Left side of the mesh looking right
+      info3 PROGMEM = { GRID_MAX_POINTS_X - 1, 0,  0, GRID_MAX_POINTS_Y,      true  };  // Right side of the mesh looking left
+    static const smart_fill_info * const info[] PROGMEM = { &info0, &info1, &info2, &info3 };
+
+    LOOP_L_N(i, COUNT(info)) {
+      const smart_fill_info *f = (smart_fill_info*)pgm_read_ptr(&info[i]);
+      const int8_t sx = pgm_read_byte(&f->sx), sy = pgm_read_byte(&f->sy),
+                   ex = pgm_read_byte(&f->ex), ey = pgm_read_byte(&f->ey);
+      if (pgm_read_byte(&f->yfirst)) {
+        const int8_t dir = ex > sx ? 1 : -1;
+        for (uint8_t y = sy; y != ey; ++y)
+          for (uint8_t x = sx; x != ex; x += dir)
+            if (smart_fill_one(x, y, dir, 0)) break;
+      }
+      else {
+        const int8_t dir = ey > sy ? 1 : -1;
+         for (uint8_t x = sx; x != ex; ++x)
+          for (uint8_t y = sy; y != ey; y += dir)
+            if (smart_fill_one(x, y, 0, dir)) break;
+      }
+    }
+  }
+
+  #if HAS_BED_PROBE
+
+    //#define VALIDATE_MESH_TILT
+
+    #include "../../../libs/vector_3.h"
+
+    void unified_bed_leveling::tilt_mesh_based_on_probed_grid(const bool do_3_pt_leveling) {
+      const float x_min = probe.min_x(), x_max = probe.max_x(),
+                  y_min = probe.min_y(), y_max = probe.max_y(),
+                  dx = (x_max - x_min) / (g29_grid_size - 1),
+                  dy = (y_max - y_min) / (g29_grid_size - 1);
+
+      xy_float_t points[3];
+      probe.get_three_points(points);
+
+      float measured_z;
+      bool abort_flag = false;
+
+      #ifdef VALIDATE_MESH_TILT
+        float z1, z2, z3;  // Needed for algorithm validation below
+      #endif
+
+      struct linear_fit_data lsf_results;
+      incremental_LSF_reset(&lsf_results);
+
+      if (do_3_pt_leveling) {
+        SERIAL_ECHOLNPGM("Tilting mesh (1/3)");
+        TERN_(HAS_DISPLAY, ui.status_printf_P(0, PSTR(S_FMT " 1/3"), GET_TEXT(MSG_LCD_TILTING_MESH)));
+
+        measured_z = probe.probe_at_point(points[0], PROBE_PT_RAISE, g29_verbose_level);
+        if (isnan(measured_z))
+          abort_flag = true;
+        else {
+          measured_z -= get_z_correction(points[0]);
+          #ifdef VALIDATE_MESH_TILT
+            z1 = measured_z;
+          #endif
+          if (g29_verbose_level > 3) {
+            serial_spaces(16);
+            SERIAL_ECHOLNPAIR("Corrected_Z=", measured_z);
+          }
+          incremental_LSF(&lsf_results, points[0], measured_z);
+        }
+
+        if (!abort_flag) {
+          SERIAL_ECHOLNPGM("Tilting mesh (2/3)");
+          TERN_(HAS_DISPLAY, ui.status_printf_P(0, PSTR(S_FMT " 2/3"), GET_TEXT(MSG_LCD_TILTING_MESH)));
+
+          measured_z = probe.probe_at_point(points[1], PROBE_PT_RAISE, g29_verbose_level);
+          #ifdef VALIDATE_MESH_TILT
+            z2 = measured_z;
+          #endif
+          if (isnan(measured_z))
+            abort_flag = true;
+          else {
+            measured_z -= get_z_correction(points[1]);
+            if (g29_verbose_level > 3) {
+              serial_spaces(16);
+              SERIAL_ECHOLNPAIR("Corrected_Z=", measured_z);
+            }
+            incremental_LSF(&lsf_results, points[1], measured_z);
+          }
+        }
+
+        if (!abort_flag) {
+          SERIAL_ECHOLNPGM("Tilting mesh (3/3)");
+          TERN_(HAS_DISPLAY, ui.status_printf_P(0, PSTR(S_FMT " 3/3"), GET_TEXT(MSG_LCD_TILTING_MESH)));
+
+          measured_z = probe.probe_at_point(points[2], PROBE_PT_STOW, g29_verbose_level);
+          #ifdef VALIDATE_MESH_TILT
+            z3 = measured_z;
+          #endif
+          if (isnan(measured_z))
+            abort_flag = true;
+          else {
+            measured_z -= get_z_correction(points[2]);
+            if (g29_verbose_level > 3) {
+              serial_spaces(16);
+              SERIAL_ECHOLNPAIR("Corrected_Z=", measured_z);
+            }
+            incremental_LSF(&lsf_results, points[2], measured_z);
+          }
+        }
+
+        probe.stow();
+        probe.move_z_after_probing();
+
+        if (abort_flag) {
+          SERIAL_ECHOLNPGM("?Error probing point. Aborting operation.");
+          return;
+        }
+      }
+      else { // !do_3_pt_leveling
+
+        bool zig_zag = false;
+
+        const uint16_t total_points = sq(g29_grid_size);
+        uint16_t point_num = 1;
+
+        xy_pos_t rpos;
+        LOOP_L_N(ix, g29_grid_size) {
+          rpos.x = x_min + ix * dx;
+          LOOP_L_N(iy, g29_grid_size) {
+            rpos.y = y_min + dy * (zig_zag ? g29_grid_size - 1 - iy : iy);
+
+            if (!abort_flag) {
+              SERIAL_ECHOLNPAIR("Tilting mesh point ", point_num, "/", total_points, "\n");
+              TERN_(HAS_DISPLAY, ui.status_printf_P(0, PSTR(S_FMT " %i/%i"), GET_TEXT(MSG_LCD_TILTING_MESH), point_num, total_points));
+
+              measured_z = probe.probe_at_point(rpos, parser.seen('E') ? PROBE_PT_STOW : PROBE_PT_RAISE, g29_verbose_level); // TODO: Needs error handling
+
+              abort_flag = isnan(measured_z);
+
+              #if ENABLED(DEBUG_LEVELING_FEATURE)
+                if (DEBUGGING(LEVELING)) {
+                  const xy_pos_t lpos = rpos.asLogical();
+                  DEBUG_CHAR('(');
+                  DEBUG_ECHO_F(rpos.x, 7);
+                  DEBUG_CHAR(',');
+                  DEBUG_ECHO_F(rpos.y, 7);
+                  DEBUG_ECHOPAIR_F(")   logical: (", lpos.x, 7);
+                  DEBUG_CHAR(',');
+                  DEBUG_ECHO_F(lpos.y, 7);
+                  DEBUG_ECHOPAIR_F(")   measured: ", measured_z, 7);
+                  DEBUG_ECHOPAIR_F("   correction: ", get_z_correction(rpos), 7);
+                }
+              #endif
+
+              measured_z -= get_z_correction(rpos) /* + probe.offset.z */ ;
+
+              if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR_F("   final >>>---> ", measured_z, 7);
+
+              if (g29_verbose_level > 3) {
+                serial_spaces(16);
+                SERIAL_ECHOLNPAIR("Corrected_Z=", measured_z);
+              }
+              incremental_LSF(&lsf_results, rpos, measured_z);
+            }
+
+            point_num++;
+          }
+
+          zig_zag ^= true;
+        }
+      }
+      probe.stow();
+      probe.move_z_after_probing();
+
+      if (abort_flag || finish_incremental_LSF(&lsf_results)) {
+        SERIAL_ECHOPGM("Could not complete LSF!");
+        return;
+      }
+
+      vector_3 normal = vector_3(lsf_results.A, lsf_results.B, 1).get_normal();
+
+      if (g29_verbose_level > 2) {
+        SERIAL_ECHOPAIR_F("bed plane normal = [", normal.x, 7);
+        SERIAL_CHAR(',');
+        SERIAL_ECHO_F(normal.y, 7);
+        SERIAL_CHAR(',');
+        SERIAL_ECHO_F(normal.z, 7);
+        SERIAL_ECHOLNPGM("]");
+      }
+
+      matrix_3x3 rotation = matrix_3x3::create_look_at(vector_3(lsf_results.A, lsf_results.B, 1));
+
+      GRID_LOOP(i, j) {
+        float mx = mesh_index_to_xpos(i),
+              my = mesh_index_to_ypos(j),
+              mz = z_values[i][j];
+
+        if (DEBUGGING(LEVELING)) {
+          DEBUG_ECHOPAIR_F("before rotation = [", mx, 7);
+          DEBUG_CHAR(',');
+          DEBUG_ECHO_F(my, 7);
+          DEBUG_CHAR(',');
+          DEBUG_ECHO_F(mz, 7);
+          DEBUG_ECHOPGM("]   ---> ");
+          DEBUG_DELAY(20);
+        }
+
+        apply_rotation_xyz(rotation, mx, my, mz);
+
+        if (DEBUGGING(LEVELING)) {
+          DEBUG_ECHOPAIR_F("after rotation = [", mx, 7);
+          DEBUG_CHAR(',');
+          DEBUG_ECHO_F(my, 7);
+          DEBUG_CHAR(',');
+          DEBUG_ECHO_F(mz, 7);
+          DEBUG_ECHOLNPGM("]");
+          DEBUG_DELAY(20);
+        }
+
+        z_values[i][j] = mz - lsf_results.D;
+        TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(i, j, z_values[i][j]));
+      }
+
+      if (DEBUGGING(LEVELING)) {
+        rotation.debug(PSTR("rotation matrix:\n"));
+        DEBUG_ECHOPAIR_F("LSF Results A=", lsf_results.A, 7);
+        DEBUG_ECHOPAIR_F("  B=", lsf_results.B, 7);
+        DEBUG_ECHOLNPAIR_F("  D=", lsf_results.D, 7);
+        DEBUG_DELAY(55);
+
+        DEBUG_ECHOPAIR_F("bed plane normal = [", normal.x, 7);
+        DEBUG_CHAR(',');
+        DEBUG_ECHO_F(normal.y, 7);
+        DEBUG_CHAR(',');
+        DEBUG_ECHO_F(normal.z, 7);
+        DEBUG_ECHOLNPGM("]");
+        DEBUG_EOL();
+
+        /**
+         * Use the code below to check the validity of the mesh tilting algorithm.
+         * 3-Point Mesh Tilt uses the same algorithm as grid-based tilting, but only
+         * three points are used in the calculation. This guarantees that each probed point
+         * has an exact match when get_z_correction() for that location is calculated.
+         * The Z error between the probed point locations and the get_z_correction()
+         * numbers for those locations should be 0.
+         */
+        #ifdef VALIDATE_MESH_TILT
+          auto d_from = []{ DEBUG_ECHOPGM("D from "); };
+          auto normed = [&](const xy_pos_t &pos, const float &zadd) {
+            return normal.x * pos.x + normal.y * pos.y + zadd;
+          };
+          auto debug_pt = [](PGM_P const pre, const xy_pos_t &pos, const float &zadd) {
+            d_from(); serialprintPGM(pre);
+            DEBUG_ECHO_F(normed(pos, zadd), 6);
+            DEBUG_ECHOLNPAIR_F("   Z error = ", zadd - get_z_correction(pos), 6);
+          };
+          debug_pt(PSTR("1st point: "), probe_pt[0], normal.z * z1);
+          debug_pt(PSTR("2nd point: "), probe_pt[1], normal.z * z2);
+          debug_pt(PSTR("3rd point: "), probe_pt[2], normal.z * z3);
+          d_from(); DEBUG_ECHOPGM("safe home with Z=");
+          DEBUG_ECHOLNPAIR_F("0 : ", normed(safe_homing_xy, 0), 6);
+          d_from(); DEBUG_ECHOPGM("safe home with Z=");
+          DEBUG_ECHOLNPAIR_F("mesh value ", normed(safe_homing_xy, get_z_correction(safe_homing_xy)), 6);
+          DEBUG_ECHOPAIR("   Z error = (", Z_SAFE_HOMING_X_POINT, ",", Z_SAFE_HOMING_Y_POINT);
+          DEBUG_ECHOLNPAIR_F(") = ", get_z_correction(safe_homing_xy), 6);
+        #endif
+      } // DEBUGGING(LEVELING)
+
+    }
+
+  #endif // HAS_BED_PROBE
+
+  #if ENABLED(UBL_G29_P31)
+    void unified_bed_leveling::smart_fill_wlsf(const float &weight_factor) {
+
+      // For each undefined mesh point, compute a distance-weighted least squares fit
+      // from all the originally populated mesh points, weighted toward the point
+      // being extrapolated so that nearby points will have greater influence on
+      // the point being extrapolated.  Then extrapolate the mesh point from WLSF.
+
+      static_assert((GRID_MAX_POINTS_Y) <= 16, "GRID_MAX_POINTS_Y too big");
+      uint16_t bitmap[GRID_MAX_POINTS_X] = { 0 };
+      struct linear_fit_data lsf_results;
+
+      SERIAL_ECHOPGM("Extrapolating mesh...");
+
+      const float weight_scaled = weight_factor * _MAX(MESH_X_DIST, MESH_Y_DIST);
+
+      GRID_LOOP(jx, jy) if (!isnan(z_values[jx][jy])) SBI(bitmap[jx], jy);
+
+      xy_pos_t ppos;
+      LOOP_L_N(ix, GRID_MAX_POINTS_X) {
+        ppos.x = mesh_index_to_xpos(ix);
+        LOOP_L_N(iy, GRID_MAX_POINTS_Y) {
+          ppos.y = mesh_index_to_ypos(iy);
+          if (isnan(z_values[ix][iy])) {
+            // undefined mesh point at (ppos.x,ppos.y), compute weighted LSF from original valid mesh points.
+            incremental_LSF_reset(&lsf_results);
+            xy_pos_t rpos;
+            LOOP_L_N(jx, GRID_MAX_POINTS_X) {
+              rpos.x = mesh_index_to_xpos(jx);
+              LOOP_L_N(jy, GRID_MAX_POINTS_Y) {
+                if (TEST(bitmap[jx], jy)) {
+                  rpos.y = mesh_index_to_ypos(jy);
+                  const float rz = z_values[jx][jy],
+                               w = 1.0f + weight_scaled / (rpos - ppos).magnitude();
+                  incremental_WLSF(&lsf_results, rpos, rz, w);
+                }
+              }
+            }
+            if (finish_incremental_LSF(&lsf_results)) {
+              SERIAL_ECHOLNPGM("Insufficient data");
+              return;
+            }
+            const float ez = -lsf_results.D - lsf_results.A * ppos.x - lsf_results.B * ppos.y;
+            z_values[ix][iy] = ez;
+            TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(ix, iy, z_values[ix][iy]));
+            idle(); // housekeeping
+          }
+        }
+      }
+
+      SERIAL_ECHOLNPGM("done");
+    }
+  #endif // UBL_G29_P31
+
+  #if ENABLED(UBL_DEVEL_DEBUGGING)
+    /**
+     * Much of the 'What?' command can be eliminated. But until we are fully debugged, it is
+     * good to have the extra information. Soon... we prune this to just a few items
+     */
+    void unified_bed_leveling::g29_what_command() {
+      report_state();
+
+      if (storage_slot == -1)
+        SERIAL_ECHOPGM("No Mesh Loaded.");
+      else
+        SERIAL_ECHOPAIR("Mesh ", storage_slot, " Loaded.");
+      SERIAL_EOL();
+      serial_delay(50);
+
+      #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
+        SERIAL_ECHOLNPAIR_F("Fade Height M420 Z", planner.z_fade_height, 4);
+      #endif
+
+      adjust_mesh_to_mean(g29_c_flag, g29_constant);
+
+      #if HAS_BED_PROBE
+        SERIAL_ECHOLNPAIR_F("Probe Offset M851 Z", probe.offset.z, 7);
+      #endif
+
+      SERIAL_ECHOLNPAIR("MESH_MIN_X  " STRINGIFY(MESH_MIN_X) "=", MESH_MIN_X); serial_delay(50);
+      SERIAL_ECHOLNPAIR("MESH_MIN_Y  " STRINGIFY(MESH_MIN_Y) "=", MESH_MIN_Y); serial_delay(50);
+      SERIAL_ECHOLNPAIR("MESH_MAX_X  " STRINGIFY(MESH_MAX_X) "=", MESH_MAX_X); serial_delay(50);
+      SERIAL_ECHOLNPAIR("MESH_MAX_Y  " STRINGIFY(MESH_MAX_Y) "=", MESH_MAX_Y); serial_delay(50);
+      SERIAL_ECHOLNPAIR("GRID_MAX_POINTS_X  ", GRID_MAX_POINTS_X);             serial_delay(50);
+      SERIAL_ECHOLNPAIR("GRID_MAX_POINTS_Y  ", GRID_MAX_POINTS_Y);             serial_delay(50);
+      SERIAL_ECHOLNPAIR("MESH_X_DIST  ", MESH_X_DIST);
+      SERIAL_ECHOLNPAIR("MESH_Y_DIST  ", MESH_Y_DIST);                         serial_delay(50);
+
+      SERIAL_ECHOPGM("X-Axis Mesh Points at: ");
+      LOOP_L_N(i, GRID_MAX_POINTS_X) {
+        SERIAL_ECHO_F(LOGICAL_X_POSITION(mesh_index_to_xpos(i)), 3);
+        SERIAL_ECHOPGM("  ");
+        serial_delay(25);
+      }
+      SERIAL_EOL();
+
+      SERIAL_ECHOPGM("Y-Axis Mesh Points at: ");
+      LOOP_L_N(i, GRID_MAX_POINTS_Y) {
+        SERIAL_ECHO_F(LOGICAL_Y_POSITION(mesh_index_to_ypos(i)), 3);
+        SERIAL_ECHOPGM("  ");
+        serial_delay(25);
+      }
+      SERIAL_EOL();
+
+      #if HAS_KILL
+        SERIAL_ECHOLNPAIR("Kill pin on :", int(KILL_PIN), "  state:", int(kill_state()));
+      #endif
+
+      SERIAL_EOL();
+      serial_delay(50);
+
+      #if ENABLED(UBL_DEVEL_DEBUGGING)
+        SERIAL_ECHOLNPAIR("ubl_state_at_invocation :", ubl_state_at_invocation, "\nubl_state_recursion_chk :", ubl_state_recursion_chk);
+        serial_delay(50);
+
+        SERIAL_ECHOLNPAIR("Meshes go from ", hex_address((void*)settings.meshes_start_index()), " to ", hex_address((void*)settings.meshes_end_index()));
+        serial_delay(50);
+
+        SERIAL_ECHOLNPAIR("sizeof(ubl) :  ", (int)sizeof(ubl));         SERIAL_EOL();
+        SERIAL_ECHOLNPAIR("z_value[][] size: ", (int)sizeof(z_values)); SERIAL_EOL();
+        serial_delay(25);
+
+        SERIAL_ECHOLNPAIR("EEPROM free for UBL: ", hex_address((void*)(settings.meshes_end_index() - settings.meshes_start_index())));
+        serial_delay(50);
+
+        SERIAL_ECHOLNPAIR("EEPROM can hold ", settings.calc_num_meshes(), " meshes.\n");
+        serial_delay(25);
+      #endif // UBL_DEVEL_DEBUGGING
+
+      if (!sanity_check()) {
+        echo_name();
+        SERIAL_ECHOLNPGM(" sanity checks passed.");
+      }
+    }
+
+    /**
+     * When we are fully debugged, the EEPROM dump command will get deleted also. But
+     * right now, it is good to have the extra information. Soon... we prune this.
+     */
+    void unified_bed_leveling::g29_eeprom_dump() {
+      uint8_t cccc;
+
+      SERIAL_ECHO_MSG("EEPROM Dump:");
+      persistentStore.access_start();
+      for (uint16_t i = 0; i < persistentStore.capacity(); i += 16) {
+        if (!(i & 0x3)) idle();
+        print_hex_word(i);
+        SERIAL_ECHOPGM(": ");
+        for (uint16_t j = 0; j < 16; j++) {
+          persistentStore.read_data(i + j, &cccc, sizeof(uint8_t));
+          print_hex_byte(cccc);
+          SERIAL_CHAR(' ');
+        }
+        SERIAL_EOL();
+      }
+      SERIAL_EOL();
+      persistentStore.access_finish();
+    }
+
+    /**
+     * When we are fully debugged, this may go away. But there are some valid
+     * use cases for the users. So we can wait and see what to do with it.
+     */
+    void unified_bed_leveling::g29_compare_current_mesh_to_stored_mesh() {
+      const int16_t a = settings.calc_num_meshes();
+
+      if (!a) {
+        SERIAL_ECHOLNPGM("?EEPROM storage not available.");
+        return;
+      }
+
+      if (!parser.has_value() || !WITHIN(g29_storage_slot, 0, a - 1)) {
+        SERIAL_ECHOLNPAIR("?Invalid storage slot.\n?Use 0 to ", a - 1);
+        return;
+      }
+
+      g29_storage_slot = parser.value_int();
+
+      float tmp_z_values[GRID_MAX_POINTS_X][GRID_MAX_POINTS_Y];
+      settings.load_mesh(g29_storage_slot, &tmp_z_values);
+
+      SERIAL_ECHOLNPAIR("Subtracting mesh in slot ", g29_storage_slot, " from current mesh.");
+
+      GRID_LOOP(x, y) {
+        z_values[x][y] -= tmp_z_values[x][y];
+        TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(x, y, z_values[x][y]));
+      }
+    }
+
+  #endif // UBL_DEVEL_DEBUGGING
+
+#endif // AUTO_BED_LEVELING_UBL
diff --git a/Marlin/src/feature/bedlevel/ubl/ubl_motion.cpp b/Marlin/src/feature/bedlevel/ubl/ubl_motion.cpp
new file mode 100644
index 0000000..8b7cd15
--- /dev/null
+++ b/Marlin/src/feature/bedlevel/ubl/ubl_motion.cpp
@@ -0,0 +1,474 @@
+/**
+ * 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 ENABLED(AUTO_BED_LEVELING_UBL)
+
+#include "../bedlevel.h"
+#include "../../../module/planner.h"
+#include "../../../module/stepper.h"
+#include "../../../module/motion.h"
+
+#if ENABLED(DELTA)
+  #include "../../../module/delta.h"
+#endif
+
+#include "../../../MarlinCore.h"
+#include <math.h>
+
+#if !UBL_SEGMENTED
+
+  void unified_bed_leveling::line_to_destination_cartesian(const feedRate_t &scaled_fr_mm_s, const uint8_t extruder) {
+    /**
+     * Much of the nozzle movement will be within the same cell. So we will do as little computation
+     * as possible to determine if this is the case. If this move is within the same cell, we will
+     * just do the required Z-Height correction, call the Planner's buffer_line() routine, and leave
+     */
+    #if HAS_POSITION_MODIFIERS
+      xyze_pos_t start = current_position, end = destination;
+      planner.apply_modifiers(start);
+      planner.apply_modifiers(end);
+    #else
+      const xyze_pos_t &start = current_position, &end = destination;
+    #endif
+
+    const xy_int8_t istart = cell_indexes(start), iend = cell_indexes(end);
+
+    // A move within the same cell needs no splitting
+    if (istart == iend) {
+
+      FINAL_MOVE:
+
+      // When UBL_Z_RAISE_WHEN_OFF_MESH is disabled Z correction is extrapolated from the edge of the mesh
+      #ifdef UBL_Z_RAISE_WHEN_OFF_MESH
+        // For a move off the UBL mesh, use a constant Z raise
+        if (!cell_index_x_valid(end.x) || !cell_index_y_valid(end.y)) {
+
+          // Note: There is no Z Correction in this case. We are off the mesh and don't know what
+          // a reasonable correction would be, UBL_Z_RAISE_WHEN_OFF_MESH will be used instead of
+          // a calculated (Bi-Linear interpolation) correction.
+
+          end.z += UBL_Z_RAISE_WHEN_OFF_MESH;
+          planner.buffer_segment(end, scaled_fr_mm_s, extruder);
+          current_position = destination;
+          return;
+        }
+      #endif
+
+      // The distance is always MESH_X_DIST so multiply by the constant reciprocal.
+      const float xratio = (end.x - mesh_index_to_xpos(iend.x)) * RECIPROCAL(MESH_X_DIST),
+                  yratio = (end.y - mesh_index_to_ypos(iend.y)) * RECIPROCAL(MESH_Y_DIST),
+                  z1 = z_values[iend.x][iend.y    ] + xratio * (z_values[iend.x + 1][iend.y    ] - z_values[iend.x][iend.y    ]),
+                  z2 = z_values[iend.x][iend.y + 1] + xratio * (z_values[iend.x + 1][iend.y + 1] - z_values[iend.x][iend.y + 1]);
+
+      // X cell-fraction done. Interpolate the two Z offsets with the Y fraction for the final Z offset.
+      const float z0 = (z1 + (z2 - z1) * yratio) * planner.fade_scaling_factor_for_z(end.z);
+
+      // Undefined parts of the Mesh in z_values[][] are NAN.
+      // Replace NAN corrections with 0.0 to prevent NAN propagation.
+      if (!isnan(z0)) end.z += z0;
+      planner.buffer_segment(end, scaled_fr_mm_s, extruder);
+      current_position = destination;
+      return;
+    }
+
+    /**
+     * Past this point the move is known to cross one or more mesh lines. Check for the most common
+     * case - crossing only one X or Y line - after details are worked out to reduce computation.
+     */
+
+    const xy_float_t dist = end - start;
+    const xy_bool_t neg { dist.x < 0, dist.y < 0 };
+    const xy_int8_t ineg { int8_t(neg.x), int8_t(neg.y) };
+    const xy_float_t sign { neg.x ? -1.0f : 1.0f, neg.y ? -1.0f : 1.0f };
+    const xy_int8_t iadd { int8_t(iend.x == istart.x ? 0 : sign.x), int8_t(iend.y == istart.y ? 0 : sign.y) };
+
+    /**
+     * Compute the extruder scaling factor for each partial move, checking for
+     * zero-length moves that would result in an infinite scaling factor.
+     * A float divide is required for this, but then it just multiplies.
+     * Also select a scaling factor based on the larger of the X and Y
+     * components. The larger of the two is used to preserve precision.
+     */
+
+    const xy_float_t ad = sign * dist;
+    const bool use_x_dist = ad.x > ad.y;
+
+    float on_axis_distance = use_x_dist ? dist.x : dist.y,
+          e_position = end.e - start.e,
+          z_position = end.z - start.z;
+
+    const float e_normalized_dist = e_position / on_axis_distance, // Allow divide by zero
+                z_normalized_dist = z_position / on_axis_distance;
+
+    xy_int8_t icell = istart;
+
+    const float ratio = dist.y / dist.x,        // Allow divide by zero
+                c = start.y - ratio * start.x;
+
+    const bool inf_normalized_flag = isinf(e_normalized_dist),
+               inf_ratio_flag = isinf(ratio);
+
+    /**
+     * Handle vertical lines that stay within one column.
+     * These need not be perfectly vertical.
+     */
+    if (iadd.x == 0) {        // Vertical line?
+      icell.y += ineg.y;      // Line going down? Just go to the bottom.
+      while (icell.y != iend.y + ineg.y) {
+        icell.y += iadd.y;
+        const float next_mesh_line_y = mesh_index_to_ypos(icell.y);
+
+        /**
+         * Skip the calculations for an infinite slope.
+         * For others the next X is the same so this can continue.
+         * Calculate X at the next Y mesh line.
+         */
+        const float rx = inf_ratio_flag ? start.x : (next_mesh_line_y - c) / ratio;
+
+        float z0 = z_correction_for_x_on_horizontal_mesh_line(rx, icell.x, icell.y)
+                   * planner.fade_scaling_factor_for_z(end.z);
+
+        // Undefined parts of the Mesh in z_values[][] are NAN.
+        // Replace NAN corrections with 0.0 to prevent NAN propagation.
+        if (isnan(z0)) z0 = 0.0;
+
+        const float ry = mesh_index_to_ypos(icell.y);
+
+        /**
+         * Without this check, it's possible to generate a zero length move, as in the case where
+         * the line is heading down, starting exactly on a mesh line boundary. Since this is rare
+         * it might be fine to remove this check and let planner.buffer_segment() filter it out.
+         */
+        if (ry != start.y) {
+          if (!inf_normalized_flag) { // fall-through faster than branch
+            on_axis_distance = use_x_dist ? rx - start.x : ry - start.y;
+            e_position = start.e + on_axis_distance * e_normalized_dist;
+            z_position = start.z + on_axis_distance * z_normalized_dist;
+          }
+          else {
+            e_position = end.e;
+            z_position = end.z;
+          }
+
+          planner.buffer_segment(rx, ry, z_position + z0, e_position, scaled_fr_mm_s, extruder);
+        } //else printf("FIRST MOVE PRUNED  ");
+      }
+
+      // At the final destination? Usually not, but when on a Y Mesh Line it's completed.
+      if (xy_pos_t(current_position) != xy_pos_t(end))
+        goto FINAL_MOVE;
+
+      current_position = destination;
+      return;
+    }
+
+    /**
+     * Handle horizontal lines that stay within one row.
+     * These need not be perfectly horizontal.
+     */
+    if (iadd.y == 0) {      // Horizontal line?
+      icell.x += ineg.x;     // Heading left? Just go to the left edge of the cell for the first move.
+      while (icell.x != iend.x + ineg.x) {
+        icell.x += iadd.x;
+        const float rx = mesh_index_to_xpos(icell.x);
+        const float ry = ratio * rx + c;    // Calculate Y at the next X mesh line
+
+        float z0 = z_correction_for_y_on_vertical_mesh_line(ry, icell.x, icell.y)
+                     * planner.fade_scaling_factor_for_z(end.z);
+
+        // Undefined parts of the Mesh in z_values[][] are NAN.
+        // Replace NAN corrections with 0.0 to prevent NAN propagation.
+        if (isnan(z0)) z0 = 0.0;
+
+        /**
+         * Without this check, it's possible to generate a zero length move, as in the case where
+         * the line is heading left, starting exactly on a mesh line boundary. Since this is rare
+         * it might be fine to remove this check and let planner.buffer_segment() filter it out.
+         */
+        if (rx != start.x) {
+          if (!inf_normalized_flag) {
+            on_axis_distance = use_x_dist ? rx - start.x : ry - start.y;
+            e_position = start.e + on_axis_distance * e_normalized_dist;  // is based on X or Y because this is a horizontal move
+            z_position = start.z + on_axis_distance * z_normalized_dist;
+          }
+          else {
+            e_position = end.e;
+            z_position = end.z;
+          }
+
+          if (!planner.buffer_segment(rx, ry, z_position + z0, e_position, scaled_fr_mm_s, extruder))
+            break;
+        } //else printf("FIRST MOVE PRUNED  ");
+      }
+
+      if (xy_pos_t(current_position) != xy_pos_t(end))
+        goto FINAL_MOVE;
+
+      current_position = destination;
+      return;
+    }
+
+    /**
+     * Generic case of a line crossing both X and Y Mesh lines.
+     */
+
+    xy_int8_t cnt = (istart - iend).ABS();
+
+    icell += ineg;
+
+    while (cnt) {
+
+      const float next_mesh_line_x = mesh_index_to_xpos(icell.x + iadd.x),
+                  next_mesh_line_y = mesh_index_to_ypos(icell.y + iadd.y),
+                  ry = ratio * next_mesh_line_x + c,    // Calculate Y at the next X mesh line
+                  rx = (next_mesh_line_y - c) / ratio;  // Calculate X at the next Y mesh line
+                                                        // (No need to worry about ratio == 0.
+                                                        //  In that case, it was already detected
+                                                        //  as a vertical line move above.)
+
+      if (neg.x == (rx > next_mesh_line_x)) { // Check if we hit the Y line first
+        // Yes!  Crossing a Y Mesh Line next
+        float z0 = z_correction_for_x_on_horizontal_mesh_line(rx, icell.x - ineg.x, icell.y + iadd.y)
+                   * planner.fade_scaling_factor_for_z(end.z);
+
+        // Undefined parts of the Mesh in z_values[][] are NAN.
+        // Replace NAN corrections with 0.0 to prevent NAN propagation.
+        if (isnan(z0)) z0 = 0.0;
+
+        if (!inf_normalized_flag) {
+          on_axis_distance = use_x_dist ? rx - start.x : next_mesh_line_y - start.y;
+          e_position = start.e + on_axis_distance * e_normalized_dist;
+          z_position = start.z + on_axis_distance * z_normalized_dist;
+        }
+        else {
+          e_position = end.e;
+          z_position = end.z;
+        }
+        if (!planner.buffer_segment(rx, next_mesh_line_y, z_position + z0, e_position, scaled_fr_mm_s, extruder))
+          break;
+        icell.y += iadd.y;
+        cnt.y--;
+      }
+      else {
+        // Yes!  Crossing a X Mesh Line next
+        float z0 = z_correction_for_y_on_vertical_mesh_line(ry, icell.x + iadd.x, icell.y - ineg.y)
+                   * planner.fade_scaling_factor_for_z(end.z);
+
+        // Undefined parts of the Mesh in z_values[][] are NAN.
+        // Replace NAN corrections with 0.0 to prevent NAN propagation.
+        if (isnan(z0)) z0 = 0.0;
+
+        if (!inf_normalized_flag) {
+          on_axis_distance = use_x_dist ? next_mesh_line_x - start.x : ry - start.y;
+          e_position = start.e + on_axis_distance * e_normalized_dist;
+          z_position = start.z + on_axis_distance * z_normalized_dist;
+        }
+        else {
+          e_position = end.e;
+          z_position = end.z;
+        }
+
+        if (!planner.buffer_segment(next_mesh_line_x, ry, z_position + z0, e_position, scaled_fr_mm_s, extruder))
+          break;
+        icell.x += iadd.x;
+        cnt.x--;
+      }
+
+      if (cnt.x < 0 || cnt.y < 0) break; // Too far! Exit the loop and go to FINAL_MOVE
+    }
+
+    if (xy_pos_t(current_position) != xy_pos_t(end))
+      goto FINAL_MOVE;
+
+    current_position = destination;
+  }
+
+#else // UBL_SEGMENTED
+
+  #if IS_SCARA
+    #define DELTA_SEGMENT_MIN_LENGTH 0.25 // SCARA minimum segment size is 0.25mm
+  #elif ENABLED(DELTA)
+    #define DELTA_SEGMENT_MIN_LENGTH 0.10 // mm (still subject to DELTA_SEGMENTS_PER_SECOND)
+  #else // CARTESIAN
+    #ifdef LEVELED_SEGMENT_LENGTH
+      #define DELTA_SEGMENT_MIN_LENGTH LEVELED_SEGMENT_LENGTH
+    #else
+      #define DELTA_SEGMENT_MIN_LENGTH 1.00 // mm (similar to G2/G3 arc segmentation)
+    #endif
+  #endif
+
+  /**
+   * Prepare a segmented linear move for DELTA/SCARA/CARTESIAN with UBL and FADE semantics.
+   * This calls planner.buffer_segment multiple times for small incremental moves.
+   * Returns true if did NOT move, false if moved (requires current_position update).
+   */
+
+  bool _O2 unified_bed_leveling::line_to_destination_segmented(const feedRate_t &scaled_fr_mm_s) {
+
+    if (!position_is_reachable(destination))  // fail if moving outside reachable boundary
+      return true;                            // did not move, so current_position still accurate
+
+    const xyze_pos_t total = destination - current_position;
+
+    const float cart_xy_mm_2 = HYPOT2(total.x, total.y),
+                cart_xy_mm = SQRT(cart_xy_mm_2);                                     // Total XY distance
+
+    #if IS_KINEMATIC
+      const float seconds = cart_xy_mm / scaled_fr_mm_s;                             // Duration of XY move at requested rate
+      uint16_t segments = LROUND(delta_segments_per_second * seconds),               // Preferred number of segments for distance @ feedrate
+               seglimit = LROUND(cart_xy_mm * RECIPROCAL(DELTA_SEGMENT_MIN_LENGTH)); // Number of segments at minimum segment length
+      NOMORE(segments, seglimit);                                                    // Limit to minimum segment length (fewer segments)
+    #else
+      uint16_t segments = LROUND(cart_xy_mm * RECIPROCAL(DELTA_SEGMENT_MIN_LENGTH)); // Cartesian fixed segment length
+    #endif
+
+    NOLESS(segments, 1U);                                                            // Must have at least one segment
+    const float inv_segments = 1.0f / segments,                                      // Reciprocal to save calculation
+                segment_xyz_mm = SQRT(cart_xy_mm_2 + sq(total.z)) * inv_segments;    // Length of each segment
+
+    #if ENABLED(SCARA_FEEDRATE_SCALING)
+      const float inv_duration = scaled_fr_mm_s / segment_xyz_mm;
+    #endif
+
+    xyze_float_t diff = total * inv_segments;
+
+    // Note that E segment distance could vary slightly as z mesh height
+    // changes for each segment, but small enough to ignore.
+
+    xyze_pos_t raw = current_position;
+
+    // Just do plain segmentation if UBL is inactive or the target is above the fade height
+    if (!planner.leveling_active || !planner.leveling_active_at_z(destination.z)) {
+      while (--segments) {
+        raw += diff;
+        planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, segment_xyz_mm
+          #if ENABLED(SCARA_FEEDRATE_SCALING)
+            , inv_duration
+          #endif
+        );
+      }
+      planner.buffer_line(destination, scaled_fr_mm_s, active_extruder, segment_xyz_mm
+        #if ENABLED(SCARA_FEEDRATE_SCALING)
+          , inv_duration
+        #endif
+      );
+      return false; // Did not set current from destination
+    }
+
+    // Otherwise perform per-segment leveling
+
+    #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
+      const float fade_scaling_factor = planner.fade_scaling_factor_for_z(destination.z);
+    #endif
+
+    // Move to first segment destination
+    raw += diff;
+
+    for (;;) {  // for each mesh cell encountered during the move
+
+      // Compute mesh cell invariants that remain constant for all segments within cell.
+      // Note for cell index, if point is outside the mesh grid (in MESH_INSET perimeter)
+      // the bilinear interpolation from the adjacent cell within the mesh will still work.
+      // Inner loop will exit each time (because out of cell bounds) but will come back
+      // in top of loop and again re-find same adjacent cell and use it, just less efficient
+      // for mesh inset area.
+
+      xy_int8_t icell = {
+        int8_t((raw.x - (MESH_MIN_X)) * RECIPROCAL(MESH_X_DIST)),
+        int8_t((raw.y - (MESH_MIN_Y)) * RECIPROCAL(MESH_Y_DIST))
+      };
+      LIMIT(icell.x, 0, (GRID_MAX_POINTS_X) - 1);
+      LIMIT(icell.y, 0, (GRID_MAX_POINTS_Y) - 1);
+
+      float z_x0y0 = z_values[icell.x  ][icell.y  ],  // z at lower left corner
+            z_x1y0 = z_values[icell.x+1][icell.y  ],  // z at upper left corner
+            z_x0y1 = z_values[icell.x  ][icell.y+1],  // z at lower right corner
+            z_x1y1 = z_values[icell.x+1][icell.y+1];  // z at upper right corner
+
+      if (isnan(z_x0y0)) z_x0y0 = 0;              // ideally activating planner.leveling_active (G29 A)
+      if (isnan(z_x1y0)) z_x1y0 = 0;              //   should refuse if any invalid mesh points
+      if (isnan(z_x0y1)) z_x0y1 = 0;              //   in order to avoid isnan tests per cell,
+      if (isnan(z_x1y1)) z_x1y1 = 0;              //   thus guessing zero for undefined points
+
+      const xy_pos_t pos = { mesh_index_to_xpos(icell.x), mesh_index_to_ypos(icell.y) };
+      xy_pos_t cell = raw - pos;
+
+      const float z_xmy0 = (z_x1y0 - z_x0y0) * RECIPROCAL(MESH_X_DIST),   // z slope per x along y0 (lower left to lower right)
+                  z_xmy1 = (z_x1y1 - z_x0y1) * RECIPROCAL(MESH_X_DIST);   // z slope per x along y1 (upper left to upper right)
+
+            float z_cxy0 = z_x0y0 + z_xmy0 * cell.x;        // z height along y0 at cell.x (changes for each cell.x in cell)
+
+      const float z_cxy1 = z_x0y1 + z_xmy1 * cell.x,        // z height along y1 at cell.x
+                  z_cxyd = z_cxy1 - z_cxy0;                 // z height difference along cell.x from y0 to y1
+
+            float z_cxym = z_cxyd * RECIPROCAL(MESH_Y_DIST); // z slope per y along cell.x from pos.y to y1 (changes for each cell.x in cell)
+
+      //    float z_cxcy = z_cxy0 + z_cxym * cell.y;        // interpolated mesh z height along cell.x at cell.y (do inside the segment loop)
+
+      // As subsequent segments step through this cell, the z_cxy0 intercept will change
+      // and the z_cxym slope will change, both as a function of cell.x within the cell, and
+      // each change by a constant for fixed segment lengths.
+
+      const float z_sxy0 = z_xmy0 * diff.x,                                       // per-segment adjustment to z_cxy0
+                  z_sxym = (z_xmy1 - z_xmy0) * RECIPROCAL(MESH_Y_DIST) * diff.x;  // per-segment adjustment to z_cxym
+
+      for (;;) {  // for all segments within this mesh cell
+
+        if (--segments == 0) raw = destination;     // if this is last segment, use destination for exact
+
+        const float z_cxcy = (z_cxy0 + z_cxym * cell.y) // interpolated mesh z height along cell.x at cell.y
+          #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
+            * fade_scaling_factor                   // apply fade factor to interpolated mesh height
+          #endif
+        ;
+
+        planner.buffer_line(raw.x, raw.y, raw.z + z_cxcy, raw.e, scaled_fr_mm_s, active_extruder, segment_xyz_mm
+          #if ENABLED(SCARA_FEEDRATE_SCALING)
+            , inv_duration
+          #endif
+        );
+
+        if (segments == 0)                        // done with last segment
+          return false;                           // didn't set current from destination
+
+        raw += diff;
+        cell += diff;
+
+        if (!WITHIN(cell.x, 0, MESH_X_DIST) || !WITHIN(cell.y, 0, MESH_Y_DIST))    // done within this cell, break to next
+          break;
+
+        // Next segment still within same mesh cell, adjust the per-segment
+        // slope and intercept to compute next z height.
+
+        z_cxy0 += z_sxy0;   // adjust z_cxy0 by per-segment z_sxy0
+        z_cxym += z_sxym;   // adjust z_cxym by per-segment z_sxym
+
+      } // segment loop
+    } // cell loop
+
+    return false; // caller will update current_position
+  }
+
+#endif // UBL_SEGMENTED
+
+#endif // AUTO_BED_LEVELING_UBL
diff --git a/Marlin/src/feature/binary_stream.cpp b/Marlin/src/feature/binary_stream.cpp
new file mode 100644
index 0000000..81e1103
--- /dev/null
+++ b/Marlin/src/feature/binary_stream.cpp
@@ -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/>.
+ *
+ */
+
+#include "../inc/MarlinConfigPre.h"
+
+#if ENABLED(BINARY_FILE_TRANSFER)
+
+#include "../sd/cardreader.h"
+#include "binary_stream.h"
+
+char* SDFileTransferProtocol::Packet::Open::data = nullptr;
+size_t SDFileTransferProtocol::data_waiting, SDFileTransferProtocol::transfer_timeout, SDFileTransferProtocol::idle_timeout;
+bool SDFileTransferProtocol::transfer_active, SDFileTransferProtocol::dummy_transfer, SDFileTransferProtocol::compression;
+
+BinaryStream binaryStream[NUM_SERIAL];
+
+#endif
diff --git a/Marlin/src/feature/binary_stream.h b/Marlin/src/feature/binary_stream.h
new file mode 100644
index 0000000..81d6e71
--- /dev/null
+++ b/Marlin/src/feature/binary_stream.h
@@ -0,0 +1,462 @@
+/**
+ * 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 BINARY_STREAM_COMPRESSION
+
+#if ENABLED(BINARY_STREAM_COMPRESSION)
+  #include "../libs/heatshrink/heatshrink_decoder.h"
+#endif
+
+inline bool bs_serial_data_available(const uint8_t index) {
+  return SERIAL_IMPL.available(index);
+}
+
+inline int bs_read_serial(const uint8_t index) {
+  return SERIAL_IMPL.read(index);
+}
+
+#if ENABLED(BINARY_STREAM_COMPRESSION)
+  static heatshrink_decoder hsd;
+  static uint8_t decode_buffer[512] = {};
+#endif
+
+class SDFileTransferProtocol  {
+private:
+  struct Packet {
+    struct [[gnu::packed]] Open {
+      static bool validate(char* buffer, size_t length) {
+        return (length > sizeof(Open) && buffer[length - 1] == '\0');
+      }
+      static Open& decode(char* buffer) {
+        data = &buffer[2];
+        return *reinterpret_cast<Open*>(buffer);
+      }
+      bool compression_enabled() { return compression & 0x1; }
+      bool dummy_transfer() { return dummy & 0x1; }
+      static char* filename() { return data; }
+      private:
+        uint8_t dummy, compression;
+        static char* data;  // variable length strings complicate things
+    };
+  };
+
+  static bool file_open(char* filename) {
+    if (!dummy_transfer) {
+      card.mount();
+      card.openFileWrite(filename);
+      if (!card.isFileOpen()) return false;
+    }
+    transfer_active = true;
+    data_waiting = 0;
+    TERN_(BINARY_STREAM_COMPRESSION, heatshrink_decoder_reset(&hsd));
+    return true;
+  }
+
+  static bool file_write(char* buffer, const size_t length) {
+    #if ENABLED(BINARY_STREAM_COMPRESSION)
+      if (compression) {
+        size_t total_processed = 0, processed_count = 0;
+        HSD_poll_res presult;
+
+        while (total_processed < length) {
+          heatshrink_decoder_sink(&hsd, reinterpret_cast<uint8_t*>(&buffer[total_processed]), length - total_processed, &processed_count);
+          total_processed += processed_count;
+          do {
+            presult = heatshrink_decoder_poll(&hsd, &decode_buffer[data_waiting], sizeof(decode_buffer) - data_waiting, &processed_count);
+            data_waiting += processed_count;
+            if (data_waiting == sizeof(decode_buffer)) {
+              if (!dummy_transfer)
+                if (card.write(decode_buffer, data_waiting) < 0) {
+                  return false;
+                }
+              data_waiting = 0;
+            }
+          } while (presult == HSDR_POLL_MORE);
+        }
+        return true;
+      }
+    #endif
+    return (dummy_transfer || card.write(buffer, length) >= 0);
+  }
+
+  static bool file_close() {
+    if (!dummy_transfer) {
+      #if ENABLED(BINARY_STREAM_COMPRESSION)
+        // flush any buffered data
+        if (data_waiting) {
+          if (card.write(decode_buffer, data_waiting) < 0) return false;
+          data_waiting = 0;
+        }
+      #endif
+      card.closefile();
+      card.release();
+    }
+    TERN_(BINARY_STREAM_COMPRESSION, heatshrink_decoder_finish(&hsd));
+    transfer_active = false;
+    return true;
+  }
+
+  static void transfer_abort() {
+    if (!dummy_transfer) {
+      card.closefile();
+      card.removeFile(card.filename);
+      card.release();
+      TERN_(BINARY_STREAM_COMPRESSION, heatshrink_decoder_finish(&hsd));
+    }
+    transfer_active = false;
+    return;
+  }
+
+  enum class FileTransfer : uint8_t { QUERY, OPEN, CLOSE, WRITE, ABORT };
+
+  static size_t data_waiting, transfer_timeout, idle_timeout;
+  static bool transfer_active, dummy_transfer, compression;
+
+public:
+
+  static void idle() {
+    // If a transfer is interrupted and a file is left open, abort it after TIMEOUT ms
+    const millis_t ms = millis();
+    if (transfer_active && ELAPSED(ms, idle_timeout)) {
+      idle_timeout = ms + IDLE_PERIOD;
+      if (ELAPSED(ms, transfer_timeout)) transfer_abort();
+    }
+  }
+
+  static void process(uint8_t packet_type, char* buffer, const uint16_t length) {
+    transfer_timeout = millis() + TIMEOUT;
+    switch (static_cast<FileTransfer>(packet_type)) {
+      case FileTransfer::QUERY:
+        SERIAL_ECHOPAIR("PFT:version:", VERSION_MAJOR, ".", VERSION_MINOR, ".", VERSION_PATCH);
+        #if ENABLED(BINARY_STREAM_COMPRESSION)
+          SERIAL_ECHOLNPAIR(":compresion:heatshrink,", HEATSHRINK_STATIC_WINDOW_BITS, ",", HEATSHRINK_STATIC_LOOKAHEAD_BITS);
+        #else
+          SERIAL_ECHOLNPGM(":compresion:none");
+        #endif
+        break;
+      case FileTransfer::OPEN:
+        if (transfer_active)
+          SERIAL_ECHOLNPGM("PFT:busy");
+        else {
+          if (Packet::Open::validate(buffer, length)) {
+            auto packet = Packet::Open::decode(buffer);
+            compression = packet.compression_enabled();
+            dummy_transfer = packet.dummy_transfer();
+            if (file_open(packet.filename())) {
+              SERIAL_ECHOLNPGM("PFT:success");
+              break;
+            }
+          }
+          SERIAL_ECHOLNPGM("PFT:fail");
+        }
+        break;
+      case FileTransfer::CLOSE:
+        if (transfer_active) {
+          if (file_close())
+            SERIAL_ECHOLNPGM("PFT:success");
+          else
+            SERIAL_ECHOLNPGM("PFT:ioerror");
+        }
+        else SERIAL_ECHOLNPGM("PFT:invalid");
+        break;
+      case FileTransfer::WRITE:
+        if (!transfer_active)
+          SERIAL_ECHOLNPGM("PFT:invalid");
+        else if (!file_write(buffer, length))
+          SERIAL_ECHOLNPGM("PFT:ioerror");
+        break;
+      case FileTransfer::ABORT:
+        transfer_abort();
+        SERIAL_ECHOLNPGM("PFT:success");
+        break;
+      default:
+        SERIAL_ECHOLNPGM("PTF:invalid");
+        break;
+    }
+  }
+
+  static const uint16_t VERSION_MAJOR = 0, VERSION_MINOR = 1, VERSION_PATCH = 0, TIMEOUT = 10000, IDLE_PERIOD = 1000;
+};
+
+class BinaryStream {
+public:
+  enum class Protocol : uint8_t { CONTROL, FILE_TRANSFER };
+
+  enum class ProtocolControl : uint8_t { SYNC = 1, CLOSE };
+
+  enum class StreamState : uint8_t { PACKET_RESET, PACKET_WAIT, PACKET_HEADER, PACKET_DATA, PACKET_FOOTER,
+                                     PACKET_PROCESS, PACKET_RESEND, PACKET_TIMEOUT, PACKET_ERROR };
+
+  struct Packet { // 10 byte protocol overhead, ascii with checksum and line number has a minimum of 7 increasing with line
+
+    union Header {
+      static constexpr uint16_t HEADER_TOKEN = 0xB5AD;
+      struct [[gnu::packed]] {
+        uint16_t token;       // packet start token
+        uint8_t sync;         // stream sync, resend id and packet loss detection
+        uint8_t meta;         // 4 bit protocol,
+                              // 4 bit packet type
+        uint16_t size;        // data length
+        uint16_t checksum;    // header checksum
+      };
+      uint8_t protocol() { return (meta >> 4) & 0xF; }
+      uint8_t type() { return meta & 0xF; }
+      void reset() { token = 0; sync = 0; meta = 0; size = 0; checksum = 0; }
+      uint8_t data[2];
+    };
+
+    union Footer {
+      struct [[gnu::packed]] {
+        uint16_t checksum; // full packet checksum
+      };
+      void reset() { checksum = 0; }
+      uint8_t data[1];
+    };
+
+    Header header;
+    Footer footer;
+    uint32_t bytes_received;
+    uint16_t checksum, header_checksum;
+    millis_t timeout;
+    char* buffer;
+
+    void reset() {
+      header.reset();
+      footer.reset();
+      bytes_received = 0;
+      checksum = 0;
+      header_checksum = 0;
+      timeout = millis() + PACKET_MAX_WAIT;
+      buffer = nullptr;
+    }
+  } packet{};
+
+  void reset() {
+    sync = 0;
+    packet_retries = 0;
+    buffer_next_index = 0;
+  }
+
+  // fletchers 16 checksum
+  uint32_t checksum(uint32_t cs, uint8_t value) {
+    uint16_t cs_low = (((cs & 0xFF) + value) % 255);
+    return ((((cs >> 8) + cs_low) % 255) << 8)  | cs_low;
+  }
+
+  // read the next byte from the data stream keeping track of
+  // whether the stream times out from data starvation
+  // takes the data variable by reference in order to return status
+  bool stream_read(uint8_t& data) {
+    if (stream_state != StreamState::PACKET_WAIT && ELAPSED(millis(), packet.timeout)) {
+      stream_state = StreamState::PACKET_TIMEOUT;
+      return false;
+    }
+    if (!bs_serial_data_available(card.transfer_port_index)) return false;
+    data = bs_read_serial(card.transfer_port_index);
+    packet.timeout = millis() + PACKET_MAX_WAIT;
+    return true;
+  }
+
+  template<const size_t buffer_size>
+  void receive(char (&buffer)[buffer_size]) {
+    uint8_t data = 0;
+    millis_t transfer_window = millis() + RX_TIMESLICE;
+
+    #if ENABLED(SDSUPPORT)
+      PORT_REDIRECT(SERIAL_PORTMASK(card.transfer_port_index));
+    #endif
+
+    #pragma GCC diagnostic push
+    #pragma GCC diagnostic ignored "-Warray-bounds"
+
+    while (PENDING(millis(), transfer_window)) {
+      switch (stream_state) {
+         /**
+          * Data stream packet handling
+          */
+        case StreamState::PACKET_RESET:
+          packet.reset();
+          stream_state = StreamState::PACKET_WAIT;
+        case StreamState::PACKET_WAIT:
+          if (!stream_read(data)) { idle(); return; }  // no active packet so don't wait
+          packet.header.data[1] = data;
+          if (packet.header.token == packet.header.HEADER_TOKEN) {
+            packet.bytes_received = 2;
+            stream_state = StreamState::PACKET_HEADER;
+          }
+          else {
+            // stream corruption drop data
+            packet.header.data[0] = data;
+          }
+          break;
+        case StreamState::PACKET_HEADER:
+          if (!stream_read(data)) break;
+
+          packet.header.data[packet.bytes_received++] = data;
+          packet.checksum = checksum(packet.checksum, data);
+
+          // header checksum calculation can't contain the checksum
+          if (packet.bytes_received == sizeof(Packet::header) - 2)
+            packet.header_checksum = packet.checksum;
+
+          if (packet.bytes_received == sizeof(Packet::header)) {
+            if (packet.header.checksum == packet.header_checksum) {
+              // The SYNC control packet is a special case in that it doesn't require the stream sync to be correct
+              if (static_cast<Protocol>(packet.header.protocol()) == Protocol::CONTROL && static_cast<ProtocolControl>(packet.header.type()) == ProtocolControl::SYNC) {
+                  SERIAL_ECHOLNPAIR("ss", sync, ",", buffer_size, ",", VERSION_MAJOR, ".", VERSION_MINOR, ".", VERSION_PATCH);
+                  stream_state = StreamState::PACKET_RESET;
+                  break;
+              }
+              if (packet.header.sync == sync) {
+                buffer_next_index = 0;
+                packet.bytes_received = 0;
+                if (packet.header.size) {
+                  stream_state = StreamState::PACKET_DATA;
+                  packet.buffer = static_cast<char *>(&buffer[0]); // multipacket buffering not implemented, always allocate whole buffer to packet
+                }
+                else
+                  stream_state = StreamState::PACKET_PROCESS;
+              }
+              else if (packet.header.sync == sync - 1) {           // ok response must have been lost
+                SERIAL_ECHOLNPAIR("ok", packet.header.sync);  // transmit valid packet received and drop the payload
+                stream_state = StreamState::PACKET_RESET;
+              }
+              else if (packet_retries) {
+                stream_state = StreamState::PACKET_RESET; // could be packets already buffered on flow controlled connections, drop them without ack
+              }
+              else {
+                SERIAL_ECHO_MSG("Datastream packet out of order");
+                stream_state = StreamState::PACKET_RESEND;
+              }
+            }
+            else {
+              SERIAL_ECHO_START();
+              SERIAL_ECHOLNPAIR("Packet header(", packet.header.sync, "?) corrupt");
+              stream_state = StreamState::PACKET_RESEND;
+            }
+          }
+          break;
+        case StreamState::PACKET_DATA:
+          if (!stream_read(data)) break;
+
+          if (buffer_next_index < buffer_size)
+            packet.buffer[buffer_next_index] = data;
+          else {
+            SERIAL_ECHO_MSG("Datastream packet data buffer overrun");
+            stream_state = StreamState::PACKET_ERROR;
+            break;
+          }
+
+          packet.checksum = checksum(packet.checksum, data);
+          packet.bytes_received++;
+          buffer_next_index++;
+
+          if (packet.bytes_received == packet.header.size) {
+            stream_state = StreamState::PACKET_FOOTER;
+            packet.bytes_received = 0;
+          }
+          break;
+        case StreamState::PACKET_FOOTER:
+          if (!stream_read(data)) break;
+
+          packet.footer.data[packet.bytes_received++] = data;
+          if (packet.bytes_received == sizeof(Packet::footer)) {
+            if (packet.footer.checksum == packet.checksum) {
+              stream_state = StreamState::PACKET_PROCESS;
+            }
+            else {
+              SERIAL_ECHO_START();
+              SERIAL_ECHOLNPAIR("Packet(", packet.header.sync, ") payload corrupt");
+              stream_state = StreamState::PACKET_RESEND;
+            }
+          }
+          break;
+        case StreamState::PACKET_PROCESS:
+          sync++;
+          packet_retries = 0;
+          bytes_received += packet.header.size;
+
+          SERIAL_ECHOLNPAIR("ok", packet.header.sync); // transmit valid packet received
+          dispatch();
+          stream_state = StreamState::PACKET_RESET;
+          break;
+        case StreamState::PACKET_RESEND:
+          if (packet_retries < MAX_RETRIES || MAX_RETRIES == 0) {
+            packet_retries++;
+            stream_state = StreamState::PACKET_RESET;
+            SERIAL_ECHO_START();
+            SERIAL_ECHOLNPAIR("Resend request ", int(packet_retries));
+            SERIAL_ECHOLNPAIR("rs", sync);
+          }
+          else
+            stream_state = StreamState::PACKET_ERROR;
+          break;
+        case StreamState::PACKET_TIMEOUT:
+          SERIAL_ECHO_MSG("Datastream timeout");
+          stream_state = StreamState::PACKET_RESEND;
+          break;
+        case StreamState::PACKET_ERROR:
+          SERIAL_ECHOLNPAIR("fe", packet.header.sync);
+          reset(); // reset everything, resync required
+          stream_state = StreamState::PACKET_RESET;
+          break;
+      }
+    }
+
+    #pragma GCC diagnostic pop
+  }
+
+  void dispatch() {
+    switch (static_cast<Protocol>(packet.header.protocol())) {
+      case Protocol::CONTROL:
+        switch (static_cast<ProtocolControl>(packet.header.type())) {
+          case ProtocolControl::CLOSE: // revert back to ASCII mode
+            card.flag.binary_mode = false;
+            break;
+          default:
+            SERIAL_ECHO_MSG("Unknown BinaryProtocolControl Packet");
+        }
+        break;
+      case Protocol::FILE_TRANSFER:
+        SDFileTransferProtocol::process(packet.header.type(), packet.buffer, packet.header.size); // send user data to be processed
+      break;
+      default:
+        SERIAL_ECHO_MSG("Unsupported Binary Protocol");
+    }
+  }
+
+  void idle() {
+    // Some Protocols may need periodic updates without new data
+    SDFileTransferProtocol::idle();
+  }
+
+  static const uint16_t PACKET_MAX_WAIT = 500, RX_TIMESLICE = 20, MAX_RETRIES = 0, VERSION_MAJOR = 0, VERSION_MINOR = 1, VERSION_PATCH = 0;
+  uint8_t  packet_retries, sync;
+  uint16_t buffer_next_index;
+  uint32_t bytes_received;
+  StreamState stream_state = StreamState::PACKET_RESET;
+};
+
+extern BinaryStream binaryStream[NUM_SERIAL];
diff --git a/Marlin/src/feature/bltouch.cpp b/Marlin/src/feature/bltouch.cpp
new file mode 100644
index 0000000..48eaf9e
--- /dev/null
+++ b/Marlin/src/feature/bltouch.cpp
@@ -0,0 +1,199 @@
+/**
+ * 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 ENABLED(BLTOUCH)
+
+#include "bltouch.h"
+
+BLTouch bltouch;
+
+bool BLTouch::last_written_mode; // Initialized by settings.load, 0 = Open Drain; 1 = 5V Drain
+
+#include "../module/servo.h"
+#include "../module/probe.h"
+
+void stop();
+
+#define DEBUG_OUT ENABLED(DEBUG_LEVELING_FEATURE)
+#include "../core/debug_out.h"
+
+bool BLTouch::command(const BLTCommand cmd, const millis_t &ms) {
+  if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPAIR("BLTouch Command :", cmd);
+  MOVE_SERVO(Z_PROBE_SERVO_NR, cmd);
+  safe_delay(_MAX(ms, (uint32_t)BLTOUCH_DELAY)); // BLTOUCH_DELAY is also the *minimum* delay
+  return triggered();
+}
+
+// Init the class and device. Call from setup().
+void BLTouch::init(const bool set_voltage/*=false*/) {
+  // Voltage Setting (if enabled). At every Marlin initialization:
+  // BLTOUCH < V3.0 and clones: This will be ignored by the probe
+  // BLTOUCH V3.0: SET_5V_MODE or SET_OD_MODE (if enabled).
+  //               OD_MODE is the default on power on, but setting it does not hurt
+  //               This mode will stay active until manual SET_OD_MODE or power cycle
+  // BLTOUCH V3.1: SET_5V_MODE or SET_OD_MODE (if enabled).
+  //               At power on, the probe will default to the eeprom settings configured by the user
+  _reset();
+  _stow();
+
+  #if ENABLED(BLTOUCH_FORCE_MODE_SET)
+
+    constexpr bool should_set = true;
+
+  #else
+
+    if (DEBUGGING(LEVELING)) {
+      DEBUG_ECHOLNPAIR("last_written_mode - ", (int)last_written_mode);
+      DEBUG_ECHOLNPGM("config mode - "
+        #if ENABLED(BLTOUCH_SET_5V_MODE)
+          "BLTOUCH_SET_5V_MODE"
+        #else
+          "OD"
+        #endif
+      );
+    }
+
+    const bool should_set = last_written_mode != ENABLED(BLTOUCH_SET_5V_MODE);
+
+  #endif
+
+  if (should_set && set_voltage)
+    mode_conv_proc(ENABLED(BLTOUCH_SET_5V_MODE));
+}
+
+void BLTouch::clear() {
+  _reset();    // RESET or RESET_SW will clear an alarm condition but...
+               // ...it will not clear a triggered condition in SW mode when the pin is currently up
+               // ANTClabs <-- CODE ERROR
+  _stow();     // STOW will pull up the pin and clear any triggered condition unless it fails, don't care
+  _deploy();   // DEPLOY to test the probe. Could fail, don't care
+  _stow();     // STOW to be ready for meaningful work. Could fail, don't care
+}
+
+bool BLTouch::triggered() { return PROBE_TRIGGERED(); }
+
+bool BLTouch::deploy_proc() {
+  // Do a DEPLOY
+  if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("BLTouch DEPLOY requested");
+
+  // Attempt to DEPLOY, wait for DEPLOY_DELAY or ALARM
+  if (_deploy_query_alarm()) {
+    // The deploy might have failed or the probe is already triggered (nozzle too low?)
+    if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("BLTouch ALARM or TRIGGER after DEPLOY, recovering");
+
+    clear();                               // Get the probe into start condition
+
+    // Last attempt to DEPLOY
+    if (_deploy_query_alarm()) {
+      // The deploy might have failed or the probe is actually triggered (nozzle too low?) again
+      if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("BLTouch Recovery Failed");
+
+      SERIAL_ERROR_MSG(STR_STOP_BLTOUCH);  // Tell the user something is wrong, needs action
+      stop();                              // but it's not too bad, no need to kill, allow restart
+
+      return true;                         // Tell our caller we goofed in case he cares to know
+    }
+  }
+
+  // One of the recommended ANTClabs ways to probe, using SW MODE
+  TERN_(BLTOUCH_FORCE_SW_MODE, _set_SW_mode());
+
+  // Now the probe is ready to issue a 10ms pulse when the pin goes up.
+  // The trigger STOW (see motion.cpp for example) will pull up the probes pin as soon as the pulse
+  // is registered.
+
+  if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("bltouch.deploy_proc() end");
+
+  return false; // report success to caller
+}
+
+bool BLTouch::stow_proc() {
+  // Do a STOW
+  if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("BLTouch STOW requested");
+
+  // A STOW will clear a triggered condition in the probe (10ms pulse).
+  // At the moment that we come in here, we might (pulse) or will (SW mode) see the trigger on the pin.
+  // So even though we know a STOW will be ignored if an ALARM condition is active, we will STOW.
+  // Note: If the probe is deployed AND in an ALARM condition, this STOW will not pull up the pin
+  // and the ALARM condition will still be there. --> ANTClabs should change this behavior maybe
+
+  // Attempt to STOW, wait for STOW_DELAY or ALARM
+  if (_stow_query_alarm()) {
+    // The stow might have failed
+    if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("BLTouch ALARM or TRIGGER after STOW, recovering");
+
+    _reset();                              // This RESET will then also pull up the pin. If it doesn't
+                                           // work and the pin is still down, there will no longer be
+                                           // an ALARM condition though.
+                                           // But one more STOW will catch that
+    // Last attempt to STOW
+    if (_stow_query_alarm()) {             // so if there is now STILL an ALARM condition:
+
+      if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("BLTouch Recovery Failed");
+
+      SERIAL_ERROR_MSG(STR_STOP_BLTOUCH);  // Tell the user something is wrong, needs action
+      stop();                              // but it's not too bad, no need to kill, allow restart
+
+      return true;                         // Tell our caller we goofed in case he cares to know
+    }
+  }
+
+  if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("bltouch.stow_proc() end");
+
+  return false; // report success to caller
+}
+
+bool BLTouch::status_proc() {
+  /**
+   * Return a TRUE for "YES, it is DEPLOYED"
+   * This function will ensure switch state is reset after execution
+   */
+
+  if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("BLTouch STATUS requested");
+
+  _set_SW_mode();              // Incidentally, _set_SW_mode() will also RESET any active alarm
+  const bool tr = triggered(); // If triggered in SW mode, the pin is up, it is STOWED
+
+  if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("BLTouch is ", (int)tr);
+
+  if (tr) _stow(); else _deploy();  // Turn off SW mode, reset any trigger, honor pin state
+  return !tr;
+}
+
+void BLTouch::mode_conv_proc(const bool M5V) {
+  /**
+   * BLTOUCH pre V3.0 and clones: No reaction at all to this sequence apart from a DEPLOY -> STOW
+   * BLTOUCH V3.0: This will set the mode (twice) and sadly, a STOW is needed at the end, because of the deploy
+   * BLTOUCH V3.1: This will set the mode and store it in the eeprom. The STOW is not needed but does not hurt
+   */
+  if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("BLTouch Set Mode - ", (int)M5V);
+  _deploy();
+  if (M5V) _set_5V_mode(); else _set_OD_mode();
+  _mode_store();
+  if (M5V) _set_5V_mode(); else _set_OD_mode();
+  _stow();
+  last_written_mode = M5V;
+}
+
+#endif // BLTOUCH
diff --git a/Marlin/src/feature/bltouch.h b/Marlin/src/feature/bltouch.h
new file mode 100644
index 0000000..8bd41f0
--- /dev/null
+++ b/Marlin/src/feature/bltouch.h
@@ -0,0 +1,113 @@
+/**
+ * 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"
+
+#if DISABLED(BLTOUCH_HS_MODE)
+  #define BLTOUCH_SLOW_MODE 1
+#endif
+
+// BLTouch commands are sent as servo angles
+typedef unsigned char BLTCommand;
+
+#define STOW_ALARM            true
+#define BLTOUCH_DEPLOY          10
+#define BLTOUCH_STOW            90
+#define BLTOUCH_SW_MODE         60
+#define BLTOUCH_SELFTEST       120
+#define BLTOUCH_MODE_STORE     130
+#define BLTOUCH_5V_MODE        140
+#define BLTOUCH_OD_MODE        150
+#define BLTOUCH_RESET          160
+
+/**
+ * The following commands require different minimum delays.
+ *
+ * 500ms required for a reliable Reset.
+ *
+ * 750ms required for Deploy/Stow, otherwise the alarm state
+ *       will not be seen until the following move command.
+ */
+
+#ifndef BLTOUCH_SET5V_DELAY
+  #define BLTOUCH_SET5V_DELAY   150
+#endif
+#ifndef BLTOUCH_SETOD_DELAY
+  #define BLTOUCH_SETOD_DELAY   150
+#endif
+#ifndef BLTOUCH_MODE_STORE_DELAY
+  #define BLTOUCH_MODE_STORE_DELAY 150
+#endif
+#ifndef BLTOUCH_DEPLOY_DELAY
+  #define BLTOUCH_DEPLOY_DELAY   750
+#endif
+#ifndef BLTOUCH_STOW_DELAY
+  #define BLTOUCH_STOW_DELAY     750
+#endif
+#ifndef BLTOUCH_RESET_DELAY
+  #define BLTOUCH_RESET_DELAY    500
+#endif
+
+class BLTouch {
+public:
+  static void init(const bool set_voltage=false);
+  static bool last_written_mode; // Initialized by settings.load, 0 = Open Drain; 1 = 5V Drain
+
+  // DEPLOY and STOW are wrapped for error handling - these are used by homing and by probing
+  FORCE_INLINE static bool deploy()              { return deploy_proc(); }
+  FORCE_INLINE static bool stow()                { return stow_proc(); }
+  FORCE_INLINE static bool status()              { return status_proc(); }
+
+  // Native BLTouch commands ("Underscore"...), used in lcd menus and internally
+  FORCE_INLINE static void _reset()              { command(BLTOUCH_RESET, BLTOUCH_RESET_DELAY); }
+
+  FORCE_INLINE static void _selftest()           { command(BLTOUCH_SELFTEST, BLTOUCH_DELAY); }
+
+  FORCE_INLINE static void _set_SW_mode()        { command(BLTOUCH_SW_MODE, BLTOUCH_DELAY); }
+  FORCE_INLINE static void _reset_SW_mode()      { if (triggered()) _stow(); else _deploy(); }
+
+  FORCE_INLINE static void _set_5V_mode()        { command(BLTOUCH_5V_MODE, BLTOUCH_SET5V_DELAY); }
+  FORCE_INLINE static void _set_OD_mode()        { command(BLTOUCH_OD_MODE, BLTOUCH_SETOD_DELAY); }
+  FORCE_INLINE static void _mode_store()         { command(BLTOUCH_MODE_STORE, BLTOUCH_MODE_STORE_DELAY); }
+
+  FORCE_INLINE static void _deploy()             { command(BLTOUCH_DEPLOY, BLTOUCH_DEPLOY_DELAY); }
+  FORCE_INLINE static void _stow()               { command(BLTOUCH_STOW, BLTOUCH_STOW_DELAY); }
+
+  FORCE_INLINE static void mode_conv_5V()        { mode_conv_proc(true); }
+  FORCE_INLINE static void mode_conv_OD()        { mode_conv_proc(false); }
+
+  static bool triggered();
+
+private:
+  FORCE_INLINE static bool _deploy_query_alarm() { return command(BLTOUCH_DEPLOY, BLTOUCH_DEPLOY_DELAY); }
+  FORCE_INLINE static bool _stow_query_alarm()   { return command(BLTOUCH_STOW, BLTOUCH_STOW_DELAY) == STOW_ALARM; }
+
+  static void clear();
+  static bool command(const BLTCommand cmd, const millis_t &ms);
+  static bool deploy_proc();
+  static bool stow_proc();
+  static bool status_proc();
+  static void mode_conv_proc(const bool M5V);
+};
+
+extern BLTouch bltouch;
diff --git a/Marlin/src/feature/cancel_object.cpp b/Marlin/src/feature/cancel_object.cpp
new file mode 100644
index 0000000..853e765
--- /dev/null
+++ b/Marlin/src/feature/cancel_object.cpp
@@ -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/>.
+ *
+ */
+#include "../inc/MarlinConfig.h"
+
+#if ENABLED(CANCEL_OBJECTS)
+
+#include "cancel_object.h"
+#include "../gcode/gcode.h"
+#include "../lcd/marlinui.h"
+
+CancelObject cancelable;
+
+int8_t CancelObject::object_count, // = 0
+       CancelObject::active_object = -1;
+uint32_t CancelObject::canceled; // = 0x0000
+bool CancelObject::skipping; // = false
+
+void CancelObject::set_active_object(const int8_t obj) {
+  active_object = obj;
+  if (WITHIN(obj, 0, 31)) {
+    if (obj >= object_count) object_count = obj + 1;
+    skipping = TEST(canceled, obj);
+  }
+  else
+    skipping = false;
+
+  #if HAS_DISPLAY
+    if (active_object >= 0)
+      ui.status_printf_P(0, PSTR(S_FMT " %i"), GET_TEXT(MSG_PRINTING_OBJECT), int(active_object));
+    else
+      ui.reset_status();
+  #endif
+}
+
+void CancelObject::cancel_object(const int8_t obj) {
+  if (WITHIN(obj, 0, 31)) {
+    SBI(canceled, obj);
+    if (obj == active_object) skipping = true;
+  }
+}
+
+void CancelObject::uncancel_object(const int8_t obj) {
+  if (WITHIN(obj, 0, 31)) {
+    CBI(canceled, obj);
+    if (obj == active_object) skipping = false;
+  }
+}
+
+void CancelObject::report() {
+  if (active_object >= 0) {
+    SERIAL_ECHO_START();
+    SERIAL_ECHOLNPAIR("Active Object: ", int(active_object));
+  }
+
+  if (canceled) {
+    SERIAL_ECHO_START();
+    SERIAL_ECHOPGM("Canceled:");
+    for (int i = 0; i < object_count; i++)
+      if (TEST(canceled, i)) { SERIAL_CHAR(' '); SERIAL_ECHO(i); }
+    SERIAL_EOL();
+  }
+}
+
+#endif // CANCEL_OBJECTS
diff --git a/Marlin/src/feature/cancel_object.h b/Marlin/src/feature/cancel_object.h
new file mode 100644
index 0000000..1d2d77f
--- /dev/null
+++ b/Marlin/src/feature/cancel_object.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
+
+#include <stdint.h>
+
+class CancelObject {
+public:
+  static bool skipping;
+  static int8_t object_count, active_object;
+  static uint32_t canceled;
+  static void set_active_object(const int8_t obj);
+  static void cancel_object(const int8_t obj);
+  static void uncancel_object(const int8_t obj);
+  static void report();
+  static inline bool is_canceled(const int8_t obj) { return TEST(canceled, obj); }
+  static inline void clear_active_object() { set_active_object(-1); }
+  static inline void cancel_active_object() { cancel_object(active_object); }
+  static inline void reset() { canceled = 0x0000; object_count = 0; clear_active_object(); }
+};
+
+extern CancelObject cancelable;
diff --git a/Marlin/src/feature/caselight.cpp b/Marlin/src/feature/caselight.cpp
new file mode 100644
index 0000000..0eba102
--- /dev/null
+++ b/Marlin/src/feature/caselight.cpp
@@ -0,0 +1,102 @@
+/**
+ * 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 ENABLED(CASE_LIGHT_ENABLE)
+
+#include "caselight.h"
+
+CaseLight caselight;
+
+#if CASELIGHT_USES_BRIGHTNESS && !defined(CASE_LIGHT_DEFAULT_BRIGHTNESS)
+  #define CASE_LIGHT_DEFAULT_BRIGHTNESS 0 // For use on PWM pin as non-PWM just sets a default
+#endif
+
+#if CASELIGHT_USES_BRIGHTNESS
+  uint8_t CaseLight::brightness = CASE_LIGHT_DEFAULT_BRIGHTNESS;
+#endif
+
+bool CaseLight::on = CASE_LIGHT_DEFAULT_ON;
+
+#if ENABLED(CASE_LIGHT_USE_NEOPIXEL)
+  LEDColor CaseLight::color =
+    #ifdef CASE_LIGHT_NEOPIXEL_COLOR
+      CASE_LIGHT_NEOPIXEL_COLOR
+    #else
+      { 255, 255, 255, 255 }
+    #endif
+  ;
+#endif
+
+#ifndef INVERT_CASE_LIGHT
+  #define INVERT_CASE_LIGHT false
+#endif
+
+void CaseLight::update(const bool sflag) {
+  #if CASELIGHT_USES_BRIGHTNESS
+    /**
+     * The brightness_sav (and sflag) is needed because ARM chips ignore
+     * a "WRITE(CASE_LIGHT_PIN,x)" command to the pins that are directly
+     * controlled by the PWM module. In order to turn them off the brightness
+     * level needs to be set to OFF. Since we can't use the PWM register to
+     * save the last brightness level we need a variable to save it.
+     */
+    static uint8_t brightness_sav;  // Save brightness info for restore on "M355 S1"
+
+    if (on || !sflag)
+      brightness_sav = brightness;  // Save brightness except for M355 S0
+    if (sflag && on)
+      brightness = brightness_sav;  // Restore last brightness for M355 S1
+
+    const uint8_t i = on ? brightness : 0, n10ct = INVERT_CASE_LIGHT ? 255 - i : i;
+  #endif
+
+  #if ENABLED(CASE_LIGHT_USE_NEOPIXEL)
+
+    leds.set_color(
+      MakeLEDColor(color.r, color.g, color.b, color.w, n10ct),
+      false
+    );
+
+  #else // !CASE_LIGHT_USE_NEOPIXEL
+
+    #if CASELIGHT_USES_BRIGHTNESS
+      if (PWM_PIN(CASE_LIGHT_PIN))
+        analogWrite(pin_t(CASE_LIGHT_PIN), (
+          #if CASE_LIGHT_MAX_PWM == 255
+            n10ct
+          #else
+            map(n10ct, 0, 255, 0, CASE_LIGHT_MAX_PWM)
+          #endif
+        ));
+      else
+    #endif
+      {
+        const bool s = on ? !INVERT_CASE_LIGHT : INVERT_CASE_LIGHT;
+        WRITE(CASE_LIGHT_PIN, s ? HIGH : LOW);
+      }
+
+  #endif // !CASE_LIGHT_USE_NEOPIXEL
+}
+
+#endif // CASE_LIGHT_ENABLE
diff --git a/Marlin/src/feature/caselight.h b/Marlin/src/feature/caselight.h
new file mode 100644
index 0000000..2198c85
--- /dev/null
+++ b/Marlin/src/feature/caselight.h
@@ -0,0 +1,51 @@
+/**
+ * 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"
+
+#if ENABLED(CASE_LIGHT_USE_NEOPIXEL)
+  #include "leds/leds.h"
+#endif
+
+#if DISABLED(CASE_LIGHT_NO_BRIGHTNESS) || ENABLED(CASE_LIGHT_USE_NEOPIXEL)
+  #define CASELIGHT_USES_BRIGHTNESS 1
+#endif
+
+class CaseLight {
+public:
+  #if CASELIGHT_USES_BRIGHTNESS
+    static uint8_t brightness;
+  #endif
+  static bool on;
+
+  static void update(const bool sflag);
+  static inline void update_brightness() { update(false); }
+  static inline void update_enabled() { update(true); }
+
+private:
+  #if ENABLED(CASE_LIGHT_USE_NEOPIXEL)
+    static LEDColor color;
+  #endif
+};
+
+extern CaseLight caselight;
diff --git a/Marlin/src/feature/closedloop.cpp b/Marlin/src/feature/closedloop.cpp
new file mode 100644
index 0000000..8a97f0c
--- /dev/null
+++ b/Marlin/src/feature/closedloop.cpp
@@ -0,0 +1,43 @@
+/**
+ * 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 ENABLED(EXTERNAL_CLOSED_LOOP_CONTROLLER)
+
+#if !PIN_EXISTS(CLOSED_LOOP_ENABLE) || !PIN_EXISTS(CLOSED_LOOP_MOVE_COMPLETE)
+  #error "CLOSED_LOOP_ENABLE_PIN and CLOSED_LOOP_MOVE_COMPLETE_PIN are required for EXTERNAL_CLOSED_LOOP_CONTROLLER."
+#endif
+
+#include "closedloop.h"
+
+ClosedLoop closedloop;
+
+void ClosedLoop::init() {
+  OUT_WRITE(CLOSED_LOOP_ENABLE_PIN, LOW);
+  SET_INPUT_PULLUP(CLOSED_LOOP_MOVE_COMPLETE_PIN);
+}
+
+void ClosedLoop::set(const byte val) {
+  OUT_WRITE(CLOSED_LOOP_ENABLE_PIN, val);
+}
+
+#endif // EXTERNAL_CLOSED_LOOP_CONTROLLER
diff --git a/Marlin/src/feature/closedloop.h b/Marlin/src/feature/closedloop.h
new file mode 100644
index 0000000..e03400c
--- /dev/null
+++ b/Marlin/src/feature/closedloop.h
@@ -0,0 +1,32 @@
+/**
+ * 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
+
+class ClosedLoop {
+public:
+  static void init();
+  static void set(const byte val);
+};
+
+extern ClosedLoop closedloop;
+
+#define CLOSED_LOOP_WAITING() (READ(CLOSED_LOOP_ENABLE_PIN) && !READ(CLOSED_LOOP_MOVE_COMPLETE_PIN))
diff --git a/Marlin/src/feature/controllerfan.cpp b/Marlin/src/feature/controllerfan.cpp
new file mode 100644
index 0000000..0206467
--- /dev/null
+++ b/Marlin/src/feature/controllerfan.cpp
@@ -0,0 +1,97 @@
+/**
+ * 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 ENABLED(USE_CONTROLLER_FAN)
+
+#include "controllerfan.h"
+#include "../module/stepper/indirection.h"
+#include "../module/temperature.h"
+
+ControllerFan controllerFan;
+
+uint8_t ControllerFan::speed;
+
+#if ENABLED(CONTROLLER_FAN_EDITABLE)
+  controllerFan_settings_t ControllerFan::settings; // {0}
+ #else
+   const controllerFan_settings_t &ControllerFan::settings = controllerFan_defaults;
+#endif
+
+void ControllerFan::setup() {
+  SET_OUTPUT(CONTROLLER_FAN_PIN);
+  init();
+}
+
+void ControllerFan::set_fan_speed(const uint8_t s) {
+  speed = s < (CONTROLLERFAN_SPEED_MIN) ? 0 : s; // Fan OFF below minimum
+}
+
+void ControllerFan::update() {
+  static millis_t lastMotorOn = 0,    // Last time a motor was turned on
+                  nextMotorCheck = 0; // Last time the state was checked
+  const millis_t ms = millis();
+  if (ELAPSED(ms, nextMotorCheck)) {
+    nextMotorCheck = ms + 2500UL; // Not a time critical function, so only check every 2.5s
+
+    #define MOTOR_IS_ON(A,B) (A##_ENABLE_READ() == bool(B##_ENABLE_ON))
+    #define _OR_ENABLED_E(N) || MOTOR_IS_ON(E##N,E)
+
+    const bool motor_on = (
+      ( DISABLED(CONTROLLER_FAN_IGNORE_Z) &&
+        (    MOTOR_IS_ON(Z,Z)
+          || TERN0(HAS_Z2_ENABLE, MOTOR_IS_ON(Z2,Z))
+          || TERN0(HAS_Z3_ENABLE, MOTOR_IS_ON(Z3,Z))
+          || TERN0(HAS_Z4_ENABLE, MOTOR_IS_ON(Z4,Z))
+        )
+      ) || (
+        DISABLED(CONTROLLER_FAN_USE_Z_ONLY) &&
+        (    MOTOR_IS_ON(X,X) || MOTOR_IS_ON(Y,Y)
+          || TERN0(HAS_X2_ENABLE, MOTOR_IS_ON(X2,X))
+          || TERN0(HAS_Y2_ENABLE, MOTOR_IS_ON(Y2,Y))
+          #if E_STEPPERS
+            REPEAT(E_STEPPERS, _OR_ENABLED_E)
+          #endif
+        )
+      )
+    );
+
+    // If any of the drivers or the heated bed are enabled...
+    if (motor_on || TERN0(HAS_HEATED_BED, thermalManager.temp_bed.soft_pwm_amount > 0))
+      lastMotorOn = ms; //... set time to NOW so the fan will turn on
+
+    // Fan Settings. Set fan > 0:
+    //  - If AutoMode is on and steppers have been enabled for CONTROLLERFAN_IDLE_TIME seconds.
+    //  - If System is on idle and idle fan speed settings is activated.
+    set_fan_speed(
+      settings.auto_mode && lastMotorOn && PENDING(ms, lastMotorOn + SEC_TO_MS(settings.duration))
+      ? settings.active_speed : settings.idle_speed
+    );
+
+    // Allow digital or PWM fan output (see M42 handling)
+    WRITE(CONTROLLER_FAN_PIN, speed);
+    analogWrite(pin_t(CONTROLLER_FAN_PIN), speed);
+  }
+}
+
+#endif // USE_CONTROLLER_FAN
diff --git a/Marlin/src/feature/controllerfan.h b/Marlin/src/feature/controllerfan.h
new file mode 100644
index 0000000..55f2d5c
--- /dev/null
+++ b/Marlin/src/feature/controllerfan.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/>.
+ *
+ */
+#pragma once
+
+#include "../inc/MarlinConfigPre.h"
+
+typedef struct {
+  uint8_t   active_speed,    // 0-255 (fullspeed); Speed with enabled stepper motors
+            idle_speed;      // 0-255 (fullspeed); Speed after idle period with all motors are disabled
+  uint16_t  duration;        // Duration in seconds for the fan to run after all motors are disabled
+  bool      auto_mode;       // Default true
+} controllerFan_settings_t;
+
+#ifndef CONTROLLERFAN_SPEED_ACTIVE
+  #define CONTROLLERFAN_SPEED_ACTIVE 255
+#endif
+#ifndef CONTROLLERFAN_SPEED_IDLE
+  #define CONTROLLERFAN_SPEED_IDLE     0
+#endif
+#ifndef CONTROLLERFAN_IDLE_TIME
+  #define CONTROLLERFAN_IDLE_TIME     60
+#endif
+
+static constexpr controllerFan_settings_t controllerFan_defaults = {
+  CONTROLLERFAN_SPEED_ACTIVE,
+  CONTROLLERFAN_SPEED_IDLE,
+  CONTROLLERFAN_IDLE_TIME,
+  true
+};
+
+#if ENABLED(USE_CONTROLLER_FAN)
+
+class ControllerFan {
+  private:
+    static uint8_t speed;
+    static void set_fan_speed(const uint8_t s);
+
+  public:
+    #if ENABLED(CONTROLLER_FAN_EDITABLE)
+      static controllerFan_settings_t settings;
+    #else
+      static const controllerFan_settings_t &settings;
+    #endif
+    static inline bool state() { return speed > 0; }
+    static inline void init() { reset(); }
+    static inline void reset() { TERN_(CONTROLLER_FAN_EDITABLE, settings = controllerFan_defaults); }
+    static void setup();
+    static void update();
+};
+
+extern ControllerFan controllerFan;
+
+#endif
diff --git a/Marlin/src/feature/dac/dac_dac084s085.cpp b/Marlin/src/feature/dac/dac_dac084s085.cpp
new file mode 100644
index 0000000..649aa55
--- /dev/null
+++ b/Marlin/src/feature/dac/dac_dac084s085.cpp
@@ -0,0 +1,98 @@
+/***************************************************************
+ *
+ * External DAC for Alligator Board
+ *
+ ****************************************************************/
+
+#include "../../inc/MarlinConfig.h"
+
+#if MB(ALLIGATOR)
+
+#include "dac_dac084s085.h"
+
+#include "../../MarlinCore.h"
+#include "../../module/stepper.h"
+#include "../../HAL/shared/Delay.h"
+
+dac084s085::dac084s085() { }
+
+void dac084s085::begin() {
+  uint8_t externalDac_buf[] = { 0x20, 0x00 }; // all off
+
+  // All SPI chip-select HIGH
+  SET_OUTPUT(DAC0_SYNC);
+  #if HAS_MULTI_EXTRUDER
+    SET_OUTPUT(DAC1_SYNC);
+  #endif
+  cshigh();
+  spiBegin();
+
+  //init onboard DAC
+  DELAY_US(2);
+  WRITE(DAC0_SYNC, LOW);
+  DELAY_US(2);
+  WRITE(DAC0_SYNC, HIGH);
+  DELAY_US(2);
+  WRITE(DAC0_SYNC, LOW);
+
+  spiSend(SPI_CHAN_DAC, externalDac_buf, COUNT(externalDac_buf));
+  WRITE(DAC0_SYNC, HIGH);
+
+  #if HAS_MULTI_EXTRUDER
+    //init Piggy DAC
+    DELAY_US(2);
+    WRITE(DAC1_SYNC, LOW);
+    DELAY_US(2);
+    WRITE(DAC1_SYNC, HIGH);
+    DELAY_US(2);
+    WRITE(DAC1_SYNC, LOW);
+
+    spiSend(SPI_CHAN_DAC, externalDac_buf, COUNT(externalDac_buf));
+    WRITE(DAC1_SYNC, HIGH);
+  #endif
+
+  return;
+}
+
+void dac084s085::setValue(const uint8_t channel, const uint8_t value) {
+  if (channel >= 7) return; // max channel (X,Y,Z,E0,E1,E2,E3)
+
+  const uint8_t externalDac_buf[] = {
+    0x10 | ((channel > 3 ? 7 : 3) - channel << 6) | (value >> 4),
+    0x00 | (value << 4)
+  };
+
+  // All SPI chip-select HIGH
+  cshigh();
+
+  if (channel > 3) {        // DAC Piggy E1,E2,E3
+    WRITE(DAC1_SYNC, LOW);
+    DELAY_US(2);
+    WRITE(DAC1_SYNC, HIGH);
+    DELAY_US(2);
+    WRITE(DAC1_SYNC, LOW);
+  }
+  else {                    // DAC onboard X,Y,Z,E0
+    WRITE(DAC0_SYNC, LOW);
+    DELAY_US(2);
+    WRITE(DAC0_SYNC, HIGH);
+    DELAY_US(2);
+    WRITE(DAC0_SYNC, LOW);
+  }
+
+  DELAY_US(2);
+  spiSend(SPI_CHAN_DAC, externalDac_buf, COUNT(externalDac_buf));
+}
+
+void dac084s085::cshigh() {
+  WRITE(DAC0_SYNC, HIGH);
+  #if HAS_MULTI_EXTRUDER
+    WRITE(DAC1_SYNC, HIGH);
+  #endif
+  WRITE(SPI_EEPROM1_CS, HIGH);
+  WRITE(SPI_EEPROM2_CS, HIGH);
+  WRITE(SPI_FLASH_CS, HIGH);
+  WRITE(SD_SS_PIN, HIGH);
+}
+
+#endif // MB(ALLIGATOR)
diff --git a/Marlin/src/feature/dac/dac_dac084s085.h b/Marlin/src/feature/dac/dac_dac084s085.h
new file mode 100644
index 0000000..5be0129
--- /dev/null
+++ b/Marlin/src/feature/dac/dac_dac084s085.h
@@ -0,0 +1,31 @@
+/**
+ * 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
+
+class dac084s085 {
+  public:
+    dac084s085();
+    static void begin();
+    static void setValue(const uint8_t channel, const uint8_t value);
+  private:
+    static void cshigh();
+};
diff --git a/Marlin/src/feature/dac/dac_mcp4728.cpp b/Marlin/src/feature/dac/dac_mcp4728.cpp
new file mode 100644
index 0000000..4f33c4e
--- /dev/null
+++ b/Marlin/src/feature/dac/dac_mcp4728.cpp
@@ -0,0 +1,154 @@
+/**
+ * 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/>.
+ *
+ */
+
+/**
+ * mcp4728.cpp - Arduino library for MicroChip MCP4728 I2C D/A converter
+ *
+ * For implementation details, please take a look at the datasheet:
+ * https://ww1.microchip.com/downloads/en/DeviceDoc/22187a.pdf
+ *
+ * For discussion and feedback, please go to:
+ * https://forum.arduino.cc/index.php/topic,51842.0.html
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(HAS_MOTOR_CURRENT_DAC)
+
+#include "dac_mcp4728.h"
+
+MCP4728 mcp4728;
+
+xyze_uint_t dac_values;
+
+/**
+ * Begin I2C, get current values (input register and eeprom) of mcp4728
+ */
+void MCP4728::init() {
+  Wire.begin();
+  Wire.requestFrom(I2C_ADDRESS(DAC_DEV_ADDRESS), uint8_t(24));
+  while (Wire.available()) {
+    char deviceID = Wire.read(),
+         hiByte = Wire.read(),
+         loByte = Wire.read();
+
+    if (!(deviceID & 0x08))
+      dac_values[(deviceID & 0x30) >> 4] = word((hiByte & 0x0F), loByte);
+  }
+}
+
+/**
+ * Write input resister value to specified channel using fastwrite method.
+ * Channel : 0-3, Values : 0-4095
+ */
+uint8_t MCP4728::analogWrite(const uint8_t channel, const uint16_t value) {
+  dac_values[channel] = value;
+  return fastWrite();
+}
+
+/**
+ * Write all input resistor values to EEPROM using SequencialWrite method.
+ * This will update both input register and EEPROM value
+ * This will also write current Vref, PowerDown, Gain settings to EEPROM
+ */
+uint8_t MCP4728::eepromWrite() {
+  Wire.beginTransmission(I2C_ADDRESS(DAC_DEV_ADDRESS));
+  Wire.write(SEQWRITE);
+  LOOP_XYZE(i) {
+    Wire.write(DAC_STEPPER_VREF << 7 | DAC_STEPPER_GAIN << 4 | highByte(dac_values[i]));
+    Wire.write(lowByte(dac_values[i]));
+  }
+  return Wire.endTransmission();
+}
+
+/**
+ * Write Voltage reference setting to all input regiters
+ */
+uint8_t MCP4728::setVref_all(const uint8_t value) {
+  Wire.beginTransmission(I2C_ADDRESS(DAC_DEV_ADDRESS));
+  Wire.write(VREFWRITE | (value ? 0x0F : 0x00));
+  return Wire.endTransmission();
+}
+/**
+ * Write Gain setting to all input regiters
+ */
+uint8_t MCP4728::setGain_all(const uint8_t value) {
+  Wire.beginTransmission(I2C_ADDRESS(DAC_DEV_ADDRESS));
+  Wire.write(GAINWRITE | (value ? 0x0F : 0x00));
+  return Wire.endTransmission();
+}
+
+/**
+ * Return Input Register value
+ */
+uint16_t MCP4728::getValue(const uint8_t channel) { return dac_values[channel]; }
+
+#if 0
+/**
+ * Steph: Might be useful in the future
+ * Return Vout
+ */
+uint16_t MCP4728::getVout(const uint8_t channel) {
+  const uint32_t vref = 2048,
+                 vOut = (vref * dac_values[channel] * (_DAC_STEPPER_GAIN + 1)) / 4096;
+  return _MIN(vOut, defaultVDD);
+}
+#endif
+
+/**
+ * Returns DAC values as a 0-100 percentage of drive strength
+ */
+uint8_t MCP4728::getDrvPct(const uint8_t channel) { return uint8_t(100.0 * dac_values[channel] / (DAC_STEPPER_MAX) + 0.5); }
+
+/**
+ * Receives all Drive strengths as 0-100 percent values, updates
+ * DAC Values array and calls fastwrite to update the DAC.
+ */
+void MCP4728::setDrvPct(xyze_uint_t &pct) {
+  dac_values = pct * (DAC_STEPPER_MAX) * 0.01f;
+  fastWrite();
+}
+
+/**
+ * FastWrite input register values - All DAC ouput update. refer to DATASHEET 5.6.1
+ * DAC Input and PowerDown bits update.
+ * No EEPROM update
+ */
+uint8_t MCP4728::fastWrite() {
+  Wire.beginTransmission(I2C_ADDRESS(DAC_DEV_ADDRESS));
+  LOOP_XYZE(i) {
+    Wire.write(highByte(dac_values[i]));
+    Wire.write(lowByte(dac_values[i]));
+  }
+  return Wire.endTransmission();
+}
+
+/**
+ * Common function for simple general commands
+ */
+uint8_t MCP4728::simpleCommand(const byte simpleCommand) {
+  Wire.beginTransmission(I2C_ADDRESS(GENERALCALL));
+  Wire.write(simpleCommand);
+  return Wire.endTransmission();
+}
+
+#endif // HAS_MOTOR_CURRENT_DAC
diff --git a/Marlin/src/feature/dac/dac_mcp4728.h b/Marlin/src/feature/dac/dac_mcp4728.h
new file mode 100644
index 0000000..3a7d5f1
--- /dev/null
+++ b/Marlin/src/feature/dac/dac_mcp4728.h
@@ -0,0 +1,82 @@
+/**
+ * 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
+
+/**
+ * Arduino library for MicroChip MCP4728 I2C D/A converter.
+ */
+
+#include "../../core/types.h"
+
+#include <Wire.h>
+
+/**
+ * The following three macros are only used in this piece of code related to mcp4728.
+ * They are defined in the standard Arduino framework but could be undefined in 32 bits Arduino frameworks.
+ * (For instance not defined in Arduino lpc176x framework)
+ * So we have to define them if needed.
+ */
+#ifndef word
+  #define word(h, l)  ((uint8_t) ((h << 8) | l))
+#endif
+
+#ifndef lowByte
+  #define lowByte(w)  ((uint8_t) ((w) & 0xFF))
+#endif
+
+#ifndef highByte
+  #define highByte(w) ((uint8_t) ((w) >> 8))
+#endif
+
+#define defaultVDD     DAC_STEPPER_MAX //was 5000 but differs with internal Vref
+#define BASE_ADDR      0x60
+#define RESET          0b00000110
+#define WAKE           0b00001001
+#define UPDATE         0b00001000
+#define MULTIWRITE     0b01000000
+#define SINGLEWRITE    0b01011000
+#define SEQWRITE       0b01010000
+#define VREFWRITE      0b10000000
+#define GAINWRITE      0b11000000
+#define POWERDOWNWRITE 0b10100000
+#define GENERALCALL    0b00000000
+#define GAINWRITE      0b11000000
+
+// This is taken from the original lib, makes it easy to edit if needed
+// DAC_OR_ADDRESS defined in pins_BOARD.h  file
+#define DAC_DEV_ADDRESS (BASE_ADDR | DAC_OR_ADDRESS)
+
+class MCP4728 {
+public:
+  static void     init();
+  static uint8_t  analogWrite(const uint8_t channel, const uint16_t value);
+  static uint8_t  eepromWrite();
+  static uint8_t  setVref_all(const uint8_t value);
+  static uint8_t  setGain_all(const uint8_t value);
+  static uint16_t getValue(const uint8_t channel);
+  static uint8_t  fastWrite();
+  static uint8_t  simpleCommand(const byte simpleCommand);
+  static uint8_t  getDrvPct(const uint8_t channel);
+  static void     setDrvPct(xyze_uint_t &pct);
+};
+
+extern MCP4728 mcp4728;
diff --git a/Marlin/src/feature/dac/stepper_dac.cpp b/Marlin/src/feature/dac/stepper_dac.cpp
new file mode 100644
index 0000000..5170a35
--- /dev/null
+++ b/Marlin/src/feature/dac/stepper_dac.cpp
@@ -0,0 +1,99 @@
+/**
+ * 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_dac.cpp - To set stepper current via DAC
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(HAS_MOTOR_CURRENT_DAC)
+
+#include "stepper_dac.h"
+#include "../../MarlinCore.h" // for SP_X_LBL...
+
+bool dac_present = false;
+constexpr xyze_uint8_t dac_order = DAC_STEPPER_ORDER;
+xyze_uint_t dac_channel_pct = DAC_MOTOR_CURRENT_DEFAULT;
+
+StepperDAC stepper_dac;
+
+int StepperDAC::init() {
+  #if PIN_EXISTS(DAC_DISABLE)
+    OUT_WRITE(DAC_DISABLE_PIN, LOW);  // set pin low to enable DAC
+  #endif
+
+  mcp4728.init();
+
+  if (mcp4728.simpleCommand(RESET)) return -1;
+
+  dac_present = true;
+
+  mcp4728.setVref_all(DAC_STEPPER_VREF);
+  mcp4728.setGain_all(DAC_STEPPER_GAIN);
+
+  if (mcp4728.getDrvPct(0) < 1 || mcp4728.getDrvPct(1) < 1 || mcp4728.getDrvPct(2) < 1 || mcp4728.getDrvPct(3) < 1 ) {
+    mcp4728.setDrvPct(dac_channel_pct);
+    mcp4728.eepromWrite();
+  }
+
+  return 0;
+}
+
+void StepperDAC::set_current_value(const uint8_t channel, uint16_t val) {
+  if (!dac_present) return;
+
+  NOMORE(val, uint16_t(DAC_STEPPER_MAX));
+
+  mcp4728.analogWrite(dac_order[channel], val);
+  mcp4728.simpleCommand(UPDATE);
+}
+
+void StepperDAC::set_current_percent(const uint8_t channel, float val) {
+  set_current_value(channel, _MIN(val, 100.0f) * (DAC_STEPPER_MAX) / 100.0f);
+}
+
+static float dac_perc(int8_t n) { return mcp4728.getDrvPct(dac_order[n]); }
+static float dac_amps(int8_t n) { return mcp4728.getValue(dac_order[n]) * 0.125 * RECIPROCAL(DAC_STEPPER_SENSE * 1000); }
+
+uint8_t StepperDAC::get_current_percent(const AxisEnum axis) { return mcp4728.getDrvPct(dac_order[axis]); }
+void StepperDAC::set_current_percents(xyze_uint8_t &pct) {
+  LOOP_XYZE(i) dac_channel_pct[i] = pct[dac_order[i]];
+  mcp4728.setDrvPct(dac_channel_pct);
+}
+
+void StepperDAC::print_values() {
+  if (!dac_present) return;
+  SERIAL_ECHO_MSG("Stepper current values in % (Amps):");
+  SERIAL_ECHO_START();
+  SERIAL_ECHOPAIR_P(  SP_X_LBL, dac_perc(X_AXIS), PSTR(" ("), dac_amps(X_AXIS), PSTR(")"));
+  SERIAL_ECHOPAIR_P(  SP_Y_LBL, dac_perc(Y_AXIS), PSTR(" ("), dac_amps(Y_AXIS), PSTR(")"));
+  SERIAL_ECHOPAIR_P(  SP_Z_LBL, dac_perc(Z_AXIS), PSTR(" ("), dac_amps(Z_AXIS), PSTR(")"));
+  SERIAL_ECHOLNPAIR_P(SP_E_LBL, dac_perc(E_AXIS), PSTR(" ("), dac_amps(E_AXIS), PSTR(")"));
+}
+
+void StepperDAC::commit_eeprom() {
+  if (!dac_present) return;
+  mcp4728.eepromWrite();
+}
+
+#endif // HAS_MOTOR_CURRENT_DAC
diff --git a/Marlin/src/feature/dac/stepper_dac.h b/Marlin/src/feature/dac/stepper_dac.h
new file mode 100644
index 0000000..6836335
--- /dev/null
+++ b/Marlin/src/feature/dac/stepper_dac.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
+
+/**
+ * stepper_dac.h   - To set stepper current via DAC
+ */
+
+#include "dac_mcp4728.h"
+
+class StepperDAC {
+public:
+  static int init();
+  static void set_current_percent(const uint8_t channel, float val);
+  static void set_current_value(const uint8_t channel, uint16_t val);
+  static void print_values();
+  static void commit_eeprom();
+  static uint8_t get_current_percent(AxisEnum axis);
+  static void set_current_percents(xyze_uint8_t &pct);
+};
+
+extern StepperDAC stepper_dac;
diff --git a/Marlin/src/feature/digipot/digipot.h b/Marlin/src/feature/digipot/digipot.h
new file mode 100644
index 0000000..3fbd1f3
--- /dev/null
+++ b/Marlin/src/feature/digipot/digipot.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
+
+//
+// Header for MCP4018 and MCP4451 current control i2c devices
+//
+class DigipotI2C {
+public:
+  static void init();
+  static void set_current(const uint8_t channel, const float current);
+};
+
+extern DigipotI2C digipot_i2c;
diff --git a/Marlin/src/feature/digipot/digipot_mcp4018.cpp b/Marlin/src/feature/digipot/digipot_mcp4018.cpp
new file mode 100644
index 0000000..37853ff
--- /dev/null
+++ b/Marlin/src/feature/digipot/digipot_mcp4018.cpp
@@ -0,0 +1,104 @@
+/**
+ * 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 ENABLED(DIGIPOT_MCP4018)
+
+#include "digipot.h"
+
+#include <Stream.h>
+#include <SlowSoftI2CMaster.h>  // https://github.com/felias-fogg/SlowSoftI2CMaster
+
+// Settings for the I2C based DIGIPOT (MCP4018) based on WT150
+
+#define DIGIPOT_A4988_Rsx               0.250
+#define DIGIPOT_A4988_Vrefmax           1.666
+#define DIGIPOT_MCP4018_MAX_VALUE     127
+
+#define DIGIPOT_A4988_Itripmax(Vref)    ((Vref) / (8.0 * DIGIPOT_A4988_Rsx))
+
+#define DIGIPOT_A4988_FACTOR            ((DIGIPOT_MCP4018_MAX_VALUE) / DIGIPOT_A4988_Itripmax(DIGIPOT_A4988_Vrefmax))
+#define DIGIPOT_A4988_MAX_CURRENT       2.0
+
+static byte current_to_wiper(const float current) {
+  const int16_t value = TERN(DIGIPOT_USE_RAW_VALUES, current, CEIL(current * DIGIPOT_A4988_FACTOR));
+  return byte(constrain(value, 0, DIGIPOT_MCP4018_MAX_VALUE));
+}
+
+static SlowSoftI2CMaster pots[DIGIPOT_I2C_NUM_CHANNELS] = {
+  SlowSoftI2CMaster(DIGIPOTS_I2C_SDA_X, DIGIPOTS_I2C_SCL, ENABLED(DIGIPOT_ENABLE_I2C_PULLUPS))
+  #if DIGIPOT_I2C_NUM_CHANNELS > 1
+    , SlowSoftI2CMaster(DIGIPOTS_I2C_SDA_Y, DIGIPOTS_I2C_SCL, ENABLED(DIGIPOT_ENABLE_I2C_PULLUPS))
+    #if DIGIPOT_I2C_NUM_CHANNELS > 2
+      , SlowSoftI2CMaster(DIGIPOTS_I2C_SDA_Z, DIGIPOTS_I2C_SCL, ENABLED(DIGIPOT_ENABLE_I2C_PULLUPS))
+      #if DIGIPOT_I2C_NUM_CHANNELS > 3
+        , SlowSoftI2CMaster(DIGIPOTS_I2C_SDA_E0, DIGIPOTS_I2C_SCL, ENABLED(DIGIPOT_ENABLE_I2C_PULLUPS))
+        #if DIGIPOT_I2C_NUM_CHANNELS > 4
+          , SlowSoftI2CMaster(DIGIPOTS_I2C_SDA_E1, DIGIPOTS_I2C_SCL, ENABLED(DIGIPOT_ENABLE_I2C_PULLUPS))
+          #if DIGIPOT_I2C_NUM_CHANNELS > 5
+            , SlowSoftI2CMaster(DIGIPOTS_I2C_SDA_E2, DIGIPOTS_I2C_SCL, ENABLED(DIGIPOT_ENABLE_I2C_PULLUPS))
+            #if DIGIPOT_I2C_NUM_CHANNELS > 6
+              , SlowSoftI2CMaster(DIGIPOTS_I2C_SDA_E3, DIGIPOTS_I2C_SCL, ENABLED(DIGIPOT_ENABLE_I2C_PULLUPS))
+              #if DIGIPOT_I2C_NUM_CHANNELS > 7
+                , SlowSoftI2CMaster(DIGIPOTS_I2C_SDA_E4, DIGIPOTS_I2C_SCL, ENABLED(DIGIPOT_ENABLE_I2C_PULLUPS))
+              #endif
+            #endif
+          #endif
+        #endif
+      #endif
+    #endif
+  #endif
+};
+
+static void digipot_i2c_send(const uint8_t channel, const byte v) {
+  if (WITHIN(channel, 0, DIGIPOT_I2C_NUM_CHANNELS - 1)) {
+    pots[channel].i2c_start(((DIGIPOT_I2C_ADDRESS_A) << 1) | I2C_WRITE);
+    pots[channel].i2c_write(v);
+    pots[channel].i2c_stop();
+  }
+}
+
+// This is for the MCP4018 I2C based digipot
+void DigipotI2C::set_current(const uint8_t channel, const float current) {
+  const float ival = _MIN(_MAX(current, 0), float(DIGIPOT_MCP4018_MAX_VALUE));
+  digipot_i2c_send(channel, current_to_wiper(ival));
+}
+
+void DigipotI2C::init() {
+  LOOP_L_N(i, DIGIPOT_I2C_NUM_CHANNELS) pots[i].i2c_init();
+
+  // Init currents according to Configuration_adv.h
+  static const float digipot_motor_current[] PROGMEM =
+    #if ENABLED(DIGIPOT_USE_RAW_VALUES)
+      DIGIPOT_MOTOR_CURRENT
+    #else
+      DIGIPOT_I2C_MOTOR_CURRENTS
+    #endif
+  ;
+  LOOP_L_N(i, COUNT(digipot_motor_current))
+    set_current(i, pgm_read_float(&digipot_motor_current[i]));
+}
+
+DigipotI2C digipot_i2c;
+
+#endif // DIGIPOT_MCP4018
diff --git a/Marlin/src/feature/digipot/digipot_mcp4451.cpp b/Marlin/src/feature/digipot/digipot_mcp4451.cpp
new file mode 100644
index 0000000..1b4cf43
--- /dev/null
+++ b/Marlin/src/feature/digipot/digipot_mcp4451.cpp
@@ -0,0 +1,100 @@
+/**
+ * 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 ENABLED(DIGIPOT_MCP4451)
+
+#include "digipot.h"
+
+#include <Stream.h>
+#include <Wire.h>
+
+#if MB(MKS_SBASE)
+  #include "digipot_mcp4451_I2C_routines.h"
+#endif
+
+// Settings for the I2C based DIGIPOT (MCP4451) on Azteeg X3 Pro
+#if MB(5DPRINT)
+  #define DIGIPOT_I2C_FACTOR      117.96f
+  #define DIGIPOT_I2C_MAX_CURRENT   1.736f
+#elif MB(AZTEEG_X5_MINI, AZTEEG_X5_MINI_WIFI)
+  #define DIGIPOT_I2C_FACTOR      113.5f
+  #define DIGIPOT_I2C_MAX_CURRENT   2.0f
+#else
+  #define DIGIPOT_I2C_FACTOR      106.7f
+  #define DIGIPOT_I2C_MAX_CURRENT   2.5f
+#endif
+
+static byte current_to_wiper(const float current) {
+  return byte(TERN(DIGIPOT_USE_RAW_VALUES, current, CEIL(DIGIPOT_I2C_FACTOR * current)));
+}
+
+static void digipot_i2c_send(const byte addr, const byte a, const byte b) {
+  #if MB(MKS_SBASE)
+    digipot_mcp4451_start(addr);
+    digipot_mcp4451_send_byte(a);
+    digipot_mcp4451_send_byte(b);
+  #else
+    Wire.beginTransmission(I2C_ADDRESS(addr));
+    Wire.write(a);
+    Wire.write(b);
+    Wire.endTransmission();
+  #endif
+}
+
+// This is for the MCP4451 I2C based digipot
+void DigipotI2C::set_current(const uint8_t channel, const float current) {
+  // These addresses are specific to Azteeg X3 Pro, can be set to others.
+  // In this case first digipot is at address A0=0, A1=0, second one is at A0=0, A1=1
+  const byte addr = channel < 4 ? DIGIPOT_I2C_ADDRESS_A : DIGIPOT_I2C_ADDRESS_B; // channel 0-3 vs 4-7
+
+  // Initial setup
+  digipot_i2c_send(addr, 0x40, 0xFF);
+  digipot_i2c_send(addr, 0xA0, 0xFF);
+
+  // Set actual wiper value
+  byte addresses[4] = { 0x00, 0x10, 0x60, 0x70 };
+  digipot_i2c_send(addr, addresses[channel & 0x3], current_to_wiper(_MIN(float(_MAX(current, 0)), DIGIPOT_I2C_MAX_CURRENT)));
+}
+
+void DigipotI2C::init() {
+  #if MB(MKS_SBASE)
+    configure_i2c(16); // Set clock_option to 16 ensure I2C is initialized at 400kHz
+  #else
+    Wire.begin();
+  #endif
+  // Set up initial currents as defined in Configuration_adv.h
+  static const float digipot_motor_current[] PROGMEM =
+    #if ENABLED(DIGIPOT_USE_RAW_VALUES)
+      DIGIPOT_MOTOR_CURRENT
+    #else
+      DIGIPOT_I2C_MOTOR_CURRENTS
+    #endif
+  ;
+  LOOP_L_N(i, COUNT(digipot_motor_current))
+    set_current(i, pgm_read_float(&digipot_motor_current[i]));
+}
+
+DigipotI2C digipot_i2c;
+
+#endif // DIGIPOT_MCP4451
diff --git a/Marlin/src/feature/direct_stepping.cpp b/Marlin/src/feature/direct_stepping.cpp
new file mode 100644
index 0000000..9766d14
--- /dev/null
+++ b/Marlin/src/feature/direct_stepping.cpp
@@ -0,0 +1,262 @@
+/**
+ * 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"
+
+#if ENABLED(DIRECT_STEPPING)
+
+#include "direct_stepping.h"
+
+#include "../MarlinCore.h"
+
+#define CHECK_PAGE(I, R) do{                                \
+  if (I >= sizeof(page_states) / sizeof(page_states[0])) {  \
+    fatal_error = true;                                     \
+    return R;                                               \
+  }                                                         \
+}while(0)
+
+#define CHECK_PAGE_STATE(I, R, S) do { \
+  CHECK_PAGE(I, R);                    \
+  if (page_states[I] != S) {           \
+    fatal_error = true;                \
+    return R;                          \
+  }                                    \
+}while(0)
+
+namespace DirectStepping {
+
+  template<typename Cfg>
+  State SerialPageManager<Cfg>::state;
+
+  template<typename Cfg>
+  volatile bool SerialPageManager<Cfg>::fatal_error;
+
+  template<typename Cfg>
+  volatile PageState SerialPageManager<Cfg>::page_states[Cfg::NUM_PAGES];
+
+  template<typename Cfg>
+  volatile bool SerialPageManager<Cfg>::page_states_dirty;
+
+  template<typename Cfg>
+  uint8_t SerialPageManager<Cfg>::pages[Cfg::NUM_PAGES][Cfg::PAGE_SIZE];
+
+  template<typename Cfg>
+  uint8_t SerialPageManager<Cfg>::checksum;
+
+  template<typename Cfg>
+  typename Cfg::write_byte_idx_t SerialPageManager<Cfg>::write_byte_idx;
+
+  template<typename Cfg>
+  typename Cfg::page_idx_t SerialPageManager<Cfg>::write_page_idx;
+
+  template<typename Cfg>
+  typename Cfg::write_byte_idx_t SerialPageManager<Cfg>::write_page_size;
+
+  template <typename Cfg>
+  void SerialPageManager<Cfg>::init() {
+    for (int i = 0 ; i < Cfg::NUM_PAGES ; i++)
+      page_states[i] = PageState::FREE;
+
+    fatal_error = false;
+    state = State::NEWLINE;
+
+    page_states_dirty = false;
+
+    SERIAL_ECHOLNPGM("pages_ready");
+  }
+
+  template<typename Cfg>
+  FORCE_INLINE bool SerialPageManager<Cfg>::maybe_store_rxd_char(uint8_t c) {
+    switch (state) {
+      default:
+      case State::MONITOR:
+        switch (c) {
+          case '\n':
+          case '\r':
+            state = State::NEWLINE;
+          default:
+            return false;
+        }
+      case State::NEWLINE:
+        switch (c) {
+          case Cfg::CONTROL_CHAR:
+            state = State::ADDRESS;
+            return true;
+          case '\n':
+          case '\r':
+            state = State::NEWLINE;
+            return false;
+          default:
+            state = State::MONITOR;
+            return false;
+        }
+      case State::ADDRESS:
+        //TODO: 16 bit address, State::ADDRESS2
+        write_page_idx = c;
+        write_byte_idx = 0;
+        checksum = 0;
+
+        CHECK_PAGE(write_page_idx, true);
+
+        if (page_states[write_page_idx] == PageState::FAIL) {
+          // Special case for fail
+          state = State::UNFAIL;
+          return true;
+        }
+
+        set_page_state(write_page_idx, PageState::WRITING);
+
+        state = Cfg::DIRECTIONAL ? State::COLLECT : State::SIZE;
+
+        return true;
+      case State::SIZE:
+        // Zero means full page size
+        write_page_size = c;
+        state = State::COLLECT;
+        return true;
+      case State::COLLECT:
+        pages[write_page_idx][write_byte_idx++] = c;
+        checksum ^= c;
+
+        // check if still collecting
+        if (Cfg::PAGE_SIZE == 256) {
+          // special case for 8-bit, check if rolled back to 0
+          if (Cfg::DIRECTIONAL || !write_page_size) { // full 256 bytes
+            if (write_byte_idx) return true;
+          } else {
+            if (write_byte_idx < write_page_size) return true;
+          }
+        } else if (Cfg::DIRECTIONAL) {
+          if (write_byte_idx != Cfg::PAGE_SIZE) return true;
+        } else {
+          if (write_byte_idx < write_page_size) return true;
+        }
+
+        state = State::CHECKSUM;
+        return true;
+      case State::CHECKSUM: {
+        const PageState page_state = (checksum == c) ? PageState::OK : PageState::FAIL;
+        set_page_state(write_page_idx, page_state);
+        state = State::MONITOR;
+        return true;
+      }
+      case State::UNFAIL:
+        if (c == 0) {
+          set_page_state(write_page_idx, PageState::FREE);
+        } else {
+          fatal_error = true;
+        }
+        state = State::MONITOR;
+        return true;
+    }
+  }
+
+  template <typename Cfg>
+  void SerialPageManager<Cfg>::write_responses() {
+    if (fatal_error) {
+      kill(GET_TEXT(MSG_BAD_PAGE));
+      return;
+    }
+
+    if (!page_states_dirty) return;
+    page_states_dirty = false;
+
+    SERIAL_ECHO(Cfg::CONTROL_CHAR);
+    constexpr int state_bits = 2;
+    constexpr int n_bytes = Cfg::NUM_PAGES >> state_bits;
+    volatile uint8_t bits_b[n_bytes] = { 0 };
+
+    for (page_idx_t i = 0 ; i < Cfg::NUM_PAGES ; i++) {
+      bits_b[i >> state_bits] |= page_states[i] << ((i * state_bits) & 0x7);
+    }
+
+    uint8_t crc = 0;
+    for (uint8_t i = 0 ; i < n_bytes ; i++) {
+      crc ^= bits_b[i];
+      SERIAL_ECHO(bits_b[i]);
+    }
+
+    SERIAL_ECHO(crc);
+    SERIAL_EOL();
+  }
+
+  template <typename Cfg>
+  FORCE_INLINE void SerialPageManager<Cfg>::set_page_state(const page_idx_t page_idx, const PageState page_state) {
+    CHECK_PAGE(page_idx,);
+
+    page_states[page_idx] = page_state;
+    page_states_dirty = true;
+  }
+
+  template <>
+  FORCE_INLINE uint8_t *PageManager::get_page(const page_idx_t page_idx) {
+    CHECK_PAGE(page_idx, nullptr);
+
+    return pages[page_idx];
+  }
+
+  template <>
+  FORCE_INLINE void PageManager::free_page(const page_idx_t page_idx) {
+    set_page_state(page_idx, PageState::FREE);
+  }
+
+};
+
+DirectStepping::PageManager page_manager;
+
+const uint8_t segment_table[DirectStepping::Config::NUM_SEGMENTS][DirectStepping::Config::SEGMENT_STEPS] PROGMEM = {
+
+  #if STEPPER_PAGE_FORMAT == SP_4x4D_128
+
+    { 1, 1, 1, 1, 1, 1, 1 }, //  0 = -7
+    { 1, 1, 1, 0, 1, 1, 1 }, //  1 = -6
+    { 1, 1, 1, 0, 1, 0, 1 }, //  2 = -5
+    { 1, 1, 0, 1, 0, 1, 0 }, //  3 = -4
+    { 1, 1, 0, 0, 1, 0, 0 }, //  4 = -3
+    { 0, 0, 1, 0, 0, 0, 1 }, //  5 = -2
+    { 0, 0, 0, 1, 0, 0, 0 }, //  6 = -1
+    { 0, 0, 0, 0, 0, 0, 0 }, //  7 =  0
+    { 0, 0, 0, 1, 0, 0, 0 }, //  8 =  1
+    { 0, 0, 1, 0, 0, 0, 1 }, //  9 =  2
+    { 1, 1, 0, 0, 1, 0, 0 }, // 10 =  3
+    { 1, 1, 0, 1, 0, 1, 0 }, // 11 =  4
+    { 1, 1, 1, 0, 1, 0, 1 }, // 12 =  5
+    { 1, 1, 1, 0, 1, 1, 1 }, // 13 =  6
+    { 1, 1, 1, 1, 1, 1, 1 }, // 14 =  7
+    { 0 }
+
+  #elif STEPPER_PAGE_FORMAT == SP_4x2_256
+
+    { 0, 0, 0 }, // 0
+    { 0, 1, 0 }, // 1
+    { 1, 0, 1 }, // 2
+    { 1, 1, 1 }, // 3
+
+  #elif STEPPER_PAGE_FORMAT == SP_4x1_512
+
+    {0} // Uncompressed format, table not used
+
+  #endif
+
+};
+
+#endif // DIRECT_STEPPING
diff --git a/Marlin/src/feature/direct_stepping.h b/Marlin/src/feature/direct_stepping.h
new file mode 100644
index 0000000..b300773
--- /dev/null
+++ b/Marlin/src/feature/direct_stepping.h
@@ -0,0 +1,133 @@
+/**
+ * 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"
+
+namespace DirectStepping {
+
+  enum State : char {
+    MONITOR, NEWLINE, ADDRESS, SIZE, COLLECT, CHECKSUM, UNFAIL
+  };
+
+  enum PageState : uint8_t {
+    FREE, WRITING, OK, FAIL
+  };
+
+  // Static state used for stepping through direct stepping pages
+  struct page_step_state_t {
+    // Current page
+    uint8_t *page;
+    // Current segment
+    uint16_t segment_idx;
+    // Current steps within segment
+    uint8_t segment_steps;
+    // Segment delta
+    xyze_uint8_t sd;
+    // Block delta
+    xyze_int_t bd;
+  };
+
+  template<typename Cfg>
+  class SerialPageManager {
+  public:
+
+    typedef typename Cfg::page_idx_t page_idx_t;
+
+    static bool maybe_store_rxd_char(uint8_t c);
+    static void write_responses();
+
+    // common methods for page managers
+    static void init();
+    static uint8_t *get_page(const page_idx_t page_idx);
+    static void free_page(const page_idx_t page_idx);
+
+  protected:
+
+    typedef typename Cfg::write_byte_idx_t write_byte_idx_t;
+
+    static State state;
+    static volatile bool fatal_error;
+
+    static volatile PageState page_states[Cfg::NUM_PAGES];
+    static volatile bool page_states_dirty;
+
+    static uint8_t pages[Cfg::NUM_PAGES][Cfg::PAGE_SIZE];
+    static uint8_t checksum;
+    static write_byte_idx_t write_byte_idx;
+    static page_idx_t write_page_idx;
+    static write_byte_idx_t write_page_size;
+
+    static void set_page_state(const page_idx_t page_idx, const PageState page_state);
+  };
+
+  template<bool b, typename T, typename F> struct TypeSelector { typedef T type;} ;
+  template<typename T, typename F> struct TypeSelector<false, T, F> { typedef F type; };
+
+  template <int num_pages, int num_axes, int bits_segment, bool dir, int segments>
+  struct config_t {
+    static constexpr char CONTROL_CHAR  = '!';
+
+    static constexpr int NUM_PAGES      = num_pages;
+    static constexpr int NUM_AXES       = num_axes;
+    static constexpr int BITS_SEGMENT   = bits_segment;
+    static constexpr int DIRECTIONAL    = dir ? 1 : 0;
+    static constexpr int SEGMENTS       = segments;
+
+    static constexpr int NUM_SEGMENTS   = _BV(BITS_SEGMENT);
+    static constexpr int SEGMENT_STEPS  = _BV(BITS_SEGMENT - DIRECTIONAL) - 1;
+    static constexpr int TOTAL_STEPS    = SEGMENT_STEPS * SEGMENTS;
+    static constexpr int PAGE_SIZE      = (NUM_AXES * BITS_SEGMENT * SEGMENTS) / 8;
+
+    typedef typename TypeSelector<(PAGE_SIZE>256), uint16_t, uint8_t>::type write_byte_idx_t;
+    typedef typename TypeSelector<(NUM_PAGES>256), uint16_t, uint8_t>::type page_idx_t;
+  };
+
+  template <uint8_t num_pages>
+  using SP_4x4D_128 = config_t<num_pages, 4, 4, true,  128>;
+
+  template <uint8_t num_pages>
+  using SP_4x2_256  = config_t<num_pages, 4, 2, false, 256>;
+
+  template <uint8_t num_pages>
+  using SP_4x1_512  = config_t<num_pages, 4, 1, false, 512>;
+
+  // configured types
+  typedef STEPPER_PAGE_FORMAT<STEPPER_PAGES> Config;
+
+  template class PAGE_MANAGER<Config>;
+  typedef PAGE_MANAGER<Config> PageManager;
+};
+
+#define SP_4x4D_128 1
+//#define SP_4x4_128 2
+//#define SP_4x2D_256 3
+#define SP_4x2_256 4
+#define SP_4x1_512 5
+
+typedef typename DirectStepping::Config::page_idx_t page_idx_t;
+
+// TODO: use config
+typedef DirectStepping::page_step_state_t page_step_state_t;
+
+extern const uint8_t segment_table[DirectStepping::Config::NUM_SEGMENTS][DirectStepping::Config::SEGMENT_STEPS];
+extern DirectStepping::PageManager page_manager;
diff --git a/Marlin/src/feature/e_parser.cpp b/Marlin/src/feature/e_parser.cpp
new file mode 100644
index 0000000..d98afcf
--- /dev/null
+++ b/Marlin/src/feature/e_parser.cpp
@@ -0,0 +1,45 @@
+/**
+ * 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/>.
+ *
+ */
+
+/**
+ * e_parser.cpp - Intercept special commands directly in the serial stream
+ */
+
+#include "../inc/MarlinConfigPre.h"
+
+#if ENABLED(EMERGENCY_PARSER)
+
+#include "e_parser.h"
+
+// Static data members
+bool EmergencyParser::killed_by_M112, // = false
+     EmergencyParser::quickstop_by_M410,
+     EmergencyParser::enabled;
+
+#if ENABLED(HOST_PROMPT_SUPPORT)
+  uint8_t EmergencyParser::M876_reason; // = 0
+#endif
+
+// Global instance
+EmergencyParser emergency_parser;
+
+#endif // EMERGENCY_PARSER
diff --git a/Marlin/src/feature/e_parser.h b/Marlin/src/feature/e_parser.h
new file mode 100644
index 0000000..659e516
--- /dev/null
+++ b/Marlin/src/feature/e_parser.h
@@ -0,0 +1,185 @@
+/**
+ * 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
+
+/**
+ * e_parser.h - Intercept special commands directly in the serial stream
+ */
+
+#include "../inc/MarlinConfigPre.h"
+
+#if ENABLED(HOST_PROMPT_SUPPORT)
+  #include "host_actions.h"
+#endif
+
+// External references
+extern bool wait_for_user, wait_for_heatup;
+
+class EmergencyParser {
+
+public:
+
+  // Currently looking for: M108, M112, M410, M876
+  enum State : char {
+    EP_RESET,
+    EP_N,
+    EP_M,
+    EP_M1,
+    EP_M10,
+    EP_M108,
+    EP_M11,
+    EP_M112,
+    EP_M4,
+    EP_M41,
+    EP_M410,
+    #if ENABLED(HOST_PROMPT_SUPPORT)
+      EP_M8,
+      EP_M87,
+      EP_M876,
+      EP_M876S,
+      EP_M876SN,
+    #endif
+    EP_IGNORE // to '\n'
+  };
+
+  static bool killed_by_M112;
+  static bool quickstop_by_M410;
+
+  #if ENABLED(HOST_PROMPT_SUPPORT)
+    static uint8_t M876_reason;
+  #endif
+
+  EmergencyParser() { enable(); }
+
+  FORCE_INLINE static void enable()  { enabled = true; }
+
+  FORCE_INLINE static void disable() { enabled = false; }
+
+  FORCE_INLINE static void update(State &state, const uint8_t c) {
+    switch (state) {
+      case EP_RESET:
+        switch (c) {
+          case ' ': case '\n': case '\r': break;
+          case 'N': state = EP_N;      break;
+          case 'M': state = EP_M;      break;
+          default: state  = EP_IGNORE;
+        }
+        break;
+
+      case EP_N:
+        switch (c) {
+          case '0' ... '9':
+          case '-': case ' ':   break;
+          case 'M': state = EP_M;      break;
+          default:  state = EP_IGNORE;
+        }
+        break;
+
+      case EP_M:
+        switch (c) {
+          case ' ': break;
+          case '1': state = EP_M1;     break;
+          case '4': state = EP_M4;     break;
+          #if ENABLED(HOST_PROMPT_SUPPORT)
+            case '8': state = EP_M8;     break;
+          #endif
+          default: state  = EP_IGNORE;
+        }
+        break;
+
+      case EP_M1:
+        switch (c) {
+          case '0': state = EP_M10;    break;
+          case '1': state = EP_M11;    break;
+          default: state  = EP_IGNORE;
+        }
+        break;
+
+      case EP_M10:
+        state = (c == '8') ? EP_M108 : EP_IGNORE;
+        break;
+
+      case EP_M11:
+        state = (c == '2') ? EP_M112 : EP_IGNORE;
+        break;
+
+      case EP_M4:
+        state = (c == '1') ? EP_M41 : EP_IGNORE;
+        break;
+
+      case EP_M41:
+        state = (c == '0') ? EP_M410 : EP_IGNORE;
+        break;
+
+      #if ENABLED(HOST_PROMPT_SUPPORT)
+      case EP_M8:
+        state = (c == '7') ? EP_M87 : EP_IGNORE;
+        break;
+
+      case EP_M87:
+        state = (c == '6') ? EP_M876 : EP_IGNORE;
+        break;
+
+      case EP_M876:
+        switch (c) {
+          case ' ': break;
+          case 'S': state = EP_M876S; break;
+          default:  state = EP_IGNORE; break;
+        }
+        break;
+
+      case EP_M876S:
+        switch (c) {
+          case ' ': break;
+          case '0' ... '9':
+            state = EP_M876SN;
+            M876_reason = (uint8_t)(c - '0');
+            break;
+        }
+        break;
+      #endif
+
+      case EP_IGNORE:
+        if (ISEOL(c)) state = EP_RESET;
+        break;
+
+      default:
+        if (ISEOL(c)) {
+          if (enabled) switch (state) {
+            case EP_M108: wait_for_user = wait_for_heatup = false; break;
+            case EP_M112: killed_by_M112 = true; break;
+            case EP_M410: quickstop_by_M410 = true; break;
+            #if ENABLED(HOST_PROMPT_SUPPORT)
+              case EP_M876SN: host_response_handler(M876_reason); break;
+            #endif
+            default: break;
+          }
+          state = EP_RESET;
+        }
+    }
+  }
+
+private:
+  static bool enabled;
+};
+
+extern EmergencyParser emergency_parser;
diff --git a/Marlin/src/feature/encoder_i2c.cpp b/Marlin/src/feature/encoder_i2c.cpp
new file mode 100644
index 0000000..fa3cf15
--- /dev/null
+++ b/Marlin/src/feature/encoder_i2c.cpp
@@ -0,0 +1,1139 @@
+/**
+ * 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/>.
+ *
+ */
+
+//todo:  add support for multiple encoders on a single axis
+//todo:    add z axis auto-leveling
+//todo:  consolidate some of the related M codes?
+//todo:  add endstop-replacement mode?
+//todo:  try faster I2C speed; tweak TWI_FREQ (400000L, or faster?); or just TWBR = ((CPU_FREQ / 400000L) - 16) / 2;
+//todo:    consider Marlin-optimized Wire library; i.e. MarlinWire, like MarlinSerial
+
+
+#include "../inc/MarlinConfig.h"
+
+#if ENABLED(I2C_POSITION_ENCODERS)
+
+#include "encoder_i2c.h"
+
+#include "../module/stepper.h"
+#include "../gcode/parser.h"
+
+#include "../feature/babystep.h"
+
+#include <Wire.h>
+
+I2CPositionEncodersMgr I2CPEM;
+
+void I2CPositionEncoder::init(const uint8_t address, const AxisEnum axis) {
+  encoderAxis = axis;
+  i2cAddress = address;
+
+  initialized++;
+
+  SERIAL_ECHOLNPAIR("Setting up encoder on ", axis_codes[encoderAxis], " axis, addr = ", address);
+
+  position = get_position();
+}
+
+void I2CPositionEncoder::update() {
+  if (!initialized || !homed || !active) return; //check encoder is set up and active
+
+  position = get_position();
+
+  //we don't want to stop things just because the encoder missed a message,
+  //so we only care about responses that indicate bad magnetic strength
+
+  if (!passes_test(false)) { //check encoder data is good
+    lastErrorTime = millis();
+    /*
+    if (trusted) { //commented out as part of the note below
+      trusted = false;
+      SERIAL_ECHOLMPAIR("Fault detected on ", axis_codes[encoderAxis], " axis encoder. Disengaging error correction until module is trusted again.");
+    }
+    */
+    return;
+  }
+
+  if (!trusted) {
+    /**
+     * This is commented out because it introduces error and can cause bad print quality.
+     *
+     * This code is intended to manage situations where the encoder has reported bad magnetic strength.
+     * This indicates that the magnetic strip was too far away from the sensor to reliably track position.
+     * When this happens, this code resets the offset based on where the printer thinks it is. This has been
+     * shown to introduce errors in actual position which result in drifting prints and poor print quality.
+     * Perhaps a better method would be to disable correction on the axis with a problem, report it to the
+     * user via the status leds on the encoder module and prompt the user to re-home the axis at which point
+     * the encoder would be re-enabled.
+     */
+
+    #if 0
+      // If the magnetic strength has been good for a certain time, start trusting the module again
+
+      if (millis() - lastErrorTime > I2CPE_TIME_TRUSTED) {
+        trusted = true;
+
+        SERIAL_ECHOLNPAIR("Untrusted encoder module on ", axis_codes[encoderAxis], " axis has been fault-free for set duration, reinstating error correction.");
+
+        //the encoder likely lost its place when the error occured, so we'll reset and use the printer's
+        //idea of where it the axis is to re-initialize
+        const float pos = planner.get_axis_position_mm(encoderAxis);
+        int32_t positionInTicks = pos * get_ticks_unit();
+
+        //shift position from previous to current position
+        zeroOffset -= (positionInTicks - get_position());
+
+        #ifdef I2CPE_DEBUG
+          SERIAL_ECHOLNPAIR("Current position is ", pos);
+          SERIAL_ECHOLNPAIR("Position in encoder ticks is ", positionInTicks);
+          SERIAL_ECHOLNPAIR("New zero-offset of ", zeroOffset);
+          SERIAL_ECHOPAIR("New position reads as ", get_position());
+          SERIAL_CHAR('(');
+          SERIAL_DECIMAL(mm_from_count(get_position()));
+          SERIAL_ECHOLNPGM(")");
+        #endif
+      }
+    #endif
+    return;
+  }
+
+  lastPosition = position;
+  const millis_t positionTime = millis();
+
+  //only do error correction if setup and enabled
+  if (ec && ecMethod != I2CPE_ECM_NONE) {
+
+    #ifdef I2CPE_EC_THRESH_PROPORTIONAL
+      const millis_t deltaTime = positionTime - lastPositionTime;
+      const uint32_t distance = ABS(position - lastPosition),
+                     speed = distance / deltaTime;
+      const float threshold = constrain((speed / 50), 1, 50) * ecThreshold;
+    #else
+      const float threshold = get_error_correct_threshold();
+    #endif
+
+    //check error
+    #if ENABLED(I2CPE_ERR_ROLLING_AVERAGE)
+      float sum = 0, diffSum = 0;
+
+      errIdx = (errIdx >= I2CPE_ERR_ARRAY_SIZE - 1) ? 0 : errIdx + 1;
+      err[errIdx] = get_axis_error_steps(false);
+
+      LOOP_L_N(i, I2CPE_ERR_ARRAY_SIZE) {
+        sum += err[i];
+        if (i) diffSum += ABS(err[i-1] - err[i]);
+      }
+
+      const int32_t error = int32_t(sum / (I2CPE_ERR_ARRAY_SIZE + 1)); //calculate average for error
+
+    #else
+      const int32_t error = get_axis_error_steps(false);
+    #endif
+
+    //SERIAL_ECHOLNPAIR("Axis error steps: ", error);
+
+    #ifdef I2CPE_ERR_THRESH_ABORT
+      if (ABS(error) > I2CPE_ERR_THRESH_ABORT * planner.settings.axis_steps_per_mm[encoderAxis]) {
+        //kill(PSTR("Significant Error"));
+        SERIAL_ECHOLNPAIR("Axis error over threshold, aborting!", error);
+        safe_delay(5000);
+      }
+    #endif
+
+    #if ENABLED(I2CPE_ERR_ROLLING_AVERAGE)
+      if (errIdx == 0) {
+        // In order to correct for "error" but avoid correcting for noise and non-skips
+        // it must be > threshold and have a difference average of < 10 and be < 2000 steps
+        if (ABS(error) > threshold * planner.settings.axis_steps_per_mm[encoderAxis]
+            && diffSum < 10 * (I2CPE_ERR_ARRAY_SIZE - 1)
+            && ABS(error) < 2000
+        ) {                              // Check for persistent error (skip)
+          errPrst[errPrstIdx++] = error; // Error must persist for I2CPE_ERR_PRST_ARRAY_SIZE error cycles. This also serves to improve the average accuracy
+          if (errPrstIdx >= I2CPE_ERR_PRST_ARRAY_SIZE) {
+            float sumP = 0;
+            LOOP_L_N(i, I2CPE_ERR_PRST_ARRAY_SIZE) sumP += errPrst[i];
+            const int32_t errorP = int32_t(sumP * RECIPROCAL(I2CPE_ERR_PRST_ARRAY_SIZE));
+            SERIAL_ECHO(axis_codes[encoderAxis]);
+            SERIAL_ECHOLNPAIR(" : CORRECT ERR ", errorP * planner.steps_to_mm[encoderAxis], "mm");
+            babystep.add_steps(encoderAxis, -LROUND(errorP));
+            errPrstIdx = 0;
+          }
+        }
+        else
+          errPrstIdx = 0;
+      }
+    #else
+      if (ABS(error) > threshold * planner.settings.axis_steps_per_mm[encoderAxis]) {
+        //SERIAL_ECHOLN(error);
+        //SERIAL_ECHOLN(position);
+        babystep.add_steps(encoderAxis, -LROUND(error / 2));
+      }
+    #endif
+
+    if (ABS(error) > I2CPE_ERR_CNT_THRESH * planner.settings.axis_steps_per_mm[encoderAxis]) {
+      const millis_t ms = millis();
+      if (ELAPSED(ms, nextErrorCountTime)) {
+        SERIAL_ECHO(axis_codes[encoderAxis]);
+        SERIAL_ECHOLNPAIR(" : LARGE ERR ", int(error), "; diffSum=", diffSum);
+        errorCount++;
+        nextErrorCountTime = ms + I2CPE_ERR_CNT_DEBOUNCE_MS;
+      }
+    }
+  }
+
+  lastPositionTime = positionTime;
+}
+
+void I2CPositionEncoder::set_homed() {
+  if (active) {
+    reset();  // Reset module's offset to zero (so current position is homed / zero)
+    delay(10);
+
+    zeroOffset = get_raw_count();
+    homed++;
+    trusted++;
+
+    #ifdef I2CPE_DEBUG
+      SERIAL_ECHO(axis_codes[encoderAxis]);
+      SERIAL_ECHOLNPAIR(" axis encoder homed, offset of ", zeroOffset, " ticks.");
+    #endif
+  }
+}
+
+void I2CPositionEncoder::set_unhomed() {
+  zeroOffset = 0;
+  homed = trusted = false;
+
+  #ifdef I2CPE_DEBUG
+    SERIAL_ECHO(axis_codes[encoderAxis]);
+    SERIAL_ECHOLNPGM(" axis encoder unhomed.");
+  #endif
+}
+
+bool I2CPositionEncoder::passes_test(const bool report) {
+  if (report) {
+    if (H != I2CPE_MAG_SIG_GOOD) SERIAL_ECHOPGM("Warning. ");
+    SERIAL_ECHO(axis_codes[encoderAxis]);
+    serial_ternary(H == I2CPE_MAG_SIG_BAD, PSTR(" axis "), PSTR("magnetic strip "), PSTR("encoder "));
+    switch (H) {
+      case I2CPE_MAG_SIG_GOOD:
+      case I2CPE_MAG_SIG_MID:
+        SERIAL_ECHO_TERNARY(H == I2CPE_MAG_SIG_GOOD, "passes test; field strength ", "good", "fair", ".\n");
+        break;
+      default:
+        SERIAL_ECHOLNPGM("not detected!");
+    }
+  }
+  return (H == I2CPE_MAG_SIG_GOOD || H == I2CPE_MAG_SIG_MID);
+}
+
+float I2CPositionEncoder::get_axis_error_mm(const bool report) {
+  const float target = planner.get_axis_position_mm(encoderAxis),
+              actual = mm_from_count(position),
+              diff = actual - target,
+              error = ABS(diff) > 10000 ? 0 : diff; // Huge error is a bad reading
+
+  if (report) {
+    SERIAL_ECHO(axis_codes[encoderAxis]);
+    SERIAL_ECHOLNPAIR(" axis target=", target, "mm; actual=", actual, "mm; err=", error, "mm");
+  }
+
+  return error;
+}
+
+int32_t I2CPositionEncoder::get_axis_error_steps(const bool report) {
+  if (!active) {
+    if (report) {
+      SERIAL_ECHO(axis_codes[encoderAxis]);
+      SERIAL_ECHOLNPGM(" axis encoder not active!");
+    }
+    return 0;
+  }
+
+  float stepperTicksPerUnit;
+  int32_t encoderTicks = position, encoderCountInStepperTicksScaled;
+  //int32_t stepperTicks = stepper.position(encoderAxis);
+
+  // With a rotary encoder we're concerned with ticks/rev; whereas with a linear we're concerned with ticks/mm
+  stepperTicksPerUnit = (type == I2CPE_ENC_TYPE_ROTARY) ? stepperTicks : planner.settings.axis_steps_per_mm[encoderAxis];
+
+  //convert both 'ticks' into same units / base
+  encoderCountInStepperTicksScaled = LROUND((stepperTicksPerUnit * encoderTicks) / encoderTicksPerUnit);
+
+  const int32_t target = stepper.position(encoderAxis);
+  int32_t error = encoderCountInStepperTicksScaled - target;
+
+  //suppress discontinuities (might be caused by bad I2C readings...?)
+  const bool suppressOutput = (ABS(error - errorPrev) > 100);
+
+  errorPrev = error;
+
+  if (report) {
+    SERIAL_ECHO(axis_codes[encoderAxis]);
+    SERIAL_ECHOLNPAIR(" axis target=", target, "; actual=", encoderCountInStepperTicksScaled, "; err=", error);
+  }
+
+  if (suppressOutput) {
+    if (report) SERIAL_ECHOLNPGM("!Discontinuity. Suppressing error.");
+    error = 0;
+  }
+
+  return error;
+}
+
+int32_t I2CPositionEncoder::get_raw_count() {
+  uint8_t index = 0;
+  i2cLong encoderCount;
+
+  encoderCount.val = 0x00;
+
+  if (Wire.requestFrom(I2C_ADDRESS(i2cAddress), uint8_t(3)) != 3) {
+    //houston, we have a problem...
+    H = I2CPE_MAG_SIG_NF;
+    return 0;
+  }
+
+  while (Wire.available())
+    encoderCount.bval[index++] = (uint8_t)Wire.read();
+
+  //extract the magnetic strength
+  H = (B00000011 & (encoderCount.bval[2] >> 6));
+
+  //extract sign bit; sign = (encoderCount.bval[2] & B00100000);
+  //set all upper bits to the sign value to overwrite H
+  encoderCount.val = (encoderCount.bval[2] & B00100000) ? (encoderCount.val | 0xFFC00000) : (encoderCount.val & 0x003FFFFF);
+
+  if (invert) encoderCount.val *= -1;
+
+  return encoderCount.val;
+}
+
+bool I2CPositionEncoder::test_axis() {
+  //only works on XYZ cartesian machines for the time being
+  if (!(encoderAxis == X_AXIS || encoderAxis == Y_AXIS || encoderAxis == Z_AXIS)) return false;
+
+  const float startPosition = soft_endstop.min[encoderAxis] + 10,
+              endPosition = soft_endstop.max[encoderAxis] - 10;
+  const feedRate_t fr_mm_s = FLOOR(homing_feedrate(encoderAxis));
+
+  ec = false;
+
+  xyze_pos_t startCoord, endCoord;
+  LOOP_XYZ(a) {
+    startCoord[a] = planner.get_axis_position_mm((AxisEnum)a);
+    endCoord[a] = planner.get_axis_position_mm((AxisEnum)a);
+  }
+  startCoord[encoderAxis] = startPosition;
+  endCoord[encoderAxis] = endPosition;
+
+  planner.synchronize();
+  startCoord.e = planner.get_axis_position_mm(E_AXIS);
+  planner.buffer_line(startCoord, fr_mm_s, 0);
+  planner.synchronize();
+
+  // if the module isn't currently trusted, wait until it is (or until it should be if things are working)
+  if (!trusted) {
+    int32_t startWaitingTime = millis();
+    while (!trusted && millis() - startWaitingTime < I2CPE_TIME_TRUSTED)
+      safe_delay(500);
+  }
+
+  if (trusted) { // if trusted, commence test
+    endCoord.e = planner.get_axis_position_mm(E_AXIS);
+    planner.buffer_line(endCoord, fr_mm_s, 0);
+    planner.synchronize();
+  }
+
+  return trusted;
+}
+
+void I2CPositionEncoder::calibrate_steps_mm(const uint8_t iter) {
+  if (type != I2CPE_ENC_TYPE_LINEAR) {
+    SERIAL_ECHOLNPGM("Steps/mm calibration requires linear encoder.");
+    return;
+  }
+
+  if (!(encoderAxis == X_AXIS || encoderAxis == Y_AXIS || encoderAxis == Z_AXIS)) {
+    SERIAL_ECHOLNPGM("Steps/mm calibration not supported for this axis.");
+    return;
+  }
+
+  float old_steps_mm, new_steps_mm,
+        startDistance, endDistance,
+        travelDistance, travelledDistance, total = 0;
+
+  int32_t startCount, stopCount;
+
+  const feedRate_t fr_mm_s = homing_feedrate(encoderAxis);
+
+  bool oldec = ec;
+  ec = false;
+
+  startDistance = 20;
+  endDistance = soft_endstop.max[encoderAxis] - 20;
+  travelDistance = endDistance - startDistance;
+
+  xyze_pos_t startCoord, endCoord;
+  LOOP_XYZ(a) {
+    startCoord[a] = planner.get_axis_position_mm((AxisEnum)a);
+    endCoord[a] = planner.get_axis_position_mm((AxisEnum)a);
+  }
+  startCoord[encoderAxis] = startDistance;
+  endCoord[encoderAxis] = endDistance;
+
+  planner.synchronize();
+
+  LOOP_L_N(i, iter) {
+    startCoord.e = planner.get_axis_position_mm(E_AXIS);
+    planner.buffer_line(startCoord, fr_mm_s, 0);
+    planner.synchronize();
+
+    delay(250);
+    startCount = get_position();
+
+    //do_blocking_move_to(endCoord);
+
+    endCoord.e = planner.get_axis_position_mm(E_AXIS);
+    planner.buffer_line(endCoord, fr_mm_s, 0);
+    planner.synchronize();
+
+    //Read encoder distance
+    delay(250);
+    stopCount = get_position();
+
+    travelledDistance = mm_from_count(ABS(stopCount - startCount));
+
+    SERIAL_ECHOLNPAIR("Attempted travel: ", travelDistance, "mm");
+    SERIAL_ECHOLNPAIR("   Actual travel:  ", travelledDistance, "mm");
+
+    //Calculate new axis steps per unit
+    old_steps_mm = planner.settings.axis_steps_per_mm[encoderAxis];
+    new_steps_mm = (old_steps_mm * travelDistance) / travelledDistance;
+
+    SERIAL_ECHOLNPAIR("Old steps/mm: ", old_steps_mm);
+    SERIAL_ECHOLNPAIR("New steps/mm: ", new_steps_mm);
+
+    //Save new value
+    planner.settings.axis_steps_per_mm[encoderAxis] = new_steps_mm;
+
+    if (iter > 1) {
+      total += new_steps_mm;
+
+      // swap start and end points so next loop runs from current position
+      const float tempCoord = startCoord[encoderAxis];
+      startCoord[encoderAxis] = endCoord[encoderAxis];
+      endCoord[encoderAxis] = tempCoord;
+    }
+  }
+
+  if (iter > 1) {
+    total /= (float)iter;
+    SERIAL_ECHOLNPAIR("Average steps/mm: ", total);
+  }
+
+  ec = oldec;
+
+  SERIAL_ECHOLNPGM("Calculated steps/mm set. Use M500 to save to EEPROM.");
+}
+
+void I2CPositionEncoder::reset() {
+  Wire.beginTransmission(I2C_ADDRESS(i2cAddress));
+  Wire.write(I2CPE_RESET_COUNT);
+  Wire.endTransmission();
+
+  TERN_(I2CPE_ERR_ROLLING_AVERAGE, ZERO(err));
+}
+
+
+bool I2CPositionEncodersMgr::I2CPE_anyaxis;
+uint8_t I2CPositionEncodersMgr::I2CPE_addr,
+        I2CPositionEncodersMgr::I2CPE_idx;
+I2CPositionEncoder I2CPositionEncodersMgr::encoders[I2CPE_ENCODER_CNT];
+
+void I2CPositionEncodersMgr::init() {
+  Wire.begin();
+
+  #if I2CPE_ENCODER_CNT > 0
+    uint8_t i = 0;
+
+    encoders[i].init(I2CPE_ENC_1_ADDR, I2CPE_ENC_1_AXIS);
+
+    #ifdef I2CPE_ENC_1_TYPE
+      encoders[i].set_type(I2CPE_ENC_1_TYPE);
+    #endif
+    #ifdef I2CPE_ENC_1_TICKS_UNIT
+      encoders[i].set_ticks_unit(I2CPE_ENC_1_TICKS_UNIT);
+    #endif
+    #ifdef I2CPE_ENC_1_TICKS_REV
+      encoders[i].set_stepper_ticks(I2CPE_ENC_1_TICKS_REV);
+    #endif
+    #ifdef I2CPE_ENC_1_INVERT
+      encoders[i].set_inverted(I2CPE_ENC_1_INVERT);
+    #endif
+    #ifdef I2CPE_ENC_1_EC_METHOD
+      encoders[i].set_ec_method(I2CPE_ENC_1_EC_METHOD);
+    #endif
+    #ifdef I2CPE_ENC_1_EC_THRESH
+      encoders[i].set_ec_threshold(I2CPE_ENC_1_EC_THRESH);
+    #endif
+
+    encoders[i].set_active(encoders[i].passes_test(true));
+
+    #if I2CPE_ENC_1_AXIS == E_AXIS
+      encoders[i].set_homed();
+    #endif
+  #endif
+
+  #if I2CPE_ENCODER_CNT > 1
+    i++;
+
+    encoders[i].init(I2CPE_ENC_2_ADDR, I2CPE_ENC_2_AXIS);
+
+    #ifdef I2CPE_ENC_2_TYPE
+      encoders[i].set_type(I2CPE_ENC_2_TYPE);
+    #endif
+    #ifdef I2CPE_ENC_2_TICKS_UNIT
+      encoders[i].set_ticks_unit(I2CPE_ENC_2_TICKS_UNIT);
+    #endif
+    #ifdef I2CPE_ENC_2_TICKS_REV
+      encoders[i].set_stepper_ticks(I2CPE_ENC_2_TICKS_REV);
+    #endif
+    #ifdef I2CPE_ENC_2_INVERT
+      encoders[i].set_inverted(I2CPE_ENC_2_INVERT);
+    #endif
+    #ifdef I2CPE_ENC_2_EC_METHOD
+      encoders[i].set_ec_method(I2CPE_ENC_2_EC_METHOD);
+    #endif
+    #ifdef I2CPE_ENC_2_EC_THRESH
+      encoders[i].set_ec_threshold(I2CPE_ENC_2_EC_THRESH);
+    #endif
+
+    encoders[i].set_active(encoders[i].passes_test(true));
+
+    #if I2CPE_ENC_2_AXIS == E_AXIS
+      encoders[i].set_homed();
+    #endif
+  #endif
+
+  #if I2CPE_ENCODER_CNT > 2
+    i++;
+
+    encoders[i].init(I2CPE_ENC_3_ADDR, I2CPE_ENC_3_AXIS);
+
+    #ifdef I2CPE_ENC_3_TYPE
+      encoders[i].set_type(I2CPE_ENC_3_TYPE);
+    #endif
+    #ifdef I2CPE_ENC_3_TICKS_UNIT
+      encoders[i].set_ticks_unit(I2CPE_ENC_3_TICKS_UNIT);
+    #endif
+    #ifdef I2CPE_ENC_3_TICKS_REV
+      encoders[i].set_stepper_ticks(I2CPE_ENC_3_TICKS_REV);
+    #endif
+    #ifdef I2CPE_ENC_3_INVERT
+      encoders[i].set_inverted(I2CPE_ENC_3_INVERT);
+    #endif
+    #ifdef I2CPE_ENC_3_EC_METHOD
+      encoders[i].set_ec_method(I2CPE_ENC_3_EC_METHOD);
+    #endif
+    #ifdef I2CPE_ENC_3_EC_THRESH
+      encoders[i].set_ec_threshold(I2CPE_ENC_3_EC_THRESH);
+    #endif
+
+  encoders[i].set_active(encoders[i].passes_test(true));
+
+    #if I2CPE_ENC_3_AXIS == E_AXIS
+      encoders[i].set_homed();
+    #endif
+  #endif
+
+  #if I2CPE_ENCODER_CNT > 3
+    i++;
+
+    encoders[i].init(I2CPE_ENC_4_ADDR, I2CPE_ENC_4_AXIS);
+
+    #ifdef I2CPE_ENC_4_TYPE
+      encoders[i].set_type(I2CPE_ENC_4_TYPE);
+    #endif
+    #ifdef I2CPE_ENC_4_TICKS_UNIT
+      encoders[i].set_ticks_unit(I2CPE_ENC_4_TICKS_UNIT);
+    #endif
+    #ifdef I2CPE_ENC_4_TICKS_REV
+      encoders[i].set_stepper_ticks(I2CPE_ENC_4_TICKS_REV);
+    #endif
+    #ifdef I2CPE_ENC_4_INVERT
+      encoders[i].set_inverted(I2CPE_ENC_4_INVERT);
+    #endif
+    #ifdef I2CPE_ENC_4_EC_METHOD
+      encoders[i].set_ec_method(I2CPE_ENC_4_EC_METHOD);
+    #endif
+    #ifdef I2CPE_ENC_4_EC_THRESH
+      encoders[i].set_ec_threshold(I2CPE_ENC_4_EC_THRESH);
+    #endif
+
+    encoders[i].set_active(encoders[i].passes_test(true));
+
+    #if I2CPE_ENC_4_AXIS == E_AXIS
+      encoders[i].set_homed();
+    #endif
+  #endif
+
+  #if I2CPE_ENCODER_CNT > 4
+    i++;
+
+    encoders[i].init(I2CPE_ENC_5_ADDR, I2CPE_ENC_5_AXIS);
+
+    #ifdef I2CPE_ENC_5_TYPE
+      encoders[i].set_type(I2CPE_ENC_5_TYPE);
+    #endif
+    #ifdef I2CPE_ENC_5_TICKS_UNIT
+      encoders[i].set_ticks_unit(I2CPE_ENC_5_TICKS_UNIT);
+    #endif
+    #ifdef I2CPE_ENC_5_TICKS_REV
+      encoders[i].set_stepper_ticks(I2CPE_ENC_5_TICKS_REV);
+    #endif
+    #ifdef I2CPE_ENC_5_INVERT
+      encoders[i].set_inverted(I2CPE_ENC_5_INVERT);
+    #endif
+    #ifdef I2CPE_ENC_5_EC_METHOD
+      encoders[i].set_ec_method(I2CPE_ENC_5_EC_METHOD);
+    #endif
+    #ifdef I2CPE_ENC_5_EC_THRESH
+      encoders[i].set_ec_threshold(I2CPE_ENC_5_EC_THRESH);
+    #endif
+
+    encoders[i].set_active(encoders[i].passes_test(true));
+
+    #if I2CPE_ENC_5_AXIS == E_AXIS
+      encoders[i].set_homed();
+    #endif
+  #endif
+
+  #if I2CPE_ENCODER_CNT > 5
+    i++;
+
+    encoders[i].init(I2CPE_ENC_6_ADDR, I2CPE_ENC_6_AXIS);
+
+    #ifdef I2CPE_ENC_6_TYPE
+      encoders[i].set_type(I2CPE_ENC_6_TYPE);
+    #endif
+    #ifdef I2CPE_ENC_6_TICKS_UNIT
+      encoders[i].set_ticks_unit(I2CPE_ENC_6_TICKS_UNIT);
+    #endif
+    #ifdef I2CPE_ENC_6_TICKS_REV
+      encoders[i].set_stepper_ticks(I2CPE_ENC_6_TICKS_REV);
+    #endif
+    #ifdef I2CPE_ENC_6_INVERT
+      encoders[i].set_inverted(I2CPE_ENC_6_INVERT);
+    #endif
+    #ifdef I2CPE_ENC_6_EC_METHOD
+      encoders[i].set_ec_method(I2CPE_ENC_6_EC_METHOD);
+    #endif
+    #ifdef I2CPE_ENC_6_EC_THRESH
+      encoders[i].set_ec_threshold(I2CPE_ENC_6_EC_THRESH);
+    #endif
+
+    encoders[i].set_active(encoders[i].passes_test(true));
+
+    #if I2CPE_ENC_6_AXIS == E_AXIS
+      encoders[i].set_homed();
+    #endif
+  #endif
+}
+
+void I2CPositionEncodersMgr::report_position(const int8_t idx, const bool units, const bool noOffset) {
+  CHECK_IDX();
+
+  if (units)
+    SERIAL_ECHOLN(noOffset ? encoders[idx].mm_from_count(encoders[idx].get_raw_count()) : encoders[idx].get_position_mm());
+  else {
+    if (noOffset) {
+      const int32_t raw_count = encoders[idx].get_raw_count();
+      SERIAL_ECHO(axis_codes[encoders[idx].get_axis()]);
+      SERIAL_CHAR(' ');
+
+      for (uint8_t j = 31; j > 0; j--)
+        SERIAL_ECHO((bool)(0x00000001 & (raw_count >> j)));
+
+      SERIAL_ECHO((bool)(0x00000001 & raw_count));
+      SERIAL_CHAR(' ');
+      SERIAL_ECHOLN(raw_count);
+    }
+    else
+      SERIAL_ECHOLN(encoders[idx].get_position());
+  }
+}
+
+void I2CPositionEncodersMgr::change_module_address(const uint8_t oldaddr, const uint8_t newaddr) {
+  // First check 'new' address is not in use
+  Wire.beginTransmission(I2C_ADDRESS(newaddr));
+  if (!Wire.endTransmission()) {
+    SERIAL_ECHOLNPAIR("?There is already a device with that address on the I2C bus! (", newaddr, ")");
+    return;
+  }
+
+  // Now check that we can find the module on the oldaddr address
+  Wire.beginTransmission(I2C_ADDRESS(oldaddr));
+  if (Wire.endTransmission()) {
+    SERIAL_ECHOLNPAIR("?No module detected at this address! (", oldaddr, ")");
+    return;
+  }
+
+  SERIAL_ECHOLNPAIR("Module found at ", oldaddr, ", changing address to ", newaddr);
+
+  // Change the modules address
+  Wire.beginTransmission(I2C_ADDRESS(oldaddr));
+  Wire.write(I2CPE_SET_ADDR);
+  Wire.write(newaddr);
+  Wire.endTransmission();
+
+  SERIAL_ECHOLNPGM("Address changed, resetting and waiting for confirmation..");
+
+  // Wait for the module to reset (can probably be improved by polling address with a timeout).
+  safe_delay(I2CPE_REBOOT_TIME);
+
+  // Look for the module at the new address.
+  Wire.beginTransmission(I2C_ADDRESS(newaddr));
+  if (Wire.endTransmission()) {
+    SERIAL_ECHOLNPGM("Address change failed! Check encoder module.");
+    return;
+  }
+
+  SERIAL_ECHOLNPGM("Address change successful!");
+
+  // Now, if this module is configured, find which encoder instance it's supposed to correspond to
+  // and enable it (it will likely have failed initialization on power-up, before the address change).
+  const int8_t idx = idx_from_addr(newaddr);
+  if (idx >= 0 && !encoders[idx].get_active()) {
+    SERIAL_ECHO(axis_codes[encoders[idx].get_axis()]);
+    SERIAL_ECHOLNPGM(" axis encoder was not detected on printer startup. Trying again.");
+    encoders[idx].set_active(encoders[idx].passes_test(true));
+  }
+}
+
+void I2CPositionEncodersMgr::report_module_firmware(const uint8_t address) {
+  // First check there is a module
+  Wire.beginTransmission(I2C_ADDRESS(address));
+  if (Wire.endTransmission()) {
+    SERIAL_ECHOLNPAIR("?No module detected at this address! (", address, ")");
+    return;
+  }
+
+  SERIAL_ECHOLNPAIR("Requesting version info from module at address ", address, ":");
+
+  Wire.beginTransmission(I2C_ADDRESS(address));
+  Wire.write(I2CPE_SET_REPORT_MODE);
+  Wire.write(I2CPE_REPORT_VERSION);
+  Wire.endTransmission();
+
+  // Read value
+  if (Wire.requestFrom(I2C_ADDRESS(address), uint8_t(32))) {
+    char c;
+    while (Wire.available() > 0 && (c = (char)Wire.read()) > 0)
+      SERIAL_ECHO(c);
+    SERIAL_EOL();
+  }
+
+  // Set module back to normal (distance) mode
+  Wire.beginTransmission(I2C_ADDRESS(address));
+  Wire.write(I2CPE_SET_REPORT_MODE);
+  Wire.write(I2CPE_REPORT_DISTANCE);
+  Wire.endTransmission();
+}
+
+int8_t I2CPositionEncodersMgr::parse() {
+  I2CPE_addr = 0;
+
+  if (parser.seen('A')) {
+
+    if (!parser.has_value()) {
+      SERIAL_ECHOLNPGM("?A seen, but no address specified! [30-200]");
+      return I2CPE_PARSE_ERR;
+    };
+
+    I2CPE_addr = parser.value_byte();
+    if (!WITHIN(I2CPE_addr, 30, 200)) { // reserve the first 30 and last 55
+      SERIAL_ECHOLNPGM("?Address out of range. [30-200]");
+      return I2CPE_PARSE_ERR;
+    }
+
+    I2CPE_idx = idx_from_addr(I2CPE_addr);
+    if (I2CPE_idx >= I2CPE_ENCODER_CNT) {
+      SERIAL_ECHOLNPGM("?No device with this address!");
+      return I2CPE_PARSE_ERR;
+    }
+  }
+  else if (parser.seenval('I')) {
+
+    if (!parser.has_value()) {
+      SERIAL_ECHOLNPAIR("?I seen, but no index specified! [0-", I2CPE_ENCODER_CNT - 1, "]");
+      return I2CPE_PARSE_ERR;
+    };
+
+    I2CPE_idx = parser.value_byte();
+    if (I2CPE_idx >= I2CPE_ENCODER_CNT) {
+      SERIAL_ECHOLNPAIR("?Index out of range. [0-", I2CPE_ENCODER_CNT - 1, "]");
+      return I2CPE_PARSE_ERR;
+    }
+
+    I2CPE_addr = encoders[I2CPE_idx].get_address();
+  }
+  else
+    I2CPE_idx = 0xFF;
+
+  I2CPE_anyaxis = parser.seen_axis();
+
+  return I2CPE_PARSE_OK;
+};
+
+/**
+ * M860:  Report the position(s) of position encoder module(s).
+ *
+ *   A<addr>  Module I2C address.  [30, 200].
+ *   I<index> Module index.  [0, I2CPE_ENCODER_CNT - 1]
+ *   O        Include homed zero-offset in returned position.
+ *   U        Units in mm or raw step count.
+ *
+ *   If A or I not specified:
+ *    X       Report on X axis encoder, if present.
+ *    Y       Report on Y axis encoder, if present.
+ *    Z       Report on Z axis encoder, if present.
+ *    E       Report on E axis encoder, if present.
+ */
+void I2CPositionEncodersMgr::M860() {
+  if (parse()) return;
+
+  const bool hasU = parser.seen('U'), hasO = parser.seen('O');
+
+  if (I2CPE_idx == 0xFF) {
+    LOOP_XYZE(i) {
+      if (!I2CPE_anyaxis || parser.seen(axis_codes[i])) {
+        const uint8_t idx = idx_from_axis(AxisEnum(i));
+        if ((int8_t)idx >= 0) report_position(idx, hasU, hasO);
+      }
+    }
+  }
+  else
+    report_position(I2CPE_idx, hasU, hasO);
+}
+
+/**
+ * M861:  Report the status of position encoder modules.
+ *
+ *   A<addr>  Module I2C address.  [30, 200].
+ *   I<index> Module index.  [0, I2CPE_ENCODER_CNT - 1]
+ *
+ *   If A or I not specified:
+ *    X       Report on X axis encoder, if present.
+ *    Y       Report on Y axis encoder, if present.
+ *    Z       Report on Z axis encoder, if present.
+ *    E       Report on E axis encoder, if present.
+ */
+void I2CPositionEncodersMgr::M861() {
+  if (parse()) return;
+
+  if (I2CPE_idx == 0xFF) {
+    LOOP_XYZE(i) {
+      if (!I2CPE_anyaxis || parser.seen(axis_codes[i])) {
+        const uint8_t idx = idx_from_axis(AxisEnum(i));
+        if ((int8_t)idx >= 0) report_status(idx);
+      }
+    }
+  }
+  else
+    report_status(I2CPE_idx);
+}
+
+/**
+ * M862:  Perform an axis continuity test for position encoder
+ *        modules.
+ *
+ *   A<addr>  Module I2C address.  [30, 200].
+ *   I<index> Module index.  [0, I2CPE_ENCODER_CNT - 1]
+ *
+ *   If A or I not specified:
+ *    X       Report on X axis encoder, if present.
+ *    Y       Report on Y axis encoder, if present.
+ *    Z       Report on Z axis encoder, if present.
+ *    E       Report on E axis encoder, if present.
+ */
+void I2CPositionEncodersMgr::M862() {
+  if (parse()) return;
+
+  if (I2CPE_idx == 0xFF) {
+    LOOP_XYZE(i) {
+      if (!I2CPE_anyaxis || parser.seen(axis_codes[i])) {
+        const uint8_t idx = idx_from_axis(AxisEnum(i));
+        if ((int8_t)idx >= 0) test_axis(idx);
+      }
+    }
+  }
+  else
+    test_axis(I2CPE_idx);
+}
+
+/**
+ * M863:  Perform steps-per-mm calibration for
+ *        position encoder modules.
+ *
+ *   A<addr>  Module I2C address.  [30, 200].
+ *   I<index> Module index.  [0, I2CPE_ENCODER_CNT - 1]
+ *   P        Number of rePeats/iterations.
+ *
+ *   If A or I not specified:
+ *    X       Report on X axis encoder, if present.
+ *    Y       Report on Y axis encoder, if present.
+ *    Z       Report on Z axis encoder, if present.
+ *    E       Report on E axis encoder, if present.
+ */
+void I2CPositionEncodersMgr::M863() {
+  if (parse()) return;
+
+  const uint8_t iterations = constrain(parser.byteval('P', 1), 1, 10);
+
+  if (I2CPE_idx == 0xFF) {
+    LOOP_XYZE(i) {
+      if (!I2CPE_anyaxis || parser.seen(axis_codes[i])) {
+        const uint8_t idx = idx_from_axis(AxisEnum(i));
+        if ((int8_t)idx >= 0) calibrate_steps_mm(idx, iterations);
+      }
+    }
+  }
+  else
+    calibrate_steps_mm(I2CPE_idx, iterations);
+}
+
+/**
+ * M864:  Change position encoder module I2C address.
+ *
+ *   A<addr>  Module current/old I2C address.  If not present,
+ *            assumes default address (030).  [30, 200].
+ *   S<addr>  Module new I2C address. [30, 200].
+ *
+ *   If S is not specified:
+ *    X       Use I2CPE_PRESET_ADDR_X (030).
+ *    Y       Use I2CPE_PRESET_ADDR_Y (031).
+ *    Z       Use I2CPE_PRESET_ADDR_Z (032).
+ *    E       Use I2CPE_PRESET_ADDR_E (033).
+ */
+void I2CPositionEncodersMgr::M864() {
+  uint8_t newAddress;
+
+  if (parse()) return;
+
+  if (!I2CPE_addr) I2CPE_addr = I2CPE_PRESET_ADDR_X;
+
+  if (parser.seen('S')) {
+    if (!parser.has_value()) {
+      SERIAL_ECHOLNPGM("?S seen, but no address specified! [30-200]");
+      return;
+    };
+
+    newAddress = parser.value_byte();
+    if (!WITHIN(newAddress, 30, 200)) {
+      SERIAL_ECHOLNPGM("?New address out of range. [30-200]");
+      return;
+    }
+  }
+  else if (!I2CPE_anyaxis) {
+    SERIAL_ECHOLNPGM("?You must specify S or [XYZE].");
+    return;
+  }
+  else {
+         if (parser.seen('X')) newAddress = I2CPE_PRESET_ADDR_X;
+    else if (parser.seen('Y')) newAddress = I2CPE_PRESET_ADDR_Y;
+    else if (parser.seen('Z')) newAddress = I2CPE_PRESET_ADDR_Z;
+    else if (parser.seen('E')) newAddress = I2CPE_PRESET_ADDR_E;
+    else return;
+  }
+
+  SERIAL_ECHOLNPAIR("Changing module at address ", I2CPE_addr, " to address ", newAddress);
+
+  change_module_address(I2CPE_addr, newAddress);
+}
+
+/**
+ * M865:  Check position encoder module firmware version.
+ *
+ *   A<addr>  Module I2C address.  [30, 200].
+ *   I<index> Module index.  [0, I2CPE_ENCODER_CNT - 1].
+ *
+ *   If A or I not specified:
+ *    X       Check X axis encoder, if present.
+ *    Y       Check Y axis encoder, if present.
+ *    Z       Check Z axis encoder, if present.
+ *    E       Check E axis encoder, if present.
+ */
+void I2CPositionEncodersMgr::M865() {
+  if (parse()) return;
+
+  if (!I2CPE_addr) {
+    LOOP_XYZE(i) {
+      if (!I2CPE_anyaxis || parser.seen(axis_codes[i])) {
+        const uint8_t idx = idx_from_axis(AxisEnum(i));
+        if ((int8_t)idx >= 0) report_module_firmware(encoders[idx].get_address());
+      }
+    }
+  }
+  else
+    report_module_firmware(I2CPE_addr);
+}
+
+/**
+ * M866:  Report or reset position encoder module error
+ *        count.
+ *
+ *   A<addr>  Module I2C address.  [30, 200].
+ *   I<index> Module index.  [0, I2CPE_ENCODER_CNT - 1].
+ *   R        Reset error counter.
+ *
+ *   If A or I not specified:
+ *    X       Act on X axis encoder, if present.
+ *    Y       Act on Y axis encoder, if present.
+ *    Z       Act on Z axis encoder, if present.
+ *    E       Act on E axis encoder, if present.
+ */
+void I2CPositionEncodersMgr::M866() {
+  if (parse()) return;
+
+  const bool hasR = parser.seen('R');
+
+  if (I2CPE_idx == 0xFF) {
+    LOOP_XYZE(i) {
+      if (!I2CPE_anyaxis || parser.seen(axis_codes[i])) {
+        const uint8_t idx = idx_from_axis(AxisEnum(i));
+        if ((int8_t)idx >= 0) {
+          if (hasR)
+            reset_error_count(idx, AxisEnum(i));
+          else
+            report_error_count(idx, AxisEnum(i));
+        }
+      }
+    }
+  }
+  else if (hasR)
+    reset_error_count(I2CPE_idx, encoders[I2CPE_idx].get_axis());
+  else
+    report_error_count(I2CPE_idx, encoders[I2CPE_idx].get_axis());
+}
+
+/**
+ * M867:  Enable/disable or toggle error correction for position encoder modules.
+ *
+ *   A<addr>  Module I2C address.  [30, 200].
+ *   I<index> Module index.  [0, I2CPE_ENCODER_CNT - 1].
+ *   S<1|0>   Enable/disable error correction. 1 enables, 0 disables.  If not
+ *            supplied, toggle.
+ *
+ *   If A or I not specified:
+ *    X       Act on X axis encoder, if present.
+ *    Y       Act on Y axis encoder, if present.
+ *    Z       Act on Z axis encoder, if present.
+ *    E       Act on E axis encoder, if present.
+ */
+void I2CPositionEncodersMgr::M867() {
+  if (parse()) return;
+
+  const int8_t onoff = parser.seenval('S') ? parser.value_int() : -1;
+
+  if (I2CPE_idx == 0xFF) {
+    LOOP_XYZE(i) {
+      if (!I2CPE_anyaxis || parser.seen(axis_codes[i])) {
+        const uint8_t idx = idx_from_axis(AxisEnum(i));
+        if ((int8_t)idx >= 0) {
+          const bool ena = onoff == -1 ? !encoders[I2CPE_idx].get_ec_enabled() : !!onoff;
+          enable_ec(idx, ena, AxisEnum(i));
+        }
+      }
+    }
+  }
+  else {
+    const bool ena = onoff == -1 ? !encoders[I2CPE_idx].get_ec_enabled() : !!onoff;
+    enable_ec(I2CPE_idx, ena, encoders[I2CPE_idx].get_axis());
+  }
+}
+
+/**
+ * M868:  Report or set position encoder module error correction
+ *        threshold.
+ *
+ *   A<addr>  Module I2C address.  [30, 200].
+ *   I<index> Module index.  [0, I2CPE_ENCODER_CNT - 1].
+ *   T        New error correction threshold.
+ *
+ *   If A not specified:
+ *    X       Act on X axis encoder, if present.
+ *    Y       Act on Y axis encoder, if present.
+ *    Z       Act on Z axis encoder, if present.
+ *    E       Act on E axis encoder, if present.
+ */
+void I2CPositionEncodersMgr::M868() {
+  if (parse()) return;
+
+  const float newThreshold = parser.seenval('T') ? parser.value_float() : -9999;
+
+  if (I2CPE_idx == 0xFF) {
+    LOOP_XYZE(i) {
+      if (!I2CPE_anyaxis || parser.seen(axis_codes[i])) {
+        const uint8_t idx = idx_from_axis(AxisEnum(i));
+        if ((int8_t)idx >= 0) {
+          if (newThreshold != -9999)
+            set_ec_threshold(idx, newThreshold, encoders[idx].get_axis());
+          else
+            get_ec_threshold(idx, encoders[idx].get_axis());
+        }
+      }
+    }
+  }
+  else if (newThreshold != -9999)
+    set_ec_threshold(I2CPE_idx, newThreshold, encoders[I2CPE_idx].get_axis());
+  else
+    get_ec_threshold(I2CPE_idx, encoders[I2CPE_idx].get_axis());
+}
+
+/**
+ * M869:  Report position encoder module error.
+ *
+ *   A<addr>  Module I2C address.  [30, 200].
+ *   I<index> Module index.  [0, I2CPE_ENCODER_CNT - 1].
+ *
+ *   If A not specified:
+ *    X       Act on X axis encoder, if present.
+ *    Y       Act on Y axis encoder, if present.
+ *    Z       Act on Z axis encoder, if present.
+ *    E       Act on E axis encoder, if present.
+ */
+void I2CPositionEncodersMgr::M869() {
+  if (parse()) return;
+
+  if (I2CPE_idx == 0xFF) {
+    LOOP_XYZE(i) {
+      if (!I2CPE_anyaxis || parser.seen(axis_codes[i])) {
+        const uint8_t idx = idx_from_axis(AxisEnum(i));
+        if ((int8_t)idx >= 0) report_error(idx);
+      }
+    }
+  }
+  else
+    report_error(I2CPE_idx);
+}
+
+#endif // I2C_POSITION_ENCODERS
diff --git a/Marlin/src/feature/encoder_i2c.h b/Marlin/src/feature/encoder_i2c.h
new file mode 100644
index 0000000..511e560
--- /dev/null
+++ b/Marlin/src/feature/encoder_i2c.h
@@ -0,0 +1,320 @@
+/**
+ * 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"
+
+#include "../module/planner.h"
+
+#include <Wire.h>
+
+//=========== Advanced / Less-Common Encoder Configuration Settings ==========
+
+#define I2CPE_EC_THRESH_PROPORTIONAL                    // if enabled adjusts the error correction threshold
+                                                        // proportional to the current speed of the axis allows
+                                                        // for very small error margin at low speeds without
+                                                        // stuttering due to reading latency at high speeds
+
+#define I2CPE_DEBUG                                     // enable encoder-related debug serial echos
+
+#define I2CPE_REBOOT_TIME             5000              // time we wait for an encoder module to reboot
+                                                        // after changing address.
+
+#define I2CPE_MAG_SIG_GOOD            0
+#define I2CPE_MAG_SIG_MID             1
+#define I2CPE_MAG_SIG_BAD             2
+#define I2CPE_MAG_SIG_NF              255
+
+#define I2CPE_REQ_REPORT              0
+#define I2CPE_RESET_COUNT             1
+#define I2CPE_SET_ADDR                2
+#define I2CPE_SET_REPORT_MODE         3
+#define I2CPE_CLEAR_EEPROM            4
+
+#define I2CPE_LED_PAR_MODE            10
+#define I2CPE_LED_PAR_BRT             11
+#define I2CPE_LED_PAR_RATE            14
+
+#define I2CPE_REPORT_DISTANCE         0
+#define I2CPE_REPORT_STRENGTH         1
+#define I2CPE_REPORT_VERSION          2
+
+// Default I2C addresses
+#define I2CPE_PRESET_ADDR_X           30
+#define I2CPE_PRESET_ADDR_Y           31
+#define I2CPE_PRESET_ADDR_Z           32
+#define I2CPE_PRESET_ADDR_E           33
+
+#define I2CPE_DEF_AXIS                X_AXIS
+#define I2CPE_DEF_ADDR                I2CPE_PRESET_ADDR_X
+
+// Error event counter; tracks how many times there is an error exceeding a certain threshold
+#define I2CPE_ERR_CNT_THRESH          3.00
+#define I2CPE_ERR_CNT_DEBOUNCE_MS     2000
+
+#if ENABLED(I2CPE_ERR_ROLLING_AVERAGE)
+  #define I2CPE_ERR_ARRAY_SIZE        32
+  #define I2CPE_ERR_PRST_ARRAY_SIZE   10
+#endif
+
+// Error Correction Methods
+#define I2CPE_ECM_NONE                0
+#define I2CPE_ECM_MICROSTEP           1
+#define I2CPE_ECM_PLANNER             2
+#define I2CPE_ECM_STALLDETECT         3
+
+// Encoder types
+#define I2CPE_ENC_TYPE_ROTARY         0
+#define I2CPE_ENC_TYPE_LINEAR         1
+
+// Parser
+#define I2CPE_PARSE_ERR               1
+#define I2CPE_PARSE_OK                0
+
+#define LOOP_PE(VAR) LOOP_L_N(VAR, I2CPE_ENCODER_CNT)
+#define CHECK_IDX() do{ if (!WITHIN(idx, 0, I2CPE_ENCODER_CNT - 1)) return; }while(0)
+
+typedef union {
+  volatile int32_t val = 0;
+  uint8_t          bval[4];
+} i2cLong;
+
+class I2CPositionEncoder {
+  private:
+    AxisEnum  encoderAxis         = I2CPE_DEF_AXIS;
+
+    uint8_t   i2cAddress          = I2CPE_DEF_ADDR,
+              ecMethod            = I2CPE_DEF_EC_METHOD,
+              type                = I2CPE_DEF_TYPE,
+              H                   = I2CPE_MAG_SIG_NF;    // Magnetic field strength
+
+    int       encoderTicksPerUnit = I2CPE_DEF_ENC_TICKS_UNIT,
+              stepperTicks        = I2CPE_DEF_TICKS_REV,
+              errorCount          = 0,
+              errorPrev           = 0;
+
+    float     ecThreshold         = I2CPE_DEF_EC_THRESH;
+
+    bool      homed               = false,
+              trusted             = false,
+              initialized         = false,
+              active              = false,
+              invert              = false,
+              ec                  = true;
+
+    int32_t   zeroOffset          = 0,
+              lastPosition        = 0,
+              position;
+
+    millis_t  lastPositionTime    = 0,
+              nextErrorCountTime  = 0,
+              lastErrorTime;
+
+    #if ENABLED(I2CPE_ERR_ROLLING_AVERAGE)
+      uint8_t errIdx = 0, errPrstIdx = 0;
+      int err[I2CPE_ERR_ARRAY_SIZE] = { 0 },
+          errPrst[I2CPE_ERR_PRST_ARRAY_SIZE] = { 0 };
+    #endif
+
+  public:
+    void init(const uint8_t address, const AxisEnum axis);
+    void reset();
+
+    void update();
+
+    void set_homed();
+    void set_unhomed();
+
+    int32_t get_raw_count();
+
+    FORCE_INLINE float mm_from_count(const int32_t count) {
+      switch (type) {
+        default: return -1;
+        case I2CPE_ENC_TYPE_LINEAR:
+          return count / encoderTicksPerUnit;
+        case I2CPE_ENC_TYPE_ROTARY:
+          return (count * stepperTicks) / (encoderTicksPerUnit * planner.settings.axis_steps_per_mm[encoderAxis]);
+      }
+    }
+
+    FORCE_INLINE float get_position_mm() { return mm_from_count(get_position()); }
+    FORCE_INLINE int32_t get_position() { return get_raw_count() - zeroOffset; }
+
+    int32_t get_axis_error_steps(const bool report);
+    float get_axis_error_mm(const bool report);
+
+    void calibrate_steps_mm(const uint8_t iter);
+
+    bool passes_test(const bool report);
+
+    bool test_axis();
+
+    FORCE_INLINE int get_error_count() { return errorCount; }
+    FORCE_INLINE void set_error_count(const int newCount) { errorCount = newCount; }
+
+    FORCE_INLINE uint8_t get_address() { return i2cAddress; }
+    FORCE_INLINE void set_address(const uint8_t addr) { i2cAddress = addr; }
+
+    FORCE_INLINE bool get_active() { return active; }
+    FORCE_INLINE void set_active(const bool a) { active = a; }
+
+    FORCE_INLINE void set_inverted(const bool i) { invert = i; }
+
+    FORCE_INLINE AxisEnum get_axis() { return encoderAxis; }
+
+    FORCE_INLINE bool get_ec_enabled() { return ec; }
+    FORCE_INLINE void set_ec_enabled(const bool enabled) { ec = enabled; }
+
+    FORCE_INLINE uint8_t get_ec_method() { return ecMethod; }
+    FORCE_INLINE void set_ec_method(const byte method) { ecMethod = method; }
+
+    FORCE_INLINE float get_ec_threshold() { return ecThreshold; }
+    FORCE_INLINE void set_ec_threshold(const float newThreshold) { ecThreshold = newThreshold; }
+
+    FORCE_INLINE int get_encoder_ticks_mm() {
+      switch (type) {
+        default: return 0;
+        case I2CPE_ENC_TYPE_LINEAR:
+          return encoderTicksPerUnit;
+        case I2CPE_ENC_TYPE_ROTARY:
+          return (int)((encoderTicksPerUnit / stepperTicks) * planner.settings.axis_steps_per_mm[encoderAxis]);
+      }
+    }
+
+    FORCE_INLINE int get_ticks_unit() { return encoderTicksPerUnit; }
+    FORCE_INLINE void set_ticks_unit(const int ticks) { encoderTicksPerUnit = ticks; }
+
+    FORCE_INLINE uint8_t get_type() { return type; }
+    FORCE_INLINE void set_type(const byte newType) { type = newType; }
+
+    FORCE_INLINE int get_stepper_ticks() { return stepperTicks; }
+    FORCE_INLINE void set_stepper_ticks(const int ticks) { stepperTicks = ticks; }
+};
+
+class I2CPositionEncodersMgr {
+  private:
+    static bool I2CPE_anyaxis;
+    static uint8_t I2CPE_addr, I2CPE_idx;
+
+  public:
+
+    static void init();
+
+    // consider only updating one endoder per call / tick if encoders become too time intensive
+    static void update() { LOOP_PE(i) encoders[i].update(); }
+
+    static void homed(const AxisEnum axis) {
+      LOOP_PE(i)
+        if (encoders[i].get_axis() == axis) encoders[i].set_homed();
+    }
+
+    static void unhomed(const AxisEnum axis) {
+      LOOP_PE(i)
+        if (encoders[i].get_axis() == axis) encoders[i].set_unhomed();
+    }
+
+    static void report_position(const int8_t idx, const bool units, const bool noOffset);
+
+    static void report_status(const int8_t idx) {
+      CHECK_IDX();
+      SERIAL_ECHOLNPAIR("Encoder ", idx, ": ");
+      encoders[idx].get_raw_count();
+      encoders[idx].passes_test(true);
+    }
+
+    static void report_error(const int8_t idx) {
+      CHECK_IDX();
+      encoders[idx].get_axis_error_steps(true);
+    }
+
+    static void test_axis(const int8_t idx) {
+      CHECK_IDX();
+      encoders[idx].test_axis();
+    }
+
+    static void calibrate_steps_mm(const int8_t idx, const int iterations) {
+      CHECK_IDX();
+      encoders[idx].calibrate_steps_mm(iterations);
+    }
+
+    static void change_module_address(const uint8_t oldaddr, const uint8_t newaddr);
+    static void report_module_firmware(const uint8_t address);
+
+    static void report_error_count(const int8_t idx, const AxisEnum axis) {
+      CHECK_IDX();
+      SERIAL_ECHOLNPAIR("Error count on ", axis_codes[axis], " axis is ", encoders[idx].get_error_count());
+    }
+
+    static void reset_error_count(const int8_t idx, const AxisEnum axis) {
+      CHECK_IDX();
+      encoders[idx].set_error_count(0);
+      SERIAL_ECHOLNPAIR("Error count on ", axis_codes[axis], " axis has been reset.");
+    }
+
+    static void enable_ec(const int8_t idx, const bool enabled, const AxisEnum axis) {
+      CHECK_IDX();
+      encoders[idx].set_ec_enabled(enabled);
+      SERIAL_ECHOPAIR("Error correction on ", axis_codes[axis]);
+      SERIAL_ECHO_TERNARY(encoders[idx].get_ec_enabled(), " axis is ", "en", "dis", "abled.\n");
+    }
+
+    static void set_ec_threshold(const int8_t idx, const float newThreshold, const AxisEnum axis) {
+      CHECK_IDX();
+      encoders[idx].set_ec_threshold(newThreshold);
+      SERIAL_ECHOLNPAIR("Error correct threshold for ", axis_codes[axis], " axis set to ", newThreshold, "mm.");
+    }
+
+    static void get_ec_threshold(const int8_t idx, const AxisEnum axis) {
+      CHECK_IDX();
+      const float threshold = encoders[idx].get_ec_threshold();
+      SERIAL_ECHOLNPAIR("Error correct threshold for ", axis_codes[axis], " axis is ", threshold, "mm.");
+    }
+
+    static int8_t idx_from_axis(const AxisEnum axis) {
+      LOOP_PE(i)
+        if (encoders[i].get_axis() == axis) return i;
+      return -1;
+    }
+
+    static int8_t idx_from_addr(const uint8_t addr) {
+      LOOP_PE(i)
+        if (encoders[i].get_address() == addr) return i;
+      return -1;
+    }
+
+    static int8_t parse();
+
+    static void M860();
+    static void M861();
+    static void M862();
+    static void M863();
+    static void M864();
+    static void M865();
+    static void M866();
+    static void M867();
+    static void M868();
+    static void M869();
+
+    static I2CPositionEncoder encoders[I2CPE_ENCODER_CNT];
+};
+
+extern I2CPositionEncodersMgr I2CPEM;
diff --git a/Marlin/src/feature/ethernet.cpp b/Marlin/src/feature/ethernet.cpp
new file mode 100644
index 0000000..ff3ba76
--- /dev/null
+++ b/Marlin/src/feature/ethernet.cpp
@@ -0,0 +1,175 @@
+/**
+ * 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"
+
+#if HAS_ETHERNET
+
+#include "ethernet.h"
+#include "../core/serial.h"
+
+#define DEBUG_OUT ENABLED(DEBUG_ETHERNET)
+#include "../core/debug_out.h"
+
+bool MarlinEthernet::hardware_enabled, // = false
+     MarlinEthernet::have_telnet_client; // = false
+
+IPAddress MarlinEthernet::ip,
+          MarlinEthernet::myDns,
+          MarlinEthernet::gateway,
+          MarlinEthernet::subnet;
+
+EthernetClient  MarlinEthernet::telnetClient;  // connected client
+
+MarlinEthernet ethernet;
+
+EthernetServer server(23);    // telnet server
+
+enum linkStates { UNLINKED, LINKING, LINKED, CONNECTING, CONNECTED, NO_HARDWARE } linkState;
+
+#ifdef __IMXRT1062__
+
+  static void teensyMAC(uint8_t * const mac) {
+    const uint32_t m1 = HW_OCOTP_MAC1, m2 = HW_OCOTP_MAC0;
+    mac[0] = m1 >> 8;
+    mac[1] = m1 >> 0;
+    mac[2] = m2 >> 24;
+    mac[3] = m2 >> 16;
+    mac[4] = m2 >> 8;
+    mac[5] = m2 >> 0;
+  }
+
+#else
+
+  byte mac[] = MAC_ADDRESS;
+
+#endif
+
+void ethernet_cable_error() { SERIAL_ERROR_MSG("Ethernet cable is not connected."); }
+
+void MarlinEthernet::init() {
+  if (!hardware_enabled) return;
+
+  SERIAL_ECHO_MSG("Starting network...");
+
+  // Init the Ethernet device
+  #ifdef __IMXRT1062__
+    uint8_t mac[6];
+    teensyMAC(mac);
+  #endif
+
+  if (!ip) {
+    Ethernet.begin(mac); // use DHCP
+  }
+  else {
+    if (!gateway) {
+      gateway = ip;
+      gateway[3] = 1;
+      myDns = gateway;
+      subnet = IPAddress(255,255,255,0);
+    }
+    if (!myDns) myDns = gateway;
+    if (!subnet) subnet = IPAddress(255,255,255,0);
+    Ethernet.begin(mac, ip, myDns, gateway, subnet);
+  }
+
+  // Check for Ethernet hardware present
+  if (Ethernet.hardwareStatus() == EthernetNoHardware) {
+    SERIAL_ERROR_MSG("No Ethernet hardware found.");
+    linkState = NO_HARDWARE;
+    return;
+  }
+
+  linkState = UNLINKED;
+
+  if (Ethernet.linkStatus() == LinkOFF)
+    ethernet_cable_error();
+}
+
+void MarlinEthernet::check() {
+  if (!hardware_enabled) return;
+
+  switch (linkState) {
+    case NO_HARDWARE:
+      break;
+
+    case UNLINKED:
+      if (Ethernet.linkStatus() == LinkOFF) break;
+
+      SERIAL_ECHOLNPGM("Ethernet cable connected");
+      server.begin();
+      linkState = LINKING;
+      break;
+
+    case LINKING:
+      if (!Ethernet.localIP()) break;
+
+      SERIAL_ECHOPGM("Successfully started telnet server with IP ");
+      MYSERIAL0.println(Ethernet.localIP());
+
+      linkState = LINKED;
+      break;
+
+    case LINKED:
+      if (Ethernet.linkStatus() == LinkOFF) {
+        ethernet_cable_error();
+        linkState = UNLINKED;
+        break;
+      }
+      telnetClient = server.accept();
+      if (telnetClient) linkState = CONNECTING;
+      break;
+
+    case CONNECTING:
+      telnetClient.println("Marlin " SHORT_BUILD_VERSION);
+      #if defined(STRING_DISTRIBUTION_DATE) && defined(STRING_CONFIG_H_AUTHOR)
+        telnetClient.println(
+          " Last Updated: " STRING_DISTRIBUTION_DATE
+          " | Author: " STRING_CONFIG_H_AUTHOR
+        );
+      #endif
+      telnetClient.println("Compiled: " __DATE__);
+
+      SERIAL_ECHOLNPGM("Client connected");
+      have_telnet_client = true;
+      linkState = CONNECTED;
+      break;
+
+    case CONNECTED:
+      if (telnetClient && !telnetClient.connected()) {
+        SERIAL_ECHOLNPGM("Client disconnected");
+        telnetClient.stop();
+        have_telnet_client = false;
+        linkState = LINKED;
+      }
+      if (Ethernet.linkStatus() == LinkOFF) {
+        ethernet_cable_error();
+        if (telnetClient) telnetClient.stop();
+        linkState = UNLINKED;
+      }
+      break;
+
+    default: break;
+  }
+}
+
+#endif // HAS_ETHERNET
diff --git a/Marlin/src/feature/ethernet.h b/Marlin/src/feature/ethernet.h
new file mode 100644
index 0000000..70a58ef
--- /dev/null
+++ b/Marlin/src/feature/ethernet.h
@@ -0,0 +1,39 @@
+/**
+ * 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
+
+#ifdef __IMXRT1062__
+  #include <NativeEthernet.h>
+#endif
+
+// Teensy 4.1 uses internal MAC Address
+
+class MarlinEthernet {
+  public:
+    static bool hardware_enabled, have_telnet_client;
+    static IPAddress ip, myDns, gateway, subnet;
+    static EthernetClient telnetClient;
+    static void init();
+    static void check();
+};
+
+extern MarlinEthernet ethernet;
diff --git a/Marlin/src/feature/fanmux.cpp b/Marlin/src/feature/fanmux.cpp
new file mode 100644
index 0000000..43952ca
--- /dev/null
+++ b/Marlin/src/feature/fanmux.cpp
@@ -0,0 +1,55 @@
+/**
+ * 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/>.
+ *
+ */
+
+/**
+ * feature/pause.cpp - Pause feature support functions
+ * This may be combined with related G-codes if features are consolidated.
+ */
+
+#include "../inc/MarlinConfig.h"
+
+#if HAS_FANMUX
+
+#include "fanmux.h"
+
+void fanmux_switch(const uint8_t e) {
+  WRITE(FANMUX0_PIN, TEST(e, 0) ? HIGH : LOW);
+  #if PIN_EXISTS(FANMUX1)
+    WRITE(FANMUX1_PIN, TEST(e, 1) ? HIGH : LOW);
+    #if PIN_EXISTS(FANMUX2)
+      WRITE(FANMUX2_PIN, TEST(e, 2) ? HIGH : LOW);
+    #endif
+  #endif
+}
+
+void fanmux_init() {
+  SET_OUTPUT(FANMUX0_PIN);
+  #if PIN_EXISTS(FANMUX1)
+    SET_OUTPUT(FANMUX1_PIN);
+    #if PIN_EXISTS(FANMUX2)
+      SET_OUTPUT(FANMUX2_PIN);
+    #endif
+  #endif
+  fanmux_switch(0);
+}
+
+#endif // HAS_FANMUX
diff --git a/Marlin/src/feature/fanmux.h b/Marlin/src/feature/fanmux.h
new file mode 100644
index 0000000..b1b0c67
--- /dev/null
+++ b/Marlin/src/feature/fanmux.h
@@ -0,0 +1,29 @@
+/**
+ * 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
+
+/**
+ * feature/fanmux.h - Cooling Fan Multiplexer support functions
+ */
+
+extern void fanmux_switch(const uint8_t e);
+extern void fanmux_init();
diff --git a/Marlin/src/feature/filwidth.cpp b/Marlin/src/feature/filwidth.cpp
new file mode 100644
index 0000000..2bd9c78
--- /dev/null
+++ b/Marlin/src/feature/filwidth.cpp
@@ -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/>.
+ *
+ */
+
+#include "../inc/MarlinConfig.h"
+
+#if ENABLED(FILAMENT_WIDTH_SENSOR)
+
+#include "filwidth.h"
+
+FilamentWidthSensor filwidth;
+
+bool FilamentWidthSensor::enabled; // = false;                          // (M405-M406) Filament Width Sensor ON/OFF.
+uint32_t FilamentWidthSensor::accum; // = 0                             // ADC accumulator
+uint16_t FilamentWidthSensor::raw; // = 0                               // Measured filament diameter - one extruder only
+float FilamentWidthSensor::nominal_mm = DEFAULT_NOMINAL_FILAMENT_DIA,   // (M104) Nominal filament width
+      FilamentWidthSensor::measured_mm = DEFAULT_MEASURED_FILAMENT_DIA, // Measured filament diameter
+      FilamentWidthSensor::e_count = 0,
+      FilamentWidthSensor::delay_dist = 0;
+uint8_t FilamentWidthSensor::meas_delay_cm = MEASUREMENT_DELAY_CM;      // Distance delay setting
+int8_t FilamentWidthSensor::ratios[MAX_MEASUREMENT_DELAY + 1],          // Ring buffer to delay measurement. (Extruder factor minus 100)
+       FilamentWidthSensor::index_r,                                    // Indexes into ring buffer
+       FilamentWidthSensor::index_w;
+
+void FilamentWidthSensor::init() {
+  const int8_t ratio = sample_to_size_ratio();
+  LOOP_L_N(i, COUNT(ratios)) ratios[i] = ratio;
+  index_r = index_w = 0;
+}
+
+#endif // FILAMENT_WIDTH_SENSOR
diff --git a/Marlin/src/feature/filwidth.h b/Marlin/src/feature/filwidth.h
new file mode 100644
index 0000000..ef3859d
--- /dev/null
+++ b/Marlin/src/feature/filwidth.h
@@ -0,0 +1,120 @@
+/**
+ * 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"
+#include "../module/planner.h"
+#include "../module/thermistor/thermistors.h"
+
+class FilamentWidthSensor {
+public:
+  static constexpr int MMD_CM = MAX_MEASUREMENT_DELAY + 1, MMD_MM = MMD_CM * 10;
+  static bool enabled;              // (M405-M406) Filament Width Sensor ON/OFF.
+  static uint32_t accum;            // ADC accumulator
+  static uint16_t raw;              // Measured filament diameter - one extruder only
+  static float nominal_mm,          // (M104) Nominal filament width
+               measured_mm,         // Measured filament diameter
+               e_count, delay_dist;
+  static uint8_t meas_delay_cm;     // Distance delay setting
+  static int8_t ratios[MMD_CM],     // Ring buffer to delay measurement. (Extruder factor minus 100)
+                index_r, index_w;   // Indexes into ring buffer
+
+  FilamentWidthSensor() { init(); }
+  static void init();
+
+  static inline void enable(const bool ena) { enabled = ena; }
+
+  static inline void set_delay_cm(const uint8_t cm) {
+    meas_delay_cm = _MIN(cm, MAX_MEASUREMENT_DELAY);
+  }
+
+  /**
+   * Convert Filament Width (mm) to an extrusion ratio
+   * and reduce to an 8 bit value.
+   *
+   * A nominal width of 1.75 and measured width of 1.73
+   * gives (100 * 1.75 / 1.73) for a ratio of 101 and
+   * a return value of 1.
+   */
+  static int8_t sample_to_size_ratio() {
+    return ABS(nominal_mm - measured_mm) <= FILWIDTH_ERROR_MARGIN
+           ? int(100.0f * nominal_mm / measured_mm) - 100 : 0;
+  }
+
+  // Apply a single ADC reading to the raw value
+  static void accumulate(const uint16_t adc) {
+    if (adc > 102)  // Ignore ADC under 0.5 volts
+      accum += (uint32_t(adc) << 7) - (accum >> 7);
+  }
+
+  // Convert raw measurement to mm
+  static inline float raw_to_mm(const uint16_t v) { return v * 5.0f * RECIPROCAL(float(MAX_RAW_THERMISTOR_VALUE)); }
+  static inline float raw_to_mm() { return raw_to_mm(raw); }
+
+  // A scaled reading is ready
+  // Divide to get to 0-16384 range since we used 1/128 IIR filter approach
+  static inline void reading_ready() { raw = accum >> 10; }
+
+  // Update mm from the raw measurement
+  static inline void update_measured_mm() { measured_mm = raw_to_mm(); }
+
+  // Update ring buffer used to delay filament measurements
+  static inline void advance_e(const float &e_move) {
+
+    // Increment counters with the E distance
+    e_count += e_move;
+    delay_dist += e_move;
+
+    // Only get new measurements on forward E movement
+    if (!UNEAR_ZERO(e_count)) {
+
+      // Loop the delay distance counter (modulus by the mm length)
+      while (delay_dist >= MMD_MM) delay_dist -= MMD_MM;
+
+      // Convert into an index (cm) into the measurement array
+      index_r = int8_t(delay_dist * 0.1f);
+
+      // If the ring buffer is not full...
+      if (index_r != index_w) {
+        e_count = 0;                            // Reset the E movement counter
+        const int8_t meas_sample = sample_to_size_ratio();
+        do {
+          if (++index_w >= MMD_CM) index_w = 0; // The next unused slot
+          ratios[index_w] = meas_sample;        // Store the measurement
+        } while (index_r != index_w);           // More slots to fill?
+      }
+    }
+  }
+
+  // Dynamically set the volumetric multiplier based on the delayed width measurement.
+  static inline void update_volumetric() {
+    if (enabled) {
+      int8_t read_index = index_r - meas_delay_cm;
+      if (read_index < 0) read_index += MMD_CM; // Loop around buffer if needed
+      LIMIT(read_index, 0, MAX_MEASUREMENT_DELAY);
+      planner.apply_filament_width_sensor(ratios[read_index]);
+    }
+  }
+
+};
+
+extern FilamentWidthSensor filwidth;
diff --git a/Marlin/src/feature/fwretract.cpp b/Marlin/src/feature/fwretract.cpp
new file mode 100644
index 0000000..2a71af1
--- /dev/null
+++ b/Marlin/src/feature/fwretract.cpp
@@ -0,0 +1,201 @@
+/**
+ * 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/>.
+ *
+ */
+
+/**
+ * fwretract.cpp - Implement firmware-based retraction
+ */
+
+#include "../inc/MarlinConfig.h"
+
+#if ENABLED(FWRETRACT)
+
+#include "fwretract.h"
+
+FWRetract fwretract; // Single instance - this calls the constructor
+
+#include "../module/motion.h"
+#include "../module/planner.h"
+#include "../module/stepper.h"
+
+#if ENABLED(RETRACT_SYNC_MIXING)
+  #include "mixing.h"
+#endif
+
+// private:
+
+#if HAS_MULTI_EXTRUDER
+  bool FWRetract::retracted_swap[EXTRUDERS];          // Which extruders are swap-retracted
+#endif
+
+// public:
+
+fwretract_settings_t FWRetract::settings;             // M207 S F Z W, M208 S F W R
+
+#if ENABLED(FWRETRACT_AUTORETRACT)
+  bool FWRetract::autoretract_enabled;                // M209 S - Autoretract switch
+#endif
+
+bool FWRetract::retracted[EXTRUDERS];                 // Which extruders are currently retracted
+
+float FWRetract::current_retract[EXTRUDERS],          // Retract value used by planner
+      FWRetract::current_hop;
+
+void FWRetract::reset() {
+  TERN_(FWRETRACT_AUTORETRACT, autoretract_enabled = false);
+  settings.retract_length = RETRACT_LENGTH;
+  settings.retract_feedrate_mm_s = RETRACT_FEEDRATE;
+  settings.retract_zraise = RETRACT_ZRAISE;
+  settings.retract_recover_extra = RETRACT_RECOVER_LENGTH;
+  settings.retract_recover_feedrate_mm_s = RETRACT_RECOVER_FEEDRATE;
+  settings.swap_retract_length = RETRACT_LENGTH_SWAP;
+  settings.swap_retract_recover_extra = RETRACT_RECOVER_LENGTH_SWAP;
+  settings.swap_retract_recover_feedrate_mm_s = RETRACT_RECOVER_FEEDRATE_SWAP;
+  current_hop = 0.0;
+
+  LOOP_L_N(i, EXTRUDERS) {
+    retracted[i] = false;
+    TERN_(HAS_MULTI_EXTRUDER, retracted_swap[i] = false);
+    current_retract[i] = 0.0;
+  }
+}
+
+/**
+ * Retract or recover according to firmware settings
+ *
+ * This function handles retract/recover moves for G10 and G11,
+ * plus auto-retract moves sent from G0/G1 when E-only moves are done.
+ *
+ * To simplify the logic, doubled retract/recover moves are ignored.
+ *
+ * Note: Auto-retract will apply the set Z hop in addition to any Z hop
+ *       included in the G-code. Use M207 Z0 to to prevent double hop.
+ */
+void FWRetract::retract(const bool retracting
+  #if HAS_MULTI_EXTRUDER
+    , bool swapping/*=false*/
+  #endif
+) {
+  // Prevent two retracts or recovers in a row
+  if (retracted[active_extruder] == retracting) return;
+
+  // Prevent two swap-retract or recovers in a row
+  #if HAS_MULTI_EXTRUDER
+    // Allow G10 S1 only after G11
+    if (swapping && retracted_swap[active_extruder] == retracting) return;
+    // G11 priority to recover the long retract if activated
+    if (!retracting) swapping = retracted_swap[active_extruder];
+  #else
+    constexpr bool swapping = false;
+  #endif
+
+  /* // debugging
+    SERIAL_ECHOLNPAIR(
+      "retracting ", retracting,
+      " swapping ", swapping,
+      " active extruder ", active_extruder
+    );
+    LOOP_L_N(i, EXTRUDERS) {
+      SERIAL_ECHOLNPAIR("retracted[", i, "] ", retracted[i]);
+      #if HAS_MULTI_EXTRUDER
+        SERIAL_ECHOLNPAIR("retracted_swap[", i, "] ", retracted_swap[i]);
+      #endif
+    }
+    SERIAL_ECHOLNPAIR("current_position.z ", current_position.z);
+    SERIAL_ECHOLNPAIR("current_position.e ", current_position.e);
+    SERIAL_ECHOLNPAIR("current_hop ", current_hop);
+  //*/
+
+  const float base_retract = TERN1(RETRACT_SYNC_MIXING, (MIXING_STEPPERS))
+                * (swapping ? settings.swap_retract_length : settings.retract_length);
+
+  // The current position will be the destination for E and Z moves
+  destination = current_position;
+
+  #if ENABLED(RETRACT_SYNC_MIXING)
+    const uint8_t old_mixing_tool = mixer.get_current_vtool();
+    mixer.T(MIXER_AUTORETRACT_TOOL);
+  #endif
+
+  const feedRate_t fr_max_z = planner.settings.max_feedrate_mm_s[Z_AXIS];
+  if (retracting) {
+    // Retract by moving from a faux E position back to the current E position
+    current_retract[active_extruder] = base_retract;
+    prepare_internal_move_to_destination(                 // set current from destination
+      settings.retract_feedrate_mm_s * TERN1(RETRACT_SYNC_MIXING, (MIXING_STEPPERS))
+    );
+
+    // Is a Z hop set, and has the hop not yet been done?
+    if (!current_hop && settings.retract_zraise > 0.01f) {  // Apply hop only once
+      current_hop += settings.retract_zraise;               // Add to the hop total (again, only once)
+      // Raise up, set_current_to_destination. Maximum Z feedrate
+      prepare_internal_move_to_destination(fr_max_z);
+    }
+  }
+  else {
+    // If a hop was done and Z hasn't changed, undo the Z hop
+    if (current_hop) {
+      current_hop = 0;
+      // Lower Z, set_current_to_destination. Maximum Z feedrate
+      prepare_internal_move_to_destination(fr_max_z);
+    }
+
+    const float extra_recover = swapping ? settings.swap_retract_recover_extra : settings.retract_recover_extra;
+    if (extra_recover) {
+      current_position.e -= extra_recover;          // Adjust the current E position by the extra amount to recover
+      sync_plan_position_e();                             // Sync the planner position so the extra amount is recovered
+    }
+
+    current_retract[active_extruder] = 0;
+
+    // Recover E, set_current_to_destination
+    prepare_internal_move_to_destination(
+      (swapping ? settings.swap_retract_recover_feedrate_mm_s : settings.retract_recover_feedrate_mm_s)
+      * TERN1(RETRACT_SYNC_MIXING, (MIXING_STEPPERS))
+    );
+  }
+
+  TERN_(RETRACT_SYNC_MIXING, mixer.T(old_mixing_tool));   // Restore original mixing tool
+
+  retracted[active_extruder] = retracting;                // Active extruder now retracted / recovered
+
+  // If swap retract/recover update the retracted_swap flag too
+  #if HAS_MULTI_EXTRUDER
+    if (swapping) retracted_swap[active_extruder] = retracting;
+  #endif
+
+  /* // debugging
+    SERIAL_ECHOLNPAIR("retracting ", retracting);
+    SERIAL_ECHOLNPAIR("swapping ", swapping);
+    SERIAL_ECHOLNPAIR("active_extruder ", active_extruder);
+    LOOP_L_N(i, EXTRUDERS) {
+      SERIAL_ECHOLNPAIR("retracted[", i, "] ", retracted[i]);
+      #if HAS_MULTI_EXTRUDER
+        SERIAL_ECHOLNPAIR("retracted_swap[", i, "] ", retracted_swap[i]);
+      #endif
+    }
+    SERIAL_ECHOLNPAIR("current_position.z ", current_position.z);
+    SERIAL_ECHOLNPAIR("current_position.e ", current_position.e);
+    SERIAL_ECHOLNPAIR("current_hop ", current_hop);
+  //*/
+}
+
+#endif // FWRETRACT
diff --git a/Marlin/src/feature/fwretract.h b/Marlin/src/feature/fwretract.h
new file mode 100644
index 0000000..1348519
--- /dev/null
+++ b/Marlin/src/feature/fwretract.h
@@ -0,0 +1,86 @@
+/**
+ * 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
+
+/**
+ * fwretract.h - Define firmware-based retraction interface
+ */
+
+#include "../inc/MarlinConfigPre.h"
+
+typedef struct {
+       float retract_length;                      // M207 S - G10 Retract length
+  feedRate_t retract_feedrate_mm_s;               // M207 F - G10 Retract feedrate
+       float retract_zraise,                      // M207 Z - G10 Retract hop size
+             retract_recover_extra;               // M208 S - G11 Recover length
+  feedRate_t retract_recover_feedrate_mm_s;       // M208 F - G11 Recover feedrate
+       float swap_retract_length,                 // M207 W - G10 Swap Retract length
+             swap_retract_recover_extra;          // M208 W - G11 Swap Recover length
+  feedRate_t swap_retract_recover_feedrate_mm_s;  // M208 R - G11 Swap Recover feedrate
+} fwretract_settings_t;
+
+#if ENABLED(FWRETRACT)
+
+class FWRetract {
+private:
+  #if HAS_MULTI_EXTRUDER
+    static bool retracted_swap[EXTRUDERS];         // Which extruders are swap-retracted
+  #endif
+
+public:
+  static fwretract_settings_t settings;
+
+  #if ENABLED(FWRETRACT_AUTORETRACT)
+    static bool autoretract_enabled;               // M209 S - Autoretract switch
+  #else
+    static constexpr bool autoretract_enabled = false;
+  #endif
+
+  static bool retracted[EXTRUDERS];                // Which extruders are currently retracted
+  static float current_retract[EXTRUDERS],         // Retract value used by planner
+               current_hop;                        // Hop value used by planner
+
+  FWRetract() { reset(); }
+
+  static void reset();
+
+  static void refresh_autoretract() {
+    LOOP_L_N(i, EXTRUDERS) retracted[i] = false;
+  }
+
+  static void enable_autoretract(const bool enable) {
+    #if ENABLED(FWRETRACT_AUTORETRACT)
+      autoretract_enabled = enable;
+      refresh_autoretract();
+    #endif
+  }
+
+  static void retract(const bool retracting
+    #if HAS_MULTI_EXTRUDER
+      , bool swapping = false
+    #endif
+  );
+};
+
+extern FWRetract fwretract;
+
+#endif // FWRETRACT
diff --git a/Marlin/src/feature/host_actions.cpp b/Marlin/src/feature/host_actions.cpp
new file mode 100644
index 0000000..5ba3a3e
--- /dev/null
+++ b/Marlin/src/feature/host_actions.cpp
@@ -0,0 +1,202 @@
+/**
+ * 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 ENABLED(HOST_ACTION_COMMANDS)
+
+#include "host_actions.h"
+
+//#define DEBUG_HOST_ACTIONS
+
+#if ENABLED(ADVANCED_PAUSE_FEATURE)
+  #include "pause.h"
+  #include "../gcode/queue.h"
+#endif
+
+#if HAS_FILAMENT_SENSOR
+  #include "runout.h"
+#endif
+
+void host_action(PGM_P const pstr, const bool eol) {
+  PORT_REDIRECT(SERIAL_ALL);
+  SERIAL_ECHOPGM("//action:");
+  serialprintPGM(pstr);
+  if (eol) SERIAL_EOL();
+}
+
+#ifdef ACTION_ON_KILL
+  void host_action_kill() { host_action(PSTR(ACTION_ON_KILL)); }
+#endif
+#ifdef ACTION_ON_PAUSE
+  void host_action_pause(const bool eol/*=true*/) { host_action(PSTR(ACTION_ON_PAUSE), eol); }
+#endif
+#ifdef ACTION_ON_PAUSED
+  void host_action_paused(const bool eol/*=true*/) { host_action(PSTR(ACTION_ON_PAUSED), eol); }
+#endif
+#ifdef ACTION_ON_RESUME
+  void host_action_resume() { host_action(PSTR(ACTION_ON_RESUME)); }
+#endif
+#ifdef ACTION_ON_RESUMED
+  void host_action_resumed() { host_action(PSTR(ACTION_ON_RESUMED)); }
+#endif
+#ifdef ACTION_ON_CANCEL
+  void host_action_cancel() { host_action(PSTR(ACTION_ON_CANCEL)); }
+#endif
+#ifdef ACTION_ON_START
+  void host_action_start() { host_action(PSTR(ACTION_ON_START)); }
+#endif
+
+#if ENABLED(HOST_PROMPT_SUPPORT)
+
+  PGMSTR(CONTINUE_STR, "Continue");
+  PGMSTR(DISMISS_STR, "Dismiss");
+
+  #if HAS_RESUME_CONTINUE
+    extern bool wait_for_user;
+  #endif
+
+  PromptReason host_prompt_reason = PROMPT_NOT_DEFINED;
+
+  void host_action_notify(const char * const message) {
+    PORT_REDIRECT(SERIAL_ALL);
+    host_action(PSTR("notification "), false);
+    SERIAL_ECHOLN(message);
+  }
+
+  void host_action_notify_P(PGM_P const message) {
+    PORT_REDIRECT(SERIAL_ALL);
+    host_action(PSTR("notification "), false);
+    serialprintPGM(message);
+    SERIAL_EOL();
+  }
+
+  void host_action_prompt(PGM_P const ptype, const bool eol=true) {
+    PORT_REDIRECT(SERIAL_ALL);
+    host_action(PSTR("prompt_"), false);
+    serialprintPGM(ptype);
+    if (eol) SERIAL_EOL();
+  }
+
+  void host_action_prompt_plus(PGM_P const ptype, PGM_P const pstr, const char extra_char='\0') {
+    host_action_prompt(ptype, false);
+    PORT_REDIRECT(SERIAL_ALL);
+    SERIAL_CHAR(' ');
+    serialprintPGM(pstr);
+    if (extra_char != '\0') SERIAL_CHAR(extra_char);
+    SERIAL_EOL();
+  }
+  void host_action_prompt_begin(const PromptReason reason, PGM_P const pstr, const char extra_char/*='\0'*/) {
+    host_action_prompt_end();
+    host_prompt_reason = reason;
+    host_action_prompt_plus(PSTR("begin"), pstr, extra_char);
+  }
+  void host_action_prompt_button(PGM_P const pstr) { host_action_prompt_plus(PSTR("button"), pstr); }
+  void host_action_prompt_end() { host_action_prompt(PSTR("end")); }
+  void host_action_prompt_show() { host_action_prompt(PSTR("show")); }
+
+  void _host_prompt_show(PGM_P const btn1/*=nullptr*/, PGM_P const btn2/*=nullptr*/) {
+    if (btn1) host_action_prompt_button(btn1);
+    if (btn2) host_action_prompt_button(btn2);
+    host_action_prompt_show();
+  }
+  void host_prompt_do(const PromptReason reason, PGM_P const pstr, PGM_P const btn1/*=nullptr*/, PGM_P const btn2/*=nullptr*/) {
+    host_action_prompt_begin(reason, pstr);
+    _host_prompt_show(btn1, btn2);
+  }
+  void host_prompt_do(const PromptReason reason, PGM_P const pstr, const char extra_char, PGM_P const btn1/*=nullptr*/, PGM_P const btn2/*=nullptr*/) {
+    host_action_prompt_begin(reason, pstr, extra_char);
+    _host_prompt_show(btn1, btn2);
+  }
+
+  void filament_load_host_prompt() {
+    const bool disable_to_continue = TERN0(HAS_FILAMENT_SENSOR, runout.filament_ran_out);
+    host_prompt_do(PROMPT_FILAMENT_RUNOUT, PSTR("Paused"), PSTR("PurgeMore"),
+      disable_to_continue ? PSTR("DisableRunout") : CONTINUE_STR
+    );
+  }
+
+  //
+  // Handle responses from the host, such as:
+  //  - Filament runout responses: Purge More, Continue
+  //  - General "Continue" response
+  //  - Resume Print response
+  //  - Dismissal of info
+  //
+  void host_response_handler(const uint8_t response) {
+    #ifdef DEBUG_HOST_ACTIONS
+      static PGMSTR(m876_prefix, "M876 Handle Re");
+      serialprintPGM(m876_prefix); SERIAL_ECHOLNPAIR("ason: ", host_prompt_reason);
+      serialprintPGM(m876_prefix); SERIAL_ECHOLNPAIR("sponse: ", response);
+    #endif
+    PGM_P msg = PSTR("UNKNOWN STATE");
+    const PromptReason hpr = host_prompt_reason;
+    host_prompt_reason = PROMPT_NOT_DEFINED;  // Reset now ahead of logic
+    switch (hpr) {
+      case PROMPT_FILAMENT_RUNOUT:
+        msg = PSTR("FILAMENT_RUNOUT");
+        switch (response) {
+
+          case 0: // "Purge More" button
+            #if BOTH(HAS_LCD_MENU, ADVANCED_PAUSE_FEATURE)
+              pause_menu_response = PAUSE_RESPONSE_EXTRUDE_MORE;  // Simulate menu selection (menu exits, doesn't extrude more)
+            #endif
+            filament_load_host_prompt();                          // Initiate another host prompt. (NOTE: The loop in load_filament may also do this!)
+            break;
+
+          case 1: // "Continue" / "Disable Runout" button
+            #if BOTH(HAS_LCD_MENU, ADVANCED_PAUSE_FEATURE)
+              pause_menu_response = PAUSE_RESPONSE_RESUME_PRINT;  // Simulate menu selection
+            #endif
+            #if HAS_FILAMENT_SENSOR
+              if (runout.filament_ran_out) {                      // Disable a triggered sensor
+                runout.enabled = false;
+                runout.reset();
+              }
+            #endif
+            break;
+        }
+        break;
+      case PROMPT_USER_CONTINUE:
+        TERN_(HAS_RESUME_CONTINUE, wait_for_user = false);
+        msg = PSTR("FILAMENT_RUNOUT_CONTINUE");
+        break;
+      case PROMPT_PAUSE_RESUME:
+        msg = PSTR("LCD_PAUSE_RESUME");
+        #if ENABLED(ADVANCED_PAUSE_FEATURE)
+          extern const char M24_STR[];
+          queue.inject_P(M24_STR);
+        #endif
+        break;
+      case PROMPT_INFO:
+        msg = PSTR("GCODE_INFO");
+        break;
+      default: break;
+    }
+    SERIAL_ECHOPGM("M876 Responding PROMPT_");
+    serialprintPGM(msg);
+    SERIAL_EOL();
+  }
+
+#endif // HOST_PROMPT_SUPPORT
+
+#endif // HOST_ACTION_COMMANDS
diff --git a/Marlin/src/feature/host_actions.h b/Marlin/src/feature/host_actions.h
new file mode 100644
index 0000000..065b59d
--- /dev/null
+++ b/Marlin/src/feature/host_actions.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
+
+#include "../inc/MarlinConfigPre.h"
+#include "../HAL/shared/Marduino.h"
+
+void host_action(PGM_P const pstr, const bool eol=true);
+
+#ifdef ACTION_ON_KILL
+  void host_action_kill();
+#endif
+#ifdef ACTION_ON_PAUSE
+  void host_action_pause(const bool eol=true);
+#endif
+#ifdef ACTION_ON_PAUSED
+  void host_action_paused(const bool eol=true);
+#endif
+#ifdef ACTION_ON_RESUME
+  void host_action_resume();
+#endif
+#ifdef ACTION_ON_RESUMED
+  void host_action_resumed();
+#endif
+#ifdef ACTION_ON_CANCEL
+  void host_action_cancel();
+#endif
+#ifdef ACTION_ON_START
+  void host_action_start();
+#endif
+
+#if ENABLED(HOST_PROMPT_SUPPORT)
+
+  extern const char CONTINUE_STR[], DISMISS_STR[];
+
+  enum PromptReason : uint8_t {
+    PROMPT_NOT_DEFINED,
+    PROMPT_FILAMENT_RUNOUT,
+    PROMPT_USER_CONTINUE,
+    PROMPT_FILAMENT_RUNOUT_REHEAT,
+    PROMPT_PAUSE_RESUME,
+    PROMPT_INFO
+  };
+
+  extern PromptReason host_prompt_reason;
+
+  void host_response_handler(const uint8_t response);
+  void host_action_notify(const char * const message);
+  void host_action_notify_P(PGM_P const message);
+  void host_action_prompt_begin(const PromptReason reason, PGM_P const pstr, const char extra_char='\0');
+  void host_action_prompt_button(PGM_P const pstr);
+  void host_action_prompt_end();
+  void host_action_prompt_show();
+  void host_prompt_do(const PromptReason reason, PGM_P const pstr, PGM_P const btn1=nullptr, PGM_P const btn2=nullptr);
+  void host_prompt_do(const PromptReason reason, PGM_P const pstr, const char extra_char, PGM_P const btn1=nullptr, PGM_P const btn2=nullptr);
+  inline void host_prompt_open(const PromptReason reason, PGM_P const pstr, PGM_P const btn1=nullptr, PGM_P const btn2=nullptr) {
+    if (host_prompt_reason == PROMPT_NOT_DEFINED) host_prompt_do(reason, pstr, btn1, btn2);
+  }
+
+  void filament_load_host_prompt();
+
+#endif
diff --git a/Marlin/src/feature/hotend_idle.cpp b/Marlin/src/feature/hotend_idle.cpp
new file mode 100644
index 0000000..7f8f20a
--- /dev/null
+++ b/Marlin/src/feature/hotend_idle.cpp
@@ -0,0 +1,89 @@
+/**
+ * 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/>.
+ *
+ */
+
+/**
+ * Hotend Idle Timeout
+ * Prevent filament in the nozzle from charring and causing a critical jam.
+ */
+
+#include "../inc/MarlinConfig.h"
+
+#if ENABLED(HOTEND_IDLE_TIMEOUT)
+
+#include "hotend_idle.h"
+#include "../gcode/gcode.h"
+
+#include "../module/temperature.h"
+#include "../module/motion.h"
+#include "../lcd/marlinui.h"
+
+extern HotendIdleProtection hotend_idle;
+
+millis_t HotendIdleProtection::next_protect_ms = 0;
+
+void HotendIdleProtection::check_hotends(const millis_t &ms) {
+  bool do_prot = false;
+  HOTEND_LOOP() {
+    if (thermalManager.degHotend(e) >= HOTEND_IDLE_MIN_TRIGGER) {
+      do_prot = true; break;
+    }
+  }
+  if (bool(next_protect_ms) != do_prot)
+    next_protect_ms = do_prot ? ms + hp_interval : 0;
+}
+
+void HotendIdleProtection::check_e_motion(const millis_t &ms) {
+  static float old_e_position = 0;
+  if (old_e_position != current_position.e) {
+    old_e_position = current_position.e;          // Track filament motion
+    if (next_protect_ms)                          // If some heater is on then...
+      next_protect_ms = ms + hp_interval;         // ...delay the timeout till later
+  }
+}
+
+void HotendIdleProtection::check() {
+  const millis_t ms = millis();                   // Shared millis
+
+  check_hotends(ms);                              // Any hotends need protection?
+  check_e_motion(ms);                             // Motion will protect them
+
+  // Hot and not moving for too long...
+  if (next_protect_ms && ELAPSED(ms, next_protect_ms))
+    timed_out();
+}
+
+// Lower (but don't raise) hotend / bed temperatures
+void HotendIdleProtection::timed_out() {
+  next_protect_ms = 0;
+  SERIAL_ECHOLNPGM("Hotend Idle Timeout");
+  LCD_MESSAGEPGM(MSG_HOTEND_IDLE_TIMEOUT);
+  HOTEND_LOOP() {
+    if ((HOTEND_IDLE_NOZZLE_TARGET) < thermalManager.degTargetHotend(e))
+      thermalManager.setTargetHotend(HOTEND_IDLE_NOZZLE_TARGET, e);
+  }
+  #if HAS_HEATED_BED
+    if ((HOTEND_IDLE_BED_TARGET) < thermalManager.degTargetBed())
+      thermalManager.setTargetBed(HOTEND_IDLE_BED_TARGET);
+  #endif
+}
+
+#endif // HOTEND_IDLE_TIMEOUT
diff --git a/Marlin/src/feature/hotend_idle.h b/Marlin/src/feature/hotend_idle.h
new file mode 100644
index 0000000..40f557d
--- /dev/null
+++ b/Marlin/src/feature/hotend_idle.h
@@ -0,0 +1,37 @@
+/**
+ * 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 "../core/millis_t.h"
+
+class HotendIdleProtection {
+public:
+  static void check();
+private:
+  static constexpr millis_t hp_interval = SEC_TO_MS(HOTEND_IDLE_TIMEOUT_SEC);
+  static millis_t next_protect_ms;
+  static void check_hotends(const millis_t &ms);
+  static void check_e_motion(const millis_t &ms);
+  static void timed_out();
+};
+
+extern HotendIdleProtection hotend_idle;
diff --git a/Marlin/src/feature/joystick.cpp b/Marlin/src/feature/joystick.cpp
new file mode 100644
index 0000000..3dca2eb
--- /dev/null
+++ b/Marlin/src/feature/joystick.cpp
@@ -0,0 +1,188 @@
+/**
+ * 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/>.
+ *
+ */
+
+/**
+ * joystick.cpp - joystick input / jogging
+ */
+
+#include "../inc/MarlinConfigPre.h"
+
+#if ENABLED(JOYSTICK)
+
+#include "joystick.h"
+
+#include "../inc/MarlinConfig.h"  // for pins
+#include "../module/planner.h"
+
+Joystick joystick;
+
+#if ENABLED(EXTENSIBLE_UI)
+  #include "../lcd/extui/ui_api.h"
+#endif
+
+#if HAS_JOY_ADC_X
+  temp_info_t Joystick::x; // = { 0 }
+  #if ENABLED(INVERT_JOY_X)
+    #define JOY_X(N) (16383 - (N))
+  #else
+    #define JOY_X(N) (N)
+  #endif
+#endif
+#if HAS_JOY_ADC_Y
+  temp_info_t Joystick::y; // = { 0 }
+  #if ENABLED(INVERT_JOY_Y)
+    #define JOY_Y(N) (16383 - (N))
+  #else
+    #define JOY_Y(N) (N)
+  #endif
+#endif
+#if HAS_JOY_ADC_Z
+  temp_info_t Joystick::z; // = { 0 }
+  #if ENABLED(INVERT_JOY_Z)
+    #define JOY_Z(N) (16383 - (N))
+  #else
+    #define JOY_Z(N) (N)
+  #endif
+#endif
+
+#if ENABLED(JOYSTICK_DEBUG)
+  void Joystick::report() {
+    SERIAL_ECHOPGM("Joystick");
+    #if HAS_JOY_ADC_X
+      SERIAL_ECHOPAIR_P(SP_X_STR, JOY_X(x.raw));
+    #endif
+    #if HAS_JOY_ADC_Y
+      SERIAL_ECHOPAIR_P(SP_Y_STR, JOY_Y(y.raw));
+    #endif
+    #if HAS_JOY_ADC_Z
+      SERIAL_ECHOPAIR_P(SP_Z_STR, JOY_Z(z.raw));
+    #endif
+    #if HAS_JOY_ADC_EN
+      SERIAL_ECHO_TERNARY(READ(JOY_EN_PIN), " EN=", "HIGH (dis", "LOW (en", "abled)");
+    #endif
+    SERIAL_EOL();
+  }
+#endif
+
+#if HAS_JOY_ADC_X || HAS_JOY_ADC_Y || HAS_JOY_ADC_Z
+
+  void Joystick::calculate(xyz_float_t &norm_jog) {
+    // Do nothing if enable pin (active-low) is not LOW
+    #if HAS_JOY_ADC_EN
+      if (READ(JOY_EN_PIN)) return;
+    #endif
+
+    auto _normalize_joy = [](float &axis_jog, const int16_t raw, const int16_t (&joy_limits)[4]) {
+      if (WITHIN(raw, joy_limits[0], joy_limits[3])) {
+        // within limits, check deadzone
+        if (raw > joy_limits[2])
+          axis_jog = (raw - joy_limits[2]) / float(joy_limits[3] - joy_limits[2]);
+        else if (raw < joy_limits[1])
+          axis_jog = (raw - joy_limits[1]) / float(joy_limits[1] - joy_limits[0]);  // negative value
+        // Map normal to jog value via quadratic relationship
+        axis_jog = SIGN(axis_jog) * sq(axis_jog);
+      }
+    };
+
+    #if HAS_JOY_ADC_X
+      static constexpr int16_t joy_x_limits[4] = JOY_X_LIMITS;
+      _normalize_joy(norm_jog.x, JOY_X(x.raw), joy_x_limits);
+    #endif
+    #if HAS_JOY_ADC_Y
+      static constexpr int16_t joy_y_limits[4] = JOY_Y_LIMITS;
+      _normalize_joy(norm_jog.y, JOY_Y(y.raw), joy_y_limits);
+    #endif
+    #if HAS_JOY_ADC_Z
+      static constexpr int16_t joy_z_limits[4] = JOY_Z_LIMITS;
+      _normalize_joy(norm_jog.z, JOY_Z(z.raw), joy_z_limits);
+    #endif
+  }
+
+#endif
+
+#if ENABLED(POLL_JOG)
+
+  void Joystick::inject_jog_moves() {
+    // Recursion barrier
+    static bool injecting_now; // = false;
+    if (injecting_now) return;
+
+    #if ENABLED(NO_MOTION_BEFORE_HOMING)
+      if (TERN0(HAS_JOY_ADC_X, axis_should_home(X_AXIS)) || TERN0(HAS_JOY_ADC_Y, axis_should_home(Y_AXIS)) || TERN0(HAS_JOY_ADC_Z, axis_should_home(Z_AXIS)))
+        return;
+    #endif
+
+    static constexpr int QUEUE_DEPTH = 5;                                // Insert up to this many movements
+    static constexpr float target_lag = 0.25f,                           // Aim for 1/4 second lag
+                           seg_time = target_lag / QUEUE_DEPTH;          // 0.05 seconds, short segments inserted every 1/20th of a second
+    static constexpr millis_t timer_limit_ms = millis_t(seg_time * 500); // 25 ms minimum delay between insertions
+
+    // The planner can merge/collapse small moves, so the movement queue is unreliable to control the lag
+    static millis_t next_run = 0;
+    if (PENDING(millis(), next_run)) return;
+    next_run = millis() + timer_limit_ms;
+
+    // Only inject a command if the planner has fewer than 5 moves and there are no unparsed commands
+    if (planner.movesplanned() >= QUEUE_DEPTH || queue.has_commands_queued())
+      return;
+
+    // Normalized jog values are 0 for no movement and -1 or +1 for as max feedrate (nonlinear relationship)
+    // Jog are initialized to zero and handling input can update values but doesn't have to
+    // You could use a two-axis joystick and a one-axis keypad and they might work together
+    xyz_float_t norm_jog{0};
+
+    // Use ADC values and defined limits. The active zone is normalized: -1..0 (dead) 0..1
+    #if HAS_JOY_ADC_X || HAS_JOY_ADC_Y || HAS_JOY_ADC_Z
+      joystick.calculate(norm_jog);
+    #endif
+
+    // Other non-joystick poll-based jogging could be implemented here
+    // with "jogging" encapsulated as a more general class.
+
+    TERN_(EXTENSIBLE_UI, ExtUI::_joystick_update(norm_jog));
+
+    // norm_jog values of [-1 .. 1] maps linearly to [-feedrate .. feedrate]
+    xyz_float_t move_dist{0};
+    float hypot2 = 0;
+    LOOP_XYZ(i) if (norm_jog[i]) {
+      move_dist[i] = seg_time * norm_jog[i] *
+        #if ENABLED(EXTENSIBLE_UI)
+          manual_feedrate_mm_s[i];
+        #else
+          planner.settings.max_feedrate_mm_s[i];
+        #endif
+      hypot2 += sq(move_dist[i]);
+    }
+
+    if (!UNEAR_ZERO(hypot2)) {
+      current_position += move_dist;
+      apply_motion_limits(current_position);
+      const float length = sqrt(hypot2);
+      injecting_now = true;
+      planner.buffer_line(current_position, length / seg_time, active_extruder, length);
+      injecting_now = false;
+    }
+  }
+
+#endif // POLL_JOG
+
+#endif // JOYSTICK
diff --git a/Marlin/src/feature/joystick.h b/Marlin/src/feature/joystick.h
new file mode 100644
index 0000000..e8e218b
--- /dev/null
+++ b/Marlin/src/feature/joystick.h
@@ -0,0 +1,44 @@
+/**
+ * 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
+
+/**
+ * joystick.h - joystick input / jogging
+ */
+
+#include "../inc/MarlinConfigPre.h"
+#include "../core/types.h"
+#include "../module/temperature.h"
+
+class Joystick {
+  friend class Temperature;
+  private:
+    TERN_(HAS_JOY_ADC_X, static temp_info_t x);
+    TERN_(HAS_JOY_ADC_Y, static temp_info_t y);
+    TERN_(HAS_JOY_ADC_Z, static temp_info_t z);
+  public:
+    TERN_(JOYSTICK_DEBUG, static void report());
+    static void calculate(xyz_float_t &norm_jog);
+    static void inject_jog_moves();
+};
+
+extern Joystick joystick;
diff --git a/Marlin/src/feature/leds/blinkm.cpp b/Marlin/src/feature/leds/blinkm.cpp
new file mode 100644
index 0000000..868eb4b
--- /dev/null
+++ b/Marlin/src/feature/leds/blinkm.cpp
@@ -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/>.
+ *
+ */
+
+/**
+ * blinkm.cpp - Control a BlinkM over i2c
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(BLINKM)
+
+#include "blinkm.h"
+#include "leds.h"
+#include <Wire.h>
+
+void blinkm_set_led_color(const LEDColor &color) {
+  Wire.begin();
+  Wire.beginTransmission(I2C_ADDRESS(0x09));
+  Wire.write('o');                    //to disable ongoing script, only needs to be used once
+  Wire.write('n');
+  Wire.write(color.r);
+  Wire.write(color.g);
+  Wire.write(color.b);
+  Wire.endTransmission();
+}
+
+#endif // BLINKM
diff --git a/Marlin/src/feature/leds/blinkm.h b/Marlin/src/feature/leds/blinkm.h
new file mode 100644
index 0000000..29a9e78
--- /dev/null
+++ b/Marlin/src/feature/leds/blinkm.h
@@ -0,0 +1,31 @@
+/**
+ * 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
+
+/**
+ * blinkm.h - Control a BlinkM over i2c
+ */
+
+struct LEDColor;
+typedef LEDColor LEDColor;
+
+void blinkm_set_led_color(const LEDColor &color);
diff --git a/Marlin/src/feature/leds/leds.cpp b/Marlin/src/feature/leds/leds.cpp
new file mode 100644
index 0000000..ef9099f
--- /dev/null
+++ b/Marlin/src/feature/leds/leds.cpp
@@ -0,0 +1,200 @@
+/**
+ * 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/>.
+ *
+ */
+
+/**
+ * leds.cpp - Marlin RGB LED general support
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if HAS_COLOR_LEDS
+
+#include "leds.h"
+
+#if ENABLED(BLINKM)
+  #include "blinkm.h"
+#endif
+
+#if ENABLED(PCA9632)
+  #include "pca9632.h"
+#endif
+
+#if ENABLED(PCA9533)
+  #include "pca9533.h"
+#endif
+
+#if ENABLED(LED_COLOR_PRESETS)
+  const LEDColor LEDLights::defaultLEDColor = MakeLEDColor(
+    LED_USER_PRESET_RED, LED_USER_PRESET_GREEN, LED_USER_PRESET_BLUE,
+    LED_USER_PRESET_WHITE, LED_USER_PRESET_BRIGHTNESS
+  );
+#endif
+
+#if EITHER(LED_CONTROL_MENU, PRINTER_EVENT_LEDS)
+  LEDColor LEDLights::color;
+  bool LEDLights::lights_on;
+#endif
+
+LEDLights leds;
+
+void LEDLights::setup() {
+  #if EITHER(RGB_LED, RGBW_LED)
+    if (PWM_PIN(RGB_LED_R_PIN)) SET_PWM(RGB_LED_R_PIN); else SET_OUTPUT(RGB_LED_R_PIN);
+    if (PWM_PIN(RGB_LED_G_PIN)) SET_PWM(RGB_LED_G_PIN); else SET_OUTPUT(RGB_LED_G_PIN);
+    if (PWM_PIN(RGB_LED_B_PIN)) SET_PWM(RGB_LED_B_PIN); else SET_OUTPUT(RGB_LED_B_PIN);
+    #if ENABLED(RGBW_LED)
+      if (PWM_PIN(RGB_LED_W_PIN)) SET_PWM(RGB_LED_W_PIN); else SET_OUTPUT(RGB_LED_W_PIN);
+    #endif
+  #endif
+  TERN_(NEOPIXEL_LED, neo.init());
+  TERN_(PCA9533, PCA9533_init());
+  TERN_(LED_USER_PRESET_STARTUP, set_default());
+}
+
+void LEDLights::set_color(const LEDColor &incol
+  #if ENABLED(NEOPIXEL_LED)
+    , bool isSequence/*=false*/
+  #endif
+) {
+
+  #if ENABLED(NEOPIXEL_LED)
+
+    const uint32_t neocolor = LEDColorWhite() == incol
+                            ? neo.Color(NEO_WHITE)
+                            : neo.Color(incol.r, incol.g, incol.b, incol.w);
+    static uint16_t nextLed = 0;
+
+    #ifdef NEOPIXEL_BKGD_LED_INDEX
+      if (NEOPIXEL_BKGD_LED_INDEX == nextLed) {
+        if (++nextLed >= neo.pixels()) nextLed = 0;
+        return;
+      }
+    #endif
+
+    neo.set_brightness(incol.i);
+
+    if (isSequence) {
+      neo.set_pixel_color(nextLed, neocolor);
+      neo.show();
+      if (++nextLed >= neo.pixels()) nextLed = 0;
+      return;
+    }
+
+    neo.set_color(neocolor);
+
+  #endif
+
+  #if ENABLED(BLINKM)
+
+    // This variant uses i2c to send the RGB components to the device.
+    blinkm_set_led_color(incol);
+
+  #endif
+
+  #if EITHER(RGB_LED, RGBW_LED)
+
+    // This variant uses 3-4 separate pins for the RGB(W) components.
+    // If the pins can do PWM then their intensity will be set.
+    #define UPDATE_RGBW(C,c) do {                       \
+      if (PWM_PIN(RGB_LED_##C##_PIN))                   \
+        analogWrite(pin_t(RGB_LED_##C##_PIN), incol.c); \
+      else                                              \
+        WRITE(RGB_LED_##C##_PIN, incol.c ? HIGH : LOW); \
+    }while(0)
+    UPDATE_RGBW(R,r); UPDATE_RGBW(G,g); UPDATE_RGBW(B,b);
+    #if ENABLED(RGBW_LED)
+      UPDATE_RGBW(W,w);
+    #endif
+
+  #endif
+
+  // Update I2C LED driver
+  TERN_(PCA9632, PCA9632_set_led_color(incol));
+  TERN_(PCA9533, PCA9533_set_rgb(incol.r, incol.g, incol.b));
+
+  #if EITHER(LED_CONTROL_MENU, PRINTER_EVENT_LEDS)
+    // Don't update the color when OFF
+    lights_on = !incol.is_off();
+    if (lights_on) color = incol;
+  #endif
+}
+
+#if ENABLED(LED_CONTROL_MENU)
+  void LEDLights::toggle() { if (lights_on) set_off(); else update(); }
+#endif
+
+#ifdef LED_BACKLIGHT_TIMEOUT
+
+  millis_t LEDLights::led_off_time; // = 0
+
+  void LEDLights::update_timeout(const bool power_on) {
+    const millis_t ms = millis();
+    if (power_on)
+      reset_timeout(ms);
+    else if (ELAPSED(ms, led_off_time))
+      set_off();
+  }
+
+#endif
+
+#if ENABLED(NEOPIXEL2_SEPARATE)
+
+  #if ENABLED(NEO2_COLOR_PRESETS)
+    const LEDColor LEDLights2::defaultLEDColor = MakeLEDColor(
+      NEO2_USER_PRESET_RED, NEO2_USER_PRESET_GREEN, NEO2_USER_PRESET_BLUE,
+      NEO2_USER_PRESET_WHITE, NEO2_USER_PRESET_BRIGHTNESS
+    );
+  #endif
+
+  #if ENABLED(LED_CONTROL_MENU)
+    LEDColor LEDLights2::color;
+    bool LEDLights2::lights_on;
+  #endif
+
+  LEDLights2 leds2;
+
+  void LEDLights2::setup() {
+    neo2.init();
+    TERN_(NEO2_USER_PRESET_STARTUP, set_default());
+  }
+
+  void LEDLights2::set_color(const LEDColor &incol) {
+    const uint32_t neocolor = LEDColorWhite() == incol
+                            ? neo2.Color(NEO2_WHITE)
+                            : neo2.Color(incol.r, incol.g, incol.b, incol.w);
+    neo2.set_brightness(incol.i);
+    neo2.set_color(neocolor);
+
+    #if ENABLED(LED_CONTROL_MENU)
+      // Don't update the color when OFF
+      lights_on = !incol.is_off();
+      if (lights_on) color = incol;
+    #endif
+  }
+
+  #if ENABLED(LED_CONTROL_MENU)
+    void LEDLights2::toggle() { if (lights_on) set_off(); else update(); }
+  #endif
+
+#endif  // NEOPIXEL2_SEPARATE
+
+#endif  // HAS_COLOR_LEDS
diff --git a/Marlin/src/feature/leds/leds.h b/Marlin/src/feature/leds/leds.h
new file mode 100644
index 0000000..57b21d5
--- /dev/null
+++ b/Marlin/src/feature/leds/leds.h
@@ -0,0 +1,253 @@
+/**
+ * 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
+
+/**
+ * leds.h - Marlin general RGB LED support
+ */
+
+#include "../../inc/MarlinConfigPre.h"
+
+#include <string.h>
+
+#if ENABLED(NEOPIXEL_LED)
+  #include "neopixel.h"
+#endif
+
+// A white component can be passed
+#if ANY(RGBW_LED, NEOPIXEL_LED, PCA9632_RGBW)
+  #define HAS_WHITE_LED 1
+#endif
+
+/**
+ * LEDcolor type for use with leds.set_color
+ */
+typedef struct LEDColor {
+  uint8_t r, g, b
+    #if HAS_WHITE_LED
+      , w
+      #if ENABLED(NEOPIXEL_LED)
+        , i
+      #endif
+    #endif
+  ;
+
+  LEDColor() : r(255), g(255), b(255)
+    #if HAS_WHITE_LED
+      , w(255)
+      #if ENABLED(NEOPIXEL_LED)
+        , i(NEOPIXEL_BRIGHTNESS)
+      #endif
+    #endif
+  {}
+
+  LEDColor(uint8_t r, uint8_t g, uint8_t b
+    #if HAS_WHITE_LED
+      , uint8_t w=0
+      #if ENABLED(NEOPIXEL_LED)
+        , uint8_t i=NEOPIXEL_BRIGHTNESS
+      #endif
+    #endif
+    ) : r(r), g(g), b(b)
+    #if HAS_WHITE_LED
+      , w(w)
+      #if ENABLED(NEOPIXEL_LED)
+        , i(i)
+      #endif
+    #endif
+  {}
+
+  LEDColor(const uint8_t (&rgbw)[4]) : r(rgbw[0]), g(rgbw[1]), b(rgbw[2])
+    #if HAS_WHITE_LED
+      , w(rgbw[3])
+      #if ENABLED(NEOPIXEL_LED)
+        , i(NEOPIXEL_BRIGHTNESS)
+      #endif
+    #endif
+  {}
+
+  LEDColor& operator=(const uint8_t (&rgbw)[4]) {
+    r = rgbw[0]; g = rgbw[1]; b = rgbw[2];
+    TERN_(HAS_WHITE_LED, w = rgbw[3]);
+    return *this;
+  }
+
+  LEDColor& operator=(const LEDColor &right) {
+    if (this != &right) memcpy(this, &right, sizeof(LEDColor));
+    return *this;
+  }
+
+  bool operator==(const LEDColor &right) {
+    if (this == &right) return true;
+    return 0 == memcmp(this, &right, sizeof(LEDColor));
+  }
+
+  bool operator!=(const LEDColor &right) { return !operator==(right); }
+
+  bool is_off() const {
+    return 3 > r + g + b + TERN0(HAS_WHITE_LED, w);
+  }
+} LEDColor;
+
+/**
+ * Color helpers and presets
+ */
+#if HAS_WHITE_LED
+  #if ENABLED(NEOPIXEL_LED)
+    #define MakeLEDColor(R,G,B,W,I) LEDColor(R, G, B, W, I)
+  #else
+    #define MakeLEDColor(R,G,B,W,I) LEDColor(R, G, B, W)
+  #endif
+#else
+  #define MakeLEDColor(R,G,B,W,I)   LEDColor(R, G, B)
+#endif
+
+#define LEDColorOff()             LEDColor(  0,   0,   0)
+#define LEDColorRed()             LEDColor(255,   0,   0)
+#if ENABLED(LED_COLORS_REDUCE_GREEN)
+  #define LEDColorOrange()        LEDColor(255,  25,   0)
+  #define LEDColorYellow()        LEDColor(255,  75,   0)
+#else
+  #define LEDColorOrange()        LEDColor(255,  80,   0)
+  #define LEDColorYellow()        LEDColor(255, 255,   0)
+#endif
+#define LEDColorGreen()           LEDColor(  0, 255,   0)
+#define LEDColorBlue()            LEDColor(  0,   0, 255)
+#define LEDColorIndigo()          LEDColor(  0, 255, 255)
+#define LEDColorViolet()          LEDColor(255,   0, 255)
+#if HAS_WHITE_LED && DISABLED(RGB_LED)
+  #define LEDColorWhite()         LEDColor(  0,   0,   0, 255)
+#else
+  #define LEDColorWhite()         LEDColor(255, 255, 255)
+#endif
+
+class LEDLights {
+public:
+  LEDLights() {} // ctor
+
+  static void setup(); // init()
+
+  static void set_color(const LEDColor &color
+    #if ENABLED(NEOPIXEL_LED)
+      , bool isSequence=false
+    #endif
+  );
+
+  static inline void set_color(uint8_t r, uint8_t g, uint8_t b
+    #if HAS_WHITE_LED
+      , uint8_t w=0
+    #endif
+    #if ENABLED(NEOPIXEL_LED)
+      , uint8_t i=NEOPIXEL_BRIGHTNESS
+      , bool isSequence=false
+    #endif
+  ) {
+    set_color(MakeLEDColor(r, g, b, w, i)
+      #if ENABLED(NEOPIXEL_LED)
+        , isSequence
+      #endif
+    );
+  }
+
+  static inline void set_off()   { set_color(LEDColorOff()); }
+  static inline void set_green() { set_color(LEDColorGreen()); }
+  static inline void set_white() { set_color(LEDColorWhite()); }
+
+  #if ENABLED(LED_COLOR_PRESETS)
+    static const LEDColor defaultLEDColor;
+    static inline void set_default()  { set_color(defaultLEDColor); }
+    static inline void set_red()      { set_color(LEDColorRed()); }
+    static inline void set_orange()   { set_color(LEDColorOrange()); }
+    static inline void set_yellow()   { set_color(LEDColorYellow()); }
+    static inline void set_blue()     { set_color(LEDColorBlue()); }
+    static inline void set_indigo()   { set_color(LEDColorIndigo()); }
+    static inline void set_violet()   { set_color(LEDColorViolet()); }
+  #endif
+
+  #if ENABLED(PRINTER_EVENT_LEDS)
+    static inline LEDColor get_color() { return lights_on ? color : LEDColorOff(); }
+  #endif
+
+  #if EITHER(LED_CONTROL_MENU, PRINTER_EVENT_LEDS)
+    static LEDColor color; // last non-off color
+    static bool lights_on; // the last set color was "on"
+  #endif
+
+  #if ENABLED(LED_CONTROL_MENU)
+    static void toggle();  // swap "off" with color
+    static inline void update() { set_color(color); }
+  #endif
+
+  #ifdef LED_BACKLIGHT_TIMEOUT
+    private:
+      static millis_t led_off_time;
+    public:
+      static inline void reset_timeout(const millis_t &ms) {
+        led_off_time = ms + LED_BACKLIGHT_TIMEOUT;
+        if (!lights_on) set_default();
+      }
+      static void update_timeout(const bool power_on);
+  #endif
+};
+
+extern LEDLights leds;
+
+#if ENABLED(NEOPIXEL2_SEPARATE)
+
+  class LEDLights2 {
+  public:
+    LEDLights2() {}
+
+    static void setup(); // init()
+
+    static void set_color(const LEDColor &color);
+
+    inline void set_color(uint8_t r, uint8_t g, uint8_t b, uint8_t w=0, uint8_t i=NEOPIXEL2_BRIGHTNESS) {
+      set_color(MakeLEDColor(r, g, b, w, i));
+    }
+
+    static inline void set_off()   { set_color(LEDColorOff()); }
+    static inline void set_green() { set_color(LEDColorGreen()); }
+    static inline void set_white() { set_color(LEDColorWhite()); }
+
+    #if ENABLED(NEO2_COLOR_PRESETS)
+      static const LEDColor defaultLEDColor;
+      static inline void set_default()  { set_color(defaultLEDColor); }
+      static inline void set_red()      { set_color(LEDColorRed()); }
+      static inline void set_orange()   { set_color(LEDColorOrange()); }
+      static inline void set_yellow()   { set_color(LEDColorYellow()); }
+      static inline void set_blue()     { set_color(LEDColorBlue()); }
+      static inline void set_indigo()   { set_color(LEDColorIndigo()); }
+      static inline void set_violet()   { set_color(LEDColorViolet()); }
+    #endif
+
+    #if ENABLED(NEOPIXEL2_SEPARATE)
+      static LEDColor color; // last non-off color
+      static bool lights_on; // the last set color was "on"
+      static void toggle();  // swap "off" with color
+      static inline void update() { set_color(color); }
+    #endif
+  };
+
+  extern LEDLights2 leds2;
+
+#endif // NEOPIXEL2_SEPARATE
diff --git a/Marlin/src/feature/leds/neopixel.cpp b/Marlin/src/feature/leds/neopixel.cpp
new file mode 100644
index 0000000..27bbeb3
--- /dev/null
+++ b/Marlin/src/feature/leds/neopixel.cpp
@@ -0,0 +1,172 @@
+/**
+ * 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/>.
+ *
+ */
+
+/**
+ * Marlin RGB LED general support
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(NEOPIXEL_LED)
+
+#include "neopixel.h"
+
+#if EITHER(NEOPIXEL_STARTUP_TEST, NEOPIXEL2_STARTUP_TEST)
+  #include "../../core/utility.h"
+#endif
+
+Marlin_NeoPixel neo;
+int8_t Marlin_NeoPixel::neoindex;
+
+Adafruit_NeoPixel Marlin_NeoPixel::adaneo1(NEOPIXEL_PIXELS, NEOPIXEL_PIN, NEOPIXEL_TYPE + NEO_KHZ800)
+  #if CONJOINED_NEOPIXEL
+    , Marlin_NeoPixel::adaneo2(NEOPIXEL_PIXELS, NEOPIXEL2_PIN, NEOPIXEL2_TYPE + NEO_KHZ800)
+  #endif
+;
+
+#ifdef NEOPIXEL_BKGD_LED_INDEX
+
+  void Marlin_NeoPixel::set_color_background() {
+    uint8_t background_color[4] = NEOPIXEL_BKGD_COLOR;
+    set_pixel_color(NEOPIXEL_BKGD_LED_INDEX, adaneo1.Color(background_color[0], background_color[1], background_color[2], background_color[3]));
+  }
+
+#endif
+
+void Marlin_NeoPixel::set_color(const uint32_t color) {
+  if (neoindex >= 0) {
+    set_pixel_color(neoindex, color);
+    neoindex = -1;
+  }
+  else {
+    for (uint16_t i = 0; i < pixels(); ++i) {
+      #ifdef NEOPIXEL_BKGD_LED_INDEX
+        if (i == NEOPIXEL_BKGD_LED_INDEX && color != 0x000000) {
+          set_color_background();
+          continue;
+        }
+      #endif
+      set_pixel_color(i, color);
+    }
+  }
+  show();
+}
+
+void Marlin_NeoPixel::set_color_startup(const uint32_t color) {
+  for (uint16_t i = 0; i < pixels(); ++i)
+    set_pixel_color(i, color);
+  show();
+}
+
+void Marlin_NeoPixel::init() {
+  neoindex = -1;                       // -1 .. NEOPIXEL_PIXELS-1 range
+  set_brightness(NEOPIXEL_BRIGHTNESS); //  0 .. 255 range
+  begin();
+  show();  // initialize to all off
+
+  #if ENABLED(NEOPIXEL_STARTUP_TEST)
+    set_color_startup(adaneo1.Color(255, 0, 0, 0));  // red
+    safe_delay(500);
+    set_color_startup(adaneo1.Color(0, 255, 0, 0));  // green
+    safe_delay(500);
+    set_color_startup(adaneo1.Color(0, 0, 255, 0));  // blue
+    safe_delay(500);
+  #endif
+
+  #ifdef NEOPIXEL_BKGD_LED_INDEX
+    set_color_background();
+  #endif
+
+  #if ENABLED(LED_USER_PRESET_STARTUP)
+    set_color(adaneo1.Color(LED_USER_PRESET_RED, LED_USER_PRESET_GREEN, LED_USER_PRESET_BLUE, LED_USER_PRESET_WHITE));
+  #else
+    set_color(adaneo1.Color(0, 0, 0, 0));
+  #endif
+}
+
+#if 0
+bool Marlin_NeoPixel::set_led_color(const uint8_t r, const uint8_t g, const uint8_t b, const uint8_t w, const uint8_t p) {
+  const uint32_t color = adaneo1.Color(r, g, b, w);
+  set_brightness(p);
+  #if DISABLED(NEOPIXEL_IS_SEQUENTIAL)
+    set_color(color);
+    return false;
+  #else
+    static uint16_t nextLed = 0;
+    set_pixel_color(nextLed, color);
+    show();
+    if (++nextLed >= pixels()) nextLed = 0;
+    return true;
+  #endif
+}
+#endif
+
+#if ENABLED(NEOPIXEL2_SEPARATE)
+
+  Marlin_NeoPixel2 neo2;
+
+  int8_t Marlin_NeoPixel2::neoindex;
+  Adafruit_NeoPixel Marlin_NeoPixel2::adaneo(NEOPIXEL2_PIXELS, NEOPIXEL2_PIN, NEOPIXEL2_TYPE);
+
+  void Marlin_NeoPixel2::set_color(const uint32_t color) {
+    if (neoindex >= 0) {
+      set_pixel_color(neoindex, color);
+      neoindex = -1;
+    }
+    else {
+      for (uint16_t i = 0; i < pixels(); ++i)
+        set_pixel_color(i, color);
+    }
+    show();
+  }
+
+  void Marlin_NeoPixel2::set_color_startup(const uint32_t color) {
+    for (uint16_t i = 0; i < pixels(); ++i)
+      set_pixel_color(i, color);
+    show();
+  }
+
+  void Marlin_NeoPixel2::init() {
+    neoindex = -1;                        // -1 .. NEOPIXEL2_PIXELS-1 range
+    set_brightness(NEOPIXEL2_BRIGHTNESS); //  0 .. 255 range
+    begin();
+    show();  // initialize to all off
+
+    #if ENABLED(NEOPIXEL2_STARTUP_TEST)
+      set_color_startup(adaneo.Color(255, 0, 0, 0));  // red
+      safe_delay(500);
+      set_color_startup(adaneo.Color(0, 255, 0, 0));  // green
+      safe_delay(500);
+      set_color_startup(adaneo.Color(0, 0, 255, 0));  // blue
+      safe_delay(500);
+    #endif
+
+    #if ENABLED(NEO2_USER_PRESET_STARTUP)
+      set_color(adaneo.Color(NEO2_USER_PRESET_RED, NEO2_USER_PRESET_GREEN, NEO2_USER_PRESET_BLUE, NEO2_USER_PRESET_WHITE));
+    #else
+      set_color(adaneo.Color(0, 0, 0, 0));
+    #endif
+  }
+
+#endif // NEOPIXEL2_SEPARATE
+
+#endif // NEOPIXEL_LED
diff --git a/Marlin/src/feature/leds/neopixel.h b/Marlin/src/feature/leds/neopixel.h
new file mode 100644
index 0000000..acf2e7f
--- /dev/null
+++ b/Marlin/src/feature/leds/neopixel.h
@@ -0,0 +1,182 @@
+/**
+ * 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
+
+/**
+ * NeoPixel support
+ */
+
+// ------------------------
+// Includes
+// ------------------------
+
+#include "../../inc/MarlinConfig.h"
+
+#include <Adafruit_NeoPixel.h>
+#include <stdint.h>
+
+// ------------------------
+// Defines
+// ------------------------
+
+#if defined(NEOPIXEL2_TYPE) && NEOPIXEL2_TYPE != NEOPIXEL_TYPE && DISABLED(NEOPIXEL2_SEPARATE)
+  #define MULTIPLE_NEOPIXEL_TYPES 1
+#endif
+
+#if EITHER(MULTIPLE_NEOPIXEL_TYPES, NEOPIXEL2_INSERIES)
+  #define CONJOINED_NEOPIXEL 1
+#endif
+
+#if NEOPIXEL_TYPE == NEO_RGB || NEOPIXEL_TYPE == NEO_RBG || NEOPIXEL_TYPE == NEO_GRB || NEOPIXEL_TYPE == NEO_GBR || NEOPIXEL_TYPE == NEO_BRG || NEOPIXEL_TYPE == NEO_BGR
+  #define NEOPIXEL_IS_RGB 1
+#else
+  #define NEOPIXEL_IS_RGBW 1
+#endif
+
+#if NEOPIXEL_IS_RGB
+  #define NEO_WHITE 255, 255, 255, 0
+#else
+  #define NEO_WHITE 0, 0, 0, 255
+#endif
+
+// ------------------------
+// Function prototypes
+// ------------------------
+
+class Marlin_NeoPixel {
+private:
+  static Adafruit_NeoPixel adaneo1
+    #if CONJOINED_NEOPIXEL
+      , adaneo2
+    #endif
+  ;
+
+public:
+  static int8_t neoindex;
+
+  static void init();
+  static void set_color_startup(const uint32_t c);
+
+  static void set_color(const uint32_t c);
+
+  #ifdef NEOPIXEL_BKGD_LED_INDEX
+    static void set_color_background();
+  #endif
+
+  static inline void begin() {
+    adaneo1.begin();
+    TERN_(CONJOINED_NEOPIXEL, adaneo2.begin());
+  }
+
+  static inline void set_pixel_color(const uint16_t n, const uint32_t c) {
+    #if ENABLED(NEOPIXEL2_INSERIES)
+      if (n >= NEOPIXEL_PIXELS) adaneo2.setPixelColor(n - (NEOPIXEL_PIXELS), c);
+      else adaneo1.setPixelColor(n, c);
+    #else
+      adaneo1.setPixelColor(n, c);
+      #if MULTIPLE_NEOPIXEL_TYPES
+        adaneo2.setPixelColor(n, c);
+      #endif
+    #endif
+  }
+
+  static inline void set_brightness(const uint8_t b) {
+    adaneo1.setBrightness(b);
+    TERN_(CONJOINED_NEOPIXEL, adaneo2.setBrightness(b));
+  }
+
+  static inline void show() {
+    // Some platforms cannot maintain PWM output when NeoPixel disables interrupts for long durations.
+    TERN_(HAS_PAUSE_SERVO_OUTPUT, PAUSE_SERVO_OUTPUT());
+    adaneo1.show();
+    #if PIN_EXISTS(NEOPIXEL2)
+      #if CONJOINED_NEOPIXEL
+        adaneo2.show();
+      #else
+        IF_DISABLED(NEOPIXEL2_SEPARATE, adaneo1.setPin(NEOPIXEL2_PIN));
+        adaneo1.show();
+        adaneo1.setPin(NEOPIXEL_PIN);
+      #endif
+    #endif
+    TERN_(HAS_PAUSE_SERVO_OUTPUT, RESUME_SERVO_OUTPUT());
+  }
+
+  #if 0
+    bool set_led_color(const uint8_t r, const uint8_t g, const uint8_t b, const uint8_t w, const uint8_t p);
+  #endif
+
+  // Accessors
+  static inline uint16_t pixels() { TERN(NEOPIXEL2_INSERIES, return adaneo1.numPixels() * 2, return adaneo1.numPixels()); }
+  static inline uint8_t brightness() { return adaneo1.getBrightness(); }
+  static inline uint32_t Color(uint8_t r, uint8_t g, uint8_t b, uint8_t w) {
+    return adaneo1.Color(r, g, b, w);
+  }
+};
+
+extern Marlin_NeoPixel neo;
+
+// Neo pixel channel 2
+#if ENABLED(NEOPIXEL2_SEPARATE)
+
+  #if NEOPIXEL2_TYPE == NEO_RGB || NEOPIXEL2_TYPE == NEO_RBG || NEOPIXEL2_TYPE == NEO_GRB || NEOPIXEL2_TYPE == NEO_GBR || NEOPIXEL2_TYPE == NEO_BRG || NEOPIXEL2_TYPE == NEO_BGR
+    #define NEOPIXEL2_IS_RGB 1
+  #else
+    #define NEOPIXEL2_IS_RGBW 1
+  #endif
+
+  #if NEOPIXEL2_IS_RGB
+    #define NEO2_WHITE 255, 255, 255, 0
+  #else
+    #define NEO2_WHITE 0, 0, 0, 255
+  #endif
+
+  class Marlin_NeoPixel2 {
+  private:
+    static Adafruit_NeoPixel adaneo;
+
+  public:
+    static int8_t neoindex;
+
+    static void init();
+    static void set_color_startup(const uint32_t c);
+
+    static void set_color(const uint32_t c);
+
+    static inline void begin() { adaneo.begin(); }
+    static inline void set_pixel_color(const uint16_t n, const uint32_t c) { adaneo.setPixelColor(n, c); }
+    static inline void set_brightness(const uint8_t b) { adaneo.setBrightness(b); }
+    static inline void show() {
+      adaneo.show();
+      adaneo.setPin(NEOPIXEL2_PIN);
+    }
+
+    // Accessors
+    static inline uint16_t pixels() { return adaneo.numPixels();}
+    static inline uint8_t brightness() { return adaneo.getBrightness(); }
+    static inline uint32_t Color(uint8_t r, uint8_t g, uint8_t b, uint8_t w) {
+      return adaneo.Color(r, g, b, w);
+    }
+  };
+
+  extern Marlin_NeoPixel2 neo2;
+
+#endif // NEOPIXEL2_SEPARATE
diff --git a/Marlin/src/feature/leds/pca9533.cpp b/Marlin/src/feature/leds/pca9533.cpp
new file mode 100644
index 0000000..0fd4d66
--- /dev/null
+++ b/Marlin/src/feature/leds/pca9533.cpp
@@ -0,0 +1,127 @@
+/*
+ * 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/>.
+ *
+ */
+
+/**
+ * PCA9533 LED controller driver (MightyBoard, FlashForge Creator Pro, etc.)
+ *  by @grauerfuchs - 1 Apr 2020
+ */
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(PCA9533)
+
+#include "pca9533.h"
+#include <Wire.h>
+
+void PCA9533_init() {
+  Wire.begin();
+  PCA9533_reset();
+}
+
+static void PCA9533_writeAllRegisters(uint8_t psc0, uint8_t pwm0, uint8_t psc1, uint8_t pwm1, uint8_t ls0){
+  uint8_t data[6] = { PCA9533_REG_PSC0 | PCA9533_REGM_AI, psc0, pwm0, psc1, pwm1, ls0 };
+  Wire.beginTransmission(PCA9533_Addr >> 1);
+  Wire.write(data, 6);
+  Wire.endTransmission();
+  delayMicroseconds(1);
+}
+
+static void PCA9533_writeRegister(uint8_t reg, uint8_t val){
+  uint8_t data[2] = { reg, val };
+  Wire.beginTransmission(PCA9533_Addr >> 1);
+  Wire.write(data, 2);
+  Wire.endTransmission();
+  delayMicroseconds(1);
+}
+
+// Reset (clear) all registers
+void PCA9533_reset() {
+  PCA9533_writeAllRegisters(0, 0, 0, 0, 0);
+}
+
+// Turn all LEDs off
+void PCA9533_setOff() {
+  PCA9533_writeRegister(PCA9533_REG_SEL, 0);
+}
+
+void PCA9533_set_rgb(uint8_t red, uint8_t green, uint8_t blue) {
+  uint8_t r_pwm0 = 0; // Register data - PWM value
+  uint8_t r_pwm1 = 0; // Register data - PWM value
+
+  uint8_t op_g = 0, op_r = 0, op_b = 0; // Opcodes - Green, Red, Blue
+
+  // Light theory! GREEN takes priority because
+  // it's the most visible to the human eye.
+       if (green ==   0)  op_g = PCA9533_LED_OP_OFF;
+  else if (green == 255)  op_g = PCA9533_LED_OP_ON;
+  else { r_pwm0 = green;  op_g = PCA9533_LED_OP_PWM0; }
+
+  // RED
+       if (red ==   0)    op_r = PCA9533_LED_OP_OFF;
+  else if (red == 255)    op_r = PCA9533_LED_OP_ON;
+  else if (r_pwm0 == 0 || r_pwm0 == red) {
+         r_pwm0 = red;    op_r = PCA9533_LED_OP_PWM0;
+  }
+  else {
+         r_pwm1 = red;    op_r = PCA9533_LED_OP_PWM1;
+  }
+
+  // BLUE
+       if (blue ==   0)   op_b = PCA9533_LED_OP_OFF;
+  else if (blue == 255)   op_b = PCA9533_LED_OP_ON;
+  else if (r_pwm0 == 0 || r_pwm0 == blue) {
+         r_pwm0 = blue;   op_b = PCA9533_LED_OP_PWM0;
+  }
+  else if (r_pwm1 == 0 || r_pwm1 == blue) {
+         r_pwm1 = blue;   op_b = PCA9533_LED_OP_PWM1;
+  }
+  else {
+    /**
+     * Conflict. 3 values are requested but only 2 channels exist.
+     * G is on channel 0 and R is on channel 1, so work from there.
+     * Find the closest match, average the values, then use the free channel.
+     */
+    uint8_t dgb = ABS(green - blue),
+            dgr = ABS(green - red),
+            dbr = ABS(blue - red);
+    if (dgb < dgr && dgb < dbr) {         // Mix with G on channel 0.
+      op_b = PCA9533_LED_OP_PWM0;
+      r_pwm0 = uint8_t(((uint16_t)green + (uint16_t)blue) / 2);
+    }
+    else if (dbr <= dgr && dbr <= dgb) {  // Mix with R on channel 1.
+      op_b = PCA9533_LED_OP_PWM1;
+      r_pwm1 = uint8_t(((uint16_t)red + (uint16_t)blue) / 2);
+    }
+    else {                                // Mix R+G on 0 and put B on 1.
+      op_r = PCA9533_LED_OP_PWM0;
+      r_pwm0 = uint8_t(((uint16_t)green + (uint16_t)red) / 2);
+      op_b = PCA9533_LED_OP_PWM1;
+      r_pwm1 = blue;
+    }
+  }
+
+  // Write the changes to the hardware
+  PCA9533_writeAllRegisters(0, r_pwm0, 0, r_pwm1,
+    (op_g << PCA9533_LED_OFS_GRN) | (op_r << PCA9533_LED_OFS_RED) | (op_b << PCA9533_LED_OFS_BLU)
+  );
+}
+
+#endif // PCA9533
diff --git a/Marlin/src/feature/leds/pca9533.h b/Marlin/src/feature/leds/pca9533.h
new file mode 100644
index 0000000..431058c
--- /dev/null
+++ b/Marlin/src/feature/leds/pca9533.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
+
+/*
+ * Driver for the PCA9533 LED controller found on the MightyBoard
+ * used by FlashForge Creator Pro, MakerBot, etc.
+ * Written 2020 APR 01 by grauerfuchs
+ */
+#include <Arduino.h>
+
+#define ENABLE_I2C_PULLUPS
+
+// Chip address (for Wire)
+#define PCA9533_Addr        0xC4
+
+// Control registers
+#define PCA9533_REG_READ    0x00
+#define PCA9533_REG_PSC0    0x01
+#define PCA9533_REG_PWM0    0x02
+#define PCA9533_REG_PSC1    0x03
+#define PCA9533_REG_PWM1    0x04
+#define PCA9533_REG_SEL     0x05
+#define PCA9533_REGM_AI     0x10
+
+// LED selector operation
+#define PCA9533_LED_OP_OFF  0B00
+#define PCA9533_LED_OP_ON   0B01
+#define PCA9533_LED_OP_PWM0 0B10
+#define PCA9533_LED_OP_PWM1 0B11
+
+// Select register bit offsets for LED colors
+#define PCA9533_LED_OFS_RED 0
+#define PCA9533_LED_OFS_GRN 2
+#define PCA9533_LED_OFS_BLU 4
+
+void PCA9533_init();
+void PCA9533_reset();
+void PCA9533_set_rgb(uint8_t red, uint8_t green, uint8_t blue);
+void PCA9533_setOff();
diff --git a/Marlin/src/feature/leds/pca9632.cpp b/Marlin/src/feature/leds/pca9632.cpp
new file mode 100644
index 0000000..bb30e0b
--- /dev/null
+++ b/Marlin/src/feature/leds/pca9632.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/>.
+ *
+ */
+
+/**
+ * Driver for the Philips PCA9632 LED driver.
+ * Written by Robert Mendon Feb 2017.
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(PCA9632)
+
+#include "pca9632.h"
+#include "leds.h"
+#include <Wire.h>
+
+#define PCA9632_MODE1_VALUE   0b00000001 //(ALLCALL)
+#define PCA9632_MODE2_VALUE   0b00010101 //(DIMMING, INVERT, CHANGE ON STOP,TOTEM)
+#define PCA9632_LEDOUT_VALUE  0b00101010
+
+/* Register addresses */
+#define PCA9632_MODE1       0x00
+#define PCA9632_MODE2       0x01
+#define PCA9632_PWM0        0x02
+#define PCA9632_PWM1        0x03
+#define PCA9632_PWM2        0x04
+#define PCA9632_PWM3        0x05
+#define PCA9632_GRPPWM      0x06
+#define PCA9632_GRPFREQ     0x07
+#define PCA9632_LEDOUT      0x08
+#define PCA9632_SUBADR1     0x09
+#define PCA9632_SUBADR2     0x0A
+#define PCA9632_SUBADR3     0x0B
+#define PCA9632_ALLCALLADDR 0x0C
+
+#define PCA9632_NO_AUTOINC  0x00
+#define PCA9632_AUTO_ALL    0x80
+#define PCA9632_AUTO_IND    0xA0
+#define PCA9632_AUTOGLO     0xC0
+#define PCA9632_AUTOGI      0xE0
+
+// Red=LED0   Green=LED1   Blue=LED2  White=LED3
+#ifndef PCA9632_RED
+  #define PCA9632_RED 0x00
+#endif
+#ifndef PCA9632_GRN
+  #define PCA9632_GRN 0x02
+#endif
+#ifndef PCA9632_BLU
+  #define PCA9632_BLU 0x04
+#endif
+#if HAS_WHITE_LED && !defined(PCA9632_WHT)
+  #define PCA9632_WHT 0x06
+#endif
+
+// If any of the color indexes are greater than 0x04 they can't use auto increment
+#if !defined(PCA9632_NO_AUTO_INC) && (PCA9632_RED > 0x04 || PCA9632_GRN > 0x04 || PCA9632_BLU > 0x04 || PCA9632_WHT > 0x04)
+  #define PCA9632_NO_AUTO_INC
+#endif
+
+#define LED_OFF   0x00
+#define LED_ON    0x01
+#define LED_PWM   0x02
+
+#define PCA9632_ADDRESS 0b01100000
+
+byte PCA_init = 0;
+
+static void PCA9632_WriteRegister(const byte addr, const byte regadd, const byte value) {
+  Wire.beginTransmission(I2C_ADDRESS(addr));
+  Wire.write(regadd);
+  Wire.write(value);
+  Wire.endTransmission();
+}
+
+static void PCA9632_WriteAllRegisters(const byte addr, const byte regadd, const byte vr, const byte vg, const byte vb
+  #if ENABLED(PCA9632_RGBW)
+    , const byte vw
+  #endif
+) {
+  #if DISABLED(PCA9632_NO_AUTO_INC)
+    uint8_t data[4];
+    data[0] = PCA9632_AUTO_IND | regadd;
+    data[1 + (PCA9632_RED >> 1)] = vr;
+    data[1 + (PCA9632_GRN >> 1)] = vg;
+    data[1 + (PCA9632_BLU >> 1)] = vb;
+    Wire.beginTransmission(I2C_ADDRESS(addr));
+    Wire.write(data, sizeof(data));
+    Wire.endTransmission();
+  #else
+    PCA9632_WriteRegister(addr, regadd + (PCA9632_RED >> 1), vr);
+    PCA9632_WriteRegister(addr, regadd + (PCA9632_GRN >> 1), vg);
+    PCA9632_WriteRegister(addr, regadd + (PCA9632_BLU >> 1), vb);
+    #if ENABLED(PCA9632_RGBW)
+      PCA9632_WriteRegister(addr, regadd + (PCA9632_WHT >> 1), vw);
+    #endif
+  #endif
+}
+
+#if 0
+  static byte PCA9632_ReadRegister(const byte addr, const byte regadd) {
+    Wire.beginTransmission(I2C_ADDRESS(addr));
+    Wire.write(regadd);
+    const byte value = Wire.read();
+    Wire.endTransmission();
+    return value;
+  }
+#endif
+
+void PCA9632_set_led_color(const LEDColor &color) {
+  Wire.begin();
+  if (!PCA_init) {
+    PCA_init = 1;
+    PCA9632_WriteRegister(PCA9632_ADDRESS,PCA9632_MODE1, PCA9632_MODE1_VALUE);
+    PCA9632_WriteRegister(PCA9632_ADDRESS,PCA9632_MODE2, PCA9632_MODE2_VALUE);
+  }
+
+  const byte LEDOUT = (color.r ? LED_PWM << PCA9632_RED : 0)
+                    | (color.g ? LED_PWM << PCA9632_GRN : 0)
+                    | (color.b ? LED_PWM << PCA9632_BLU : 0)
+                    #if ENABLED(PCA9632_RGBW)
+                      | (color.w ? LED_PWM << PCA9632_WHT : 0)
+                    #endif
+                    ;
+
+  PCA9632_WriteAllRegisters(PCA9632_ADDRESS,PCA9632_PWM0, color.r, color.g, color.b
+    #if ENABLED(PCA9632_RGBW)
+      , color.w
+    #endif
+  );
+  PCA9632_WriteRegister(PCA9632_ADDRESS,PCA9632_LEDOUT, LEDOUT);
+}
+
+#if ENABLED(PCA9632_BUZZER)
+
+  void PCA9632_buzz(const long, const uint16_t) {
+    uint8_t data[] = PCA9632_BUZZER_DATA;
+    Wire.beginTransmission(I2C_ADDRESS(PCA9632_ADDRESS));
+    Wire.write(data, sizeof(data));
+    Wire.endTransmission();
+  }
+
+#endif // PCA9632_BUZZER
+
+#endif // PCA9632
diff --git a/Marlin/src/feature/leds/pca9632.h b/Marlin/src/feature/leds/pca9632.h
new file mode 100644
index 0000000..fb59a8c
--- /dev/null
+++ b/Marlin/src/feature/leds/pca9632.h
@@ -0,0 +1,37 @@
+/**
+ * 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
+
+/**
+ * Driver for the Philips PCA9632 LED driver.
+ * Written by Robert Mendon Feb 2017.
+ */
+
+struct LEDColor;
+typedef LEDColor LEDColor;
+
+void PCA9632_set_led_color(const LEDColor &color);
+
+#if ENABLED(PCA9632_BUZZER)
+  #include <stdint.h>
+  void PCA9632_buzz(const long, const uint16_t);
+#endif
diff --git a/Marlin/src/feature/leds/printer_event_leds.cpp b/Marlin/src/feature/leds/printer_event_leds.cpp
new file mode 100644
index 0000000..3a6b91a
--- /dev/null
+++ b/Marlin/src/feature/leds/printer_event_leds.cpp
@@ -0,0 +1,82 @@
+/**
+ * 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/>.
+ *
+ */
+
+/**
+ * feature/leds/printer_event_leds.cpp - LED color changing based on printer status
+ */
+
+#include "../../inc/MarlinConfigPre.h"
+
+#if ENABLED(PRINTER_EVENT_LEDS)
+
+#include "printer_event_leds.h"
+
+PrinterEventLEDs printerEventLEDs;
+
+#if HAS_LEDS_OFF_FLAG
+  bool PrinterEventLEDs::leds_off_after_print; // = false
+#endif
+
+#if HAS_TEMP_HOTEND || HAS_HEATED_BED
+
+  uint8_t PrinterEventLEDs::old_intensity = 0;
+
+  inline uint8_t pel_intensity(const float &start, const float &current, const float &target) {
+    if (uint16_t(start) == uint16_t(target)) return 255;
+    return (uint8_t)map(constrain(current, start, target), start, target, 0.f, 255.f);
+  }
+
+  inline void pel_set_rgb(const uint8_t r, const uint8_t g, const uint8_t b) {
+    leds.set_color(
+      MakeLEDColor(r, g, b, 0, neo.brightness())
+      #if ENABLED(NEOPIXEL_IS_SEQUENTIAL)
+        , true
+      #endif
+    );
+  }
+
+#endif
+
+#if HAS_TEMP_HOTEND
+
+  void PrinterEventLEDs::onHotendHeating(const float &start, const float &current, const float &target) {
+    const uint8_t blue = pel_intensity(start, current, target);
+    if (blue != old_intensity) {
+      old_intensity = blue;
+      pel_set_rgb(255, 0, 255 - blue);
+    }
+  }
+
+#endif
+
+#if HAS_HEATED_BED
+
+  void PrinterEventLEDs::onBedHeating(const float &start, const float &current, const float &target) {
+    const uint8_t red = pel_intensity(start, current, target);
+    if (red != old_intensity) {
+      old_intensity = red;
+      pel_set_rgb(red, 0, 255);
+    }
+  }
+#endif
+
+#endif // PRINTER_EVENT_LEDS
diff --git a/Marlin/src/feature/leds/printer_event_leds.h b/Marlin/src/feature/leds/printer_event_leds.h
new file mode 100644
index 0000000..86ec292
--- /dev/null
+++ b/Marlin/src/feature/leds/printer_event_leds.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/>.
+ *
+ */
+#pragma once
+
+/**
+ * feature/leds/printer_event_leds.h - LED color changing based on printer status
+ */
+
+#include "leds.h"
+#include "../../inc/MarlinConfig.h"
+
+class PrinterEventLEDs {
+private:
+  static uint8_t old_intensity;
+
+  #if HAS_LEDS_OFF_FLAG
+    static bool leds_off_after_print;
+  #endif
+
+  static inline void set_done() {
+    #if ENABLED(LED_COLOR_PRESETS)
+      leds.set_default();
+    #else
+      leds.set_off();
+    #endif
+  }
+
+public:
+  #if HAS_TEMP_HOTEND
+    static inline LEDColor onHotendHeatingStart() { old_intensity = 0; return leds.get_color(); }
+    static void onHotendHeating(const float &start, const float &current, const float &target);
+  #endif
+
+  #if HAS_HEATED_BED
+    static inline LEDColor onBedHeatingStart() { old_intensity = 127; return leds.get_color(); }
+    static void onBedHeating(const float &start, const float &current, const float &target);
+  #endif
+
+  #if HAS_TEMP_HOTEND || HAS_HEATED_BED
+    static inline void onHeatingDone() { leds.set_white(); }
+    static inline void onPidTuningDone(LEDColor c) { leds.set_color(c); }
+  #endif
+
+  #if ENABLED(SDSUPPORT)
+
+    static inline void onPrintCompleted() {
+      leds.set_green();
+      #if HAS_LEDS_OFF_FLAG
+        leds_off_after_print = true;
+      #else
+        safe_delay(2000);
+        set_done();
+      #endif
+    }
+
+    static inline void onResumeAfterWait() {
+      #if HAS_LEDS_OFF_FLAG
+        if (leds_off_after_print) {
+          set_done();
+          leds_off_after_print = false;
+        }
+      #endif
+    }
+
+  #endif // SDSUPPORT
+};
+
+extern PrinterEventLEDs printerEventLEDs;
diff --git a/Marlin/src/feature/leds/tempstat.cpp b/Marlin/src/feature/leds/tempstat.cpp
new file mode 100644
index 0000000..880258f
--- /dev/null
+++ b/Marlin/src/feature/leds/tempstat.cpp
@@ -0,0 +1,55 @@
+/**
+ * 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/>.
+ *
+ */
+
+/**
+ * Marlin RGB LED general support
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(TEMP_STAT_LEDS)
+
+#include "tempstat.h"
+#include "../../module/temperature.h"
+
+void handle_status_leds() {
+  static int8_t old_red = -1;  // Invalid value to force LED initialization
+  static millis_t next_status_led_update_ms = 0;
+  if (ELAPSED(millis(), next_status_led_update_ms)) {
+    next_status_led_update_ms += 500; // Update every 0.5s
+    float max_temp = TERN0(HAS_HEATED_BED, _MAX(thermalManager.degTargetBed(), thermalManager.degBed()));
+    HOTEND_LOOP()
+      max_temp = _MAX(max_temp, thermalManager.degHotend(e), thermalManager.degTargetHotend(e));
+    const int8_t new_red = (max_temp > 55.0) ? HIGH : (max_temp < 54.0 || old_red < 0) ? LOW : old_red;
+    if (new_red != old_red) {
+      old_red = new_red;
+      #if PIN_EXISTS(STAT_LED_RED)
+        WRITE(STAT_LED_RED_PIN, new_red);
+      #endif
+      #if PIN_EXISTS(STAT_LED_BLUE)
+        WRITE(STAT_LED_BLUE_PIN, !new_red);
+      #endif
+    }
+  }
+}
+
+#endif // TEMP_STAT_LEDS
diff --git a/Marlin/src/feature/leds/tempstat.h b/Marlin/src/feature/leds/tempstat.h
new file mode 100644
index 0000000..a8b919b
--- /dev/null
+++ b/Marlin/src/feature/leds/tempstat.h
@@ -0,0 +1,28 @@
+/**
+ * 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
+
+/**
+ * Marlin general RGB LED support
+ */
+
+void handle_status_leds();
diff --git a/Marlin/src/feature/max7219.cpp b/Marlin/src/feature/max7219.cpp
new file mode 100644
index 0000000..ebcb564
--- /dev/null
+++ b/Marlin/src/feature/max7219.cpp
@@ -0,0 +1,700 @@
+/**
+ * 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/>.
+ *
+ */
+
+/**
+ * This module is off by default, but can be enabled to facilitate the display of
+ * extra debug information during code development.
+ *
+ * Just connect up 5V and GND to give it power, then connect up the pins assigned
+ * in Configuration_adv.h. For example, on the Re-ARM you could use:
+ *
+ *   #define MAX7219_CLK_PIN   77
+ *   #define MAX7219_DIN_PIN   78
+ *   #define MAX7219_LOAD_PIN  79
+ *
+ * send() is called automatically at startup, and then there are a number of
+ * support functions available to control the LEDs in the 8x8 grid.
+ */
+
+#include "../inc/MarlinConfigPre.h"
+
+#if ENABLED(MAX7219_DEBUG)
+
+#define MAX7219_ERRORS // Disable to save 406 bytes of Program Memory
+
+#include "max7219.h"
+
+#include "../module/planner.h"
+#include "../module/stepper.h"
+#include "../MarlinCore.h"
+#include "../HAL/shared/Delay.h"
+
+#if ENABLED(MAX7219_SIDE_BY_SIDE) && MAX7219_NUMBER_UNITS > 1
+  #define HAS_SIDE_BY_SIDE 1
+#endif
+
+#if _ROT == 0 || _ROT == 180
+  #define MAX7219_X_LEDS TERN(HAS_SIDE_BY_SIDE, 8, MAX7219_LINES)
+  #define MAX7219_Y_LEDS TERN(HAS_SIDE_BY_SIDE, MAX7219_LINES, 8)
+#elif _ROT == 90 || _ROT == 270
+  #define MAX7219_X_LEDS TERN(HAS_SIDE_BY_SIDE, MAX7219_LINES, 8)
+  #define MAX7219_Y_LEDS TERN(HAS_SIDE_BY_SIDE, 8, MAX7219_LINES)
+#else
+  #error "MAX7219_ROTATE must be a multiple of +/- 90°."
+#endif
+
+Max7219 max7219;
+
+uint8_t Max7219::led_line[MAX7219_LINES]; // = { 0 };
+uint8_t Max7219::suspended; // = 0;
+
+#define LINE_REG(Q)     (max7219_reg_digit0 + ((Q) & 0x7))
+
+#if _ROT == 0 || _ROT == 270
+  #define _LED_BIT(Q)   (7 - ((Q) & 0x7))
+#else
+  #define _LED_BIT(Q)   ((Q) & 0x7)
+#endif
+#if _ROT == 0 || _ROT == 180
+  #define LED_BIT(X,Y)  _LED_BIT(X)
+#else
+  #define LED_BIT(X,Y)  _LED_BIT(Y)
+#endif
+#if _ROT == 0 || _ROT == 90
+  #define _LED_IND(P,Q) (_LED_TOP(P) + ((Q) & 0x7))
+#else
+  #define _LED_IND(P,Q) (_LED_TOP(P) + (7 - ((Q) & 0x7)))
+#endif
+
+#if HAS_SIDE_BY_SIDE
+  #if (_ROT == 0 || _ROT == 90) == DISABLED(MAX7219_REVERSE_ORDER)
+    #define _LED_TOP(Q)  ((MAX7219_NUMBER_UNITS - 1 - ((Q) >> 3)) << 3)
+  #else
+    #define _LED_TOP(Q)  ((Q) & ~0x7)
+  #endif
+  #if _ROT == 0 || _ROT == 180
+    #define LED_IND(X,Y)  _LED_IND(Y,Y)
+  #elif _ROT == 90 || _ROT == 270
+    #define LED_IND(X,Y)  _LED_IND(X,X)
+  #endif
+#else
+  #if (_ROT == 0 || _ROT == 270) == DISABLED(MAX7219_REVERSE_ORDER)
+    #define _LED_TOP(Q)  ((Q) & ~0x7)
+  #else
+    #define _LED_TOP(Q)  ((MAX7219_NUMBER_UNITS - 1 - ((Q) >> 3)) << 3)
+  #endif
+  #if _ROT == 0 || _ROT == 180
+    #define LED_IND(X,Y)  _LED_IND(X,Y)
+  #elif _ROT == 90 || _ROT == 270
+    #define LED_IND(X,Y)  _LED_IND(Y,X)
+  #endif
+#endif
+
+#define XOR_7219(X,Y) do{ led_line[LED_IND(X,Y)] ^=  _BV(LED_BIT(X,Y)); }while(0)
+#define SET_7219(X,Y) do{ led_line[LED_IND(X,Y)] |=  _BV(LED_BIT(X,Y)); }while(0)
+#define CLR_7219(X,Y) do{ led_line[LED_IND(X,Y)] &= ~_BV(LED_BIT(X,Y)); }while(0)
+#define BIT_7219(X,Y) TEST(led_line[LED_IND(X,Y)], LED_BIT(X,Y))
+
+#ifdef CPU_32_BIT
+  #define SIG_DELAY() DELAY_US(1)   // Approximate a 1µs delay on 32-bit ARM
+  #undef CRITICAL_SECTION_START
+  #undef CRITICAL_SECTION_END
+  #define CRITICAL_SECTION_START() NOOP
+  #define CRITICAL_SECTION_END()   NOOP
+#else
+  #define SIG_DELAY() DELAY_NS(250)
+#endif
+
+void Max7219::error(const char * const func, const int32_t v1, const int32_t v2/*=-1*/) {
+  #if ENABLED(MAX7219_ERRORS)
+    SERIAL_ECHOPGM("??? Max7219::");
+    serialprintPGM(func);
+    SERIAL_CHAR('(');
+    SERIAL_ECHO(v1);
+    if (v2 > 0) SERIAL_ECHOPAIR(", ", v2);
+    SERIAL_CHAR(')');
+    SERIAL_EOL();
+  #else
+    UNUSED(func); UNUSED(v1); UNUSED(v2);
+  #endif
+}
+
+/**
+ * Flip the lowest n_bytes of the supplied bits:
+ *  flipped(x, 1) flips the low 8  bits of x.
+ *  flipped(x, 2) flips the low 16 bits of x.
+ *  flipped(x, 3) flips the low 24 bits of x.
+ *  flipped(x, 4) flips the low 32 bits of x.
+ */
+inline uint32_t flipped(const uint32_t bits, const uint8_t n_bytes) {
+  uint32_t mask = 1, outbits = 0;
+  LOOP_L_N(b, n_bytes * 8) {
+    outbits <<= 1;
+    if (bits & mask) outbits |= 1;
+    mask <<= 1;
+  }
+  return outbits;
+}
+
+void Max7219::noop() {
+  CRITICAL_SECTION_START();
+  SIG_DELAY();
+  WRITE(MAX7219_DIN_PIN, LOW);
+  for (uint8_t i = 16; i--;) {
+    SIG_DELAY();
+    WRITE(MAX7219_CLK_PIN, LOW);
+    SIG_DELAY();
+    SIG_DELAY();
+    WRITE(MAX7219_CLK_PIN, HIGH);
+    SIG_DELAY();
+  }
+  CRITICAL_SECTION_END();
+}
+
+void Max7219::putbyte(uint8_t data) {
+  CRITICAL_SECTION_START();
+  for (uint8_t i = 8; i--;) {
+    SIG_DELAY();
+    WRITE(MAX7219_CLK_PIN, LOW);       // tick
+    SIG_DELAY();
+    WRITE(MAX7219_DIN_PIN, (data & 0x80) ? HIGH : LOW);  // send 1 or 0 based on data bit
+    SIG_DELAY();
+    WRITE(MAX7219_CLK_PIN, HIGH);      // tock
+    SIG_DELAY();
+    data <<= 1;
+  }
+  CRITICAL_SECTION_END();
+}
+
+void Max7219::pulse_load() {
+  SIG_DELAY();
+  WRITE(MAX7219_LOAD_PIN, LOW);  // tell the chip to load the data
+  SIG_DELAY();
+  WRITE(MAX7219_LOAD_PIN, HIGH);
+  SIG_DELAY();
+}
+
+void Max7219::send(const uint8_t reg, const uint8_t data) {
+  SIG_DELAY();
+  CRITICAL_SECTION_START();
+  SIG_DELAY();
+  putbyte(reg);          // specify register
+  SIG_DELAY();
+  putbyte(data);         // put data
+  CRITICAL_SECTION_END();
+}
+
+// Send out a single native row of bits to just one unit
+void Max7219::refresh_unit_line(const uint8_t line) {
+  if (suspended) return;
+  #if MAX7219_NUMBER_UNITS == 1
+    send(LINE_REG(line), led_line[line]);
+  #else
+    for (uint8_t u = MAX7219_NUMBER_UNITS; u--;)
+      if (u == (line >> 3)) send(LINE_REG(line), led_line[line]); else noop();
+  #endif
+  pulse_load();
+}
+
+// Send out a single native row of bits to all units
+void Max7219::refresh_line(const uint8_t line) {
+  if (suspended) return;
+  #if MAX7219_NUMBER_UNITS == 1
+    refresh_unit_line(line);
+  #else
+    for (uint8_t u = MAX7219_NUMBER_UNITS; u--;)
+      send(LINE_REG(line), led_line[(u << 3) | (line & 0x7)]);
+  #endif
+  pulse_load();
+}
+
+void Max7219::set(const uint8_t line, const uint8_t bits) {
+  led_line[line] = bits;
+  refresh_unit_line(line);
+}
+
+#if ENABLED(MAX7219_NUMERIC)
+
+  // Draw an integer with optional leading zeros and optional decimal point
+  void Max7219::print(const uint8_t start, int16_t value, uint8_t size, const bool leadzero=false, bool dec=false) {
+    if (suspended) return;
+    constexpr uint8_t led_numeral[10] = { 0x7E, 0x60, 0x6D, 0x79, 0x63, 0x5B, 0x5F, 0x70, 0x7F, 0x7A },
+                      led_decimal = 0x80, led_minus = 0x01;
+    bool blank = false, neg = value < 0;
+    if (neg) value *= -1;
+    while (size--) {
+      const bool minus = neg && blank;
+      if (minus) neg = false;
+      send(
+        max7219_reg_digit0 + start + size,
+        minus ? led_minus : blank ? 0x00 : led_numeral[value % 10] | (dec ? led_decimal : 0x00)
+      );
+      pulse_load();  // tell the chips to load the clocked out data
+      value /= 10;
+      if (!value && !leadzero) blank = true;
+      dec = false;
+    }
+  }
+
+  // Draw a float with a decimal point and optional digits
+  void Max7219::print(const uint8_t start, const float value, const uint8_t pre_size, const uint8_t post_size, const bool leadzero=false) {
+    if (pre_size) print(start, value, pre_size, leadzero, !!post_size);
+    if (post_size) {
+      const int16_t after = ABS(value) * (10 ^ post_size);
+      print(start + pre_size, after, post_size, true);
+    }
+  }
+
+#endif // MAX7219_NUMERIC
+
+// Modify a single LED bit and send the changed line
+void Max7219::led_set(const uint8_t x, const uint8_t y, const bool on) {
+  if (x >= MAX7219_X_LEDS || y >= MAX7219_Y_LEDS) return error(PSTR("led_set"), x, y);
+  if (BIT_7219(x, y) == on) return;
+  XOR_7219(x, y);
+  refresh_unit_line(LED_IND(x, y));
+}
+
+void Max7219::led_on(const uint8_t x, const uint8_t y) {
+  if (x >= MAX7219_X_LEDS || y >= MAX7219_Y_LEDS) return error(PSTR("led_on"), x, y);
+  led_set(x, y, true);
+}
+
+void Max7219::led_off(const uint8_t x, const uint8_t y) {
+  if (x >= MAX7219_X_LEDS || y >= MAX7219_Y_LEDS) return error(PSTR("led_off"), x, y);
+  led_set(x, y, false);
+}
+
+void Max7219::led_toggle(const uint8_t x, const uint8_t y) {
+  if (x >= MAX7219_X_LEDS || y >= MAX7219_Y_LEDS) return error(PSTR("led_toggle"), x, y);
+  led_set(x, y, !BIT_7219(x, y));
+}
+
+void Max7219::send_row(const uint8_t row) {
+  if (suspended) return;
+  #if _ROT == 0 || _ROT == 180            // Native Lines are horizontal too
+    #if MAX7219_X_LEDS <= 8
+      refresh_unit_line(LED_IND(0, row)); // A single unit line
+    #else
+      refresh_line(LED_IND(0, row));      // Same line, all units
+    #endif
+  #else                                   // Native lines are vertical
+    UNUSED(row);
+    refresh();                            // Actually a column
+  #endif
+}
+
+void Max7219::send_column(const uint8_t col) {
+  if (suspended) return;
+  #if _ROT == 90 || _ROT == 270           // Native Lines are vertical too
+    #if MAX7219_Y_LEDS <= 8
+      refresh_unit_line(LED_IND(col, 0)); // A single unit line
+    #else
+      refresh_line(LED_IND(col, 0));      // Same line, all units
+    #endif
+  #else                                   // Native lines are horizontal
+    UNUSED(col);
+    refresh();                            // Actually a row
+  #endif
+}
+
+void Max7219::clear() {
+  ZERO(led_line);
+  refresh();
+}
+
+void Max7219::fill() {
+  memset(led_line, 0xFF, sizeof(led_line));
+  refresh();
+}
+
+void Max7219::clear_row(const uint8_t row) {
+  if (row >= MAX7219_Y_LEDS) return error(PSTR("clear_row"), row);
+  LOOP_L_N(x, MAX7219_X_LEDS) CLR_7219(x, row);
+  send_row(row);
+}
+
+void Max7219::clear_column(const uint8_t col) {
+  if (col >= MAX7219_X_LEDS) return error(PSTR("set_column"), col);
+  LOOP_L_N(y, MAX7219_Y_LEDS) CLR_7219(col, y);
+  send_column(col);
+}
+
+/**
+ * Plot the low order bits of val to the specified row of the matrix.
+ * With 4 Max7219 units in the chain, it's possible to set 32 bits at
+ * once with a single call to the function (if rotated 90° or 270°).
+ */
+void Max7219::set_row(const uint8_t row, const uint32_t val) {
+  if (row >= MAX7219_Y_LEDS) return error(PSTR("set_row"), row);
+  uint32_t mask = _BV32(MAX7219_X_LEDS - 1);
+  LOOP_L_N(x, MAX7219_X_LEDS) {
+    if (val & mask) SET_7219(x, row); else CLR_7219(x, row);
+    mask >>= 1;
+  }
+  send_row(row);
+}
+
+/**
+ * Plot the low order bits of val to the specified column of the matrix.
+ * With 4 Max7219 units in the chain, it's possible to set 32 bits at
+ * once with a single call to the function (if rotated 0° or 180°).
+ */
+void Max7219::set_column(const uint8_t col, const uint32_t val) {
+  if (col >= MAX7219_X_LEDS) return error(PSTR("set_column"), col);
+  uint32_t mask = _BV32(MAX7219_Y_LEDS - 1);
+  LOOP_L_N(y, MAX7219_Y_LEDS) {
+    if (val & mask) SET_7219(col, y); else CLR_7219(col, y);
+    mask >>= 1;
+  }
+  send_column(col);
+}
+
+void Max7219::set_rows_16bits(const uint8_t y, uint32_t val) {
+  #if MAX7219_X_LEDS == 8
+    if (y > MAX7219_Y_LEDS - 2) return error(PSTR("set_rows_16bits"), y, val);
+    set_row(y + 1, val); val >>= 8;
+    set_row(y + 0, val);
+  #else // at least 16 bits on each row
+    if (y > MAX7219_Y_LEDS - 1) return error(PSTR("set_rows_16bits"), y, val);
+    set_row(y, val);
+  #endif
+}
+
+void Max7219::set_rows_32bits(const uint8_t y, uint32_t val) {
+  #if MAX7219_X_LEDS == 8
+    if (y > MAX7219_Y_LEDS - 4) return error(PSTR("set_rows_32bits"), y, val);
+    set_row(y + 3, val); val >>= 8;
+    set_row(y + 2, val); val >>= 8;
+    set_row(y + 1, val); val >>= 8;
+    set_row(y + 0, val);
+  #elif MAX7219_X_LEDS == 16
+    if (y > MAX7219_Y_LEDS - 2) return error(PSTR("set_rows_32bits"), y, val);
+    set_row(y + 1, val); val >>= 16;
+    set_row(y + 0, val);
+  #else // at least 24 bits on each row.  In the 3 matrix case, just display the low 24 bits
+    if (y > MAX7219_Y_LEDS - 1) return error(PSTR("set_rows_32bits"), y, val);
+    set_row(y, val);
+  #endif
+}
+
+void Max7219::set_columns_16bits(const uint8_t x, uint32_t val) {
+  #if MAX7219_Y_LEDS == 8
+    if (x > MAX7219_X_LEDS - 2) return error(PSTR("set_columns_16bits"), x, val);
+    set_column(x + 0, val); val >>= 8;
+    set_column(x + 1, val);
+  #else // at least 16 bits in each column
+    if (x > MAX7219_X_LEDS - 1) return error(PSTR("set_columns_16bits"), x, val);
+    set_column(x, val);
+  #endif
+}
+
+void Max7219::set_columns_32bits(const uint8_t x, uint32_t val) {
+  #if MAX7219_Y_LEDS == 8
+    if (x > MAX7219_X_LEDS - 4) return error(PSTR("set_rows_32bits"), x, val);
+    set_column(x + 3, val); val >>= 8;
+    set_column(x + 2, val); val >>= 8;
+    set_column(x + 1, val); val >>= 8;
+    set_column(x + 0, val);
+  #elif MAX7219_Y_LEDS == 16
+    if (x > MAX7219_X_LEDS - 2) return error(PSTR("set_rows_32bits"), x, val);
+    set_column(x + 1, val); val >>= 16;
+    set_column(x + 0, val);
+  #else // at least 24 bits on each row.  In the 3 matrix case, just display the low 24 bits
+    if (x > MAX7219_X_LEDS - 1) return error(PSTR("set_rows_32bits"), x, val);
+    set_column(x, val);
+  #endif
+}
+
+// Initialize the Max7219
+void Max7219::register_setup() {
+  LOOP_L_N(i, MAX7219_NUMBER_UNITS)
+    send(max7219_reg_scanLimit, 0x07);
+  pulse_load();                               // Tell the chips to load the clocked out data
+
+  LOOP_L_N(i, MAX7219_NUMBER_UNITS)
+    send(max7219_reg_decodeMode, 0x00);       // Using an led matrix (not digits)
+  pulse_load();                               // Tell the chips to load the clocked out data
+
+  LOOP_L_N(i, MAX7219_NUMBER_UNITS)
+    send(max7219_reg_shutdown, 0x01);         // Not in shutdown mode
+  pulse_load();                               // Tell the chips to load the clocked out data
+
+  LOOP_L_N(i, MAX7219_NUMBER_UNITS)
+    send(max7219_reg_displayTest, 0x00);      // No display test
+  pulse_load();                               // Tell the chips to load the clocked out data
+
+  LOOP_L_N(i, MAX7219_NUMBER_UNITS)
+    send(max7219_reg_intensity, 0x01 & 0x0F); // The first 0x0F is the value you can set
+                                              // Range: 0x00 to 0x0F
+  pulse_load();                               // Tell the chips to load the clocked out data
+}
+
+#ifdef MAX7219_INIT_TEST
+
+  uint8_t test_mode = 0;
+  millis_t next_patt_ms;
+  bool patt_on;
+
+  #if MAX7219_INIT_TEST == 2
+
+    #define MAX7219_LEDS (MAX7219_X_LEDS * MAX7219_Y_LEDS)
+
+    constexpr millis_t pattern_delay = 4;
+
+    int8_t spiralx, spiraly, spiral_dir;
+    IF<(MAX7219_LEDS > 255), uint16_t, uint8_t>::type spiral_count;
+
+    void Max7219::test_pattern() {
+      constexpr int8_t way[][2] = { { 1, 0 }, { 0, 1 }, { -1, 0 }, { 0, -1 } };
+      led_set(spiralx, spiraly, patt_on);
+      const int8_t x = spiralx + way[spiral_dir][0], y = spiraly + way[spiral_dir][1];
+      if (!WITHIN(x, 0, MAX7219_X_LEDS - 1) || !WITHIN(y, 0, MAX7219_Y_LEDS - 1) || BIT_7219(x, y) == patt_on)
+        spiral_dir = (spiral_dir + 1) & 0x3;
+      spiralx += way[spiral_dir][0];
+      spiraly += way[spiral_dir][1];
+      if (!spiral_count--) {
+        if (!patt_on)
+          test_mode = 0;
+        else {
+          spiral_count = MAX7219_LEDS;
+          spiralx = spiraly = spiral_dir = 0;
+          patt_on = false;
+        }
+      }
+    }
+
+  #else
+
+    constexpr millis_t pattern_delay = 20;
+    int8_t sweep_count, sweepx, sweep_dir;
+
+    void Max7219::test_pattern() {
+      set_column(sweepx, patt_on ? 0xFFFFFFFF : 0x00000000);
+      sweepx += sweep_dir;
+      if (!WITHIN(sweepx, 0, MAX7219_X_LEDS - 1)) {
+        if (!patt_on) {
+          sweep_dir *= -1;
+          sweepx += sweep_dir;
+        }
+        else
+          sweepx -= MAX7219_X_LEDS * sweep_dir;
+        patt_on ^= true;
+        next_patt_ms += 100;
+        if (++test_mode > 4) test_mode = 0;
+      }
+    }
+
+  #endif
+
+  void Max7219::run_test_pattern() {
+    const millis_t ms = millis();
+    if (PENDING(ms, next_patt_ms)) return;
+    next_patt_ms = ms + pattern_delay;
+    test_pattern();
+  }
+
+  void Max7219::start_test_pattern() {
+    clear();
+    test_mode = 1;
+    patt_on = true;
+    #if MAX7219_INIT_TEST == 2
+      spiralx = spiraly = spiral_dir = 0;
+      spiral_count = MAX7219_LEDS;
+    #else
+      sweep_dir = 1;
+      sweepx = 0;
+      sweep_count = MAX7219_X_LEDS;
+    #endif
+  }
+
+#endif // MAX7219_INIT_TEST
+
+void Max7219::init() {
+  SET_OUTPUT(MAX7219_DIN_PIN);
+  SET_OUTPUT(MAX7219_CLK_PIN);
+  OUT_WRITE(MAX7219_LOAD_PIN, HIGH);
+  delay(1);
+
+  register_setup();
+
+  LOOP_LE_N(i, 7) {  // Empty registers to turn all LEDs off
+    led_line[i] = 0x00;
+    send(max7219_reg_digit0 + i, 0);
+    pulse_load();                     // Tell the chips to load the clocked out data
+  }
+
+  #ifdef MAX7219_INIT_TEST
+    start_test_pattern();
+  #endif
+}
+
+/**
+ * This code demonstrates some simple debugging using a single 8x8 LED Matrix. If your feature could
+ * benefit from matrix display, add its code here. Very little processing is required, so the 7219 is
+ * ideal for debugging when realtime feedback is important but serial output can't be used.
+ */
+
+// Apply changes to update a marker
+void Max7219::mark16(const uint8_t pos, const uint8_t v1, const uint8_t v2) {
+  #if MAX7219_X_LEDS > 8    // At least 16 LEDs on the X-Axis. Use single line.
+    led_off(v1 & 0xF, pos);
+     led_on(v2 & 0xF, pos);
+  #elif MAX7219_Y_LEDS > 8  // At least 16 LEDs on the Y-Axis. Use a single column.
+    led_off(pos, v1 & 0xF);
+     led_on(pos, v2 & 0xF);
+  #else                     // Single 8x8 LED matrix. Use two lines to get 16 LEDs.
+    led_off(v1 & 0x7, pos + (v1 >= 8));
+     led_on(v2 & 0x7, pos + (v2 >= 8));
+  #endif
+}
+
+// Apply changes to update a tail-to-head range
+void Max7219::range16(const uint8_t y, const uint8_t ot, const uint8_t nt, const uint8_t oh, const uint8_t nh) {
+  #if MAX7219_X_LEDS > 8    // At least 16 LEDs on the X-Axis. Use single line.
+    if (ot != nt) for (uint8_t n = ot & 0xF; n != (nt & 0xF) && n != (nh & 0xF); n = (n + 1) & 0xF)
+      led_off(n & 0xF, y);
+    if (oh != nh) for (uint8_t n = (oh + 1) & 0xF; n != ((nh + 1) & 0xF); n = (n + 1) & 0xF)
+       led_on(n & 0xF, y);
+  #elif MAX7219_Y_LEDS > 8  // At least 16 LEDs on the Y-Axis. Use a single column.
+    if (ot != nt) for (uint8_t n = ot & 0xF; n != (nt & 0xF) && n != (nh & 0xF); n = (n + 1) & 0xF)
+      led_off(y, n & 0xF);
+    if (oh != nh) for (uint8_t n = (oh + 1) & 0xF; n != ((nh + 1) & 0xF); n = (n + 1) & 0xF)
+       led_on(y, n & 0xF);
+  #else                     // Single 8x8 LED matrix. Use two lines to get 16 LEDs.
+    if (ot != nt) for (uint8_t n = ot & 0xF; n != (nt & 0xF) && n != (nh & 0xF); n = (n + 1) & 0xF)
+      led_off(n & 0x7, y + (n >= 8));
+    if (oh != nh) for (uint8_t n = (oh + 1) & 0xF; n != ((nh + 1) & 0xF); n = (n + 1) & 0xF)
+       led_on(n & 0x7, y + (n >= 8));
+  #endif
+}
+
+// Apply changes to update a quantity
+void Max7219::quantity16(const uint8_t pos, const uint8_t ov, const uint8_t nv) {
+  for (uint8_t i = _MIN(nv, ov); i < _MAX(nv, ov); i++)
+    led_set(
+      #if MAX7219_X_LEDS > 8    // At least 16 LEDs on the X-Axis. Use single line.
+        i, pos
+      #elif MAX7219_Y_LEDS > 8  // At least 16 LEDs on the Y-Axis. Use a single column.
+        pos, i
+      #else                     // Single 8x8 LED matrix. Use two lines to get 16 LEDs.
+        i >> 1, pos + (i & 1)
+      #endif
+      , nv >= ov
+    );
+}
+
+void Max7219::idle_tasks() {
+  #define MAX7219_USE_HEAD (defined(MAX7219_DEBUG_PLANNER_HEAD) || defined(MAX7219_DEBUG_PLANNER_QUEUE))
+  #define MAX7219_USE_TAIL (defined(MAX7219_DEBUG_PLANNER_TAIL) || defined(MAX7219_DEBUG_PLANNER_QUEUE))
+  #if MAX7219_USE_HEAD || MAX7219_USE_TAIL
+    CRITICAL_SECTION_START();
+    #if MAX7219_USE_HEAD
+      const uint8_t head = planner.block_buffer_head;
+    #endif
+    #if MAX7219_USE_TAIL
+      const uint8_t tail = planner.block_buffer_tail;
+    #endif
+    CRITICAL_SECTION_END();
+  #endif
+
+  #if ENABLED(MAX7219_DEBUG_PRINTER_ALIVE)
+    static uint8_t refresh_cnt; // = 0
+    constexpr uint16_t refresh_limit = 5;
+    static millis_t next_blink = 0;
+    const millis_t ms = millis();
+    const bool do_blink = ELAPSED(ms, next_blink);
+  #else
+    static uint16_t refresh_cnt; // = 0
+    constexpr bool do_blink = true;
+    constexpr uint16_t refresh_limit = 50000;
+  #endif
+
+  // Some Max7219 units are vulnerable to electrical noise, especially
+  // with long wires next to high current wires. If the display becomes
+  // corrupted, this will fix it within a couple seconds.
+  if (do_blink && ++refresh_cnt >= refresh_limit) {
+    refresh_cnt = 0;
+    register_setup();
+  }
+
+  #ifdef MAX7219_INIT_TEST
+    if (test_mode) {
+      run_test_pattern();
+      return;
+    }
+  #endif
+
+  #if ENABLED(MAX7219_DEBUG_PRINTER_ALIVE)
+    if (do_blink) {
+      led_toggle(MAX7219_X_LEDS - 1, MAX7219_Y_LEDS - 1);
+      next_blink = ms + 1000;
+    }
+  #endif
+
+  #if defined(MAX7219_DEBUG_PLANNER_HEAD) && defined(MAX7219_DEBUG_PLANNER_TAIL) && MAX7219_DEBUG_PLANNER_HEAD == MAX7219_DEBUG_PLANNER_TAIL
+
+    static int16_t last_head_cnt = 0xF, last_tail_cnt = 0xF;
+
+    if (last_head_cnt != head || last_tail_cnt != tail) {
+      range16(MAX7219_DEBUG_PLANNER_HEAD, last_tail_cnt, tail, last_head_cnt, head);
+      last_head_cnt = head;
+      last_tail_cnt = tail;
+    }
+
+  #else
+
+    #ifdef MAX7219_DEBUG_PLANNER_HEAD
+      static int16_t last_head_cnt = 0x1;
+      if (last_head_cnt != head) {
+        mark16(MAX7219_DEBUG_PLANNER_HEAD, last_head_cnt, head);
+        last_head_cnt = head;
+      }
+    #endif
+
+    #ifdef MAX7219_DEBUG_PLANNER_TAIL
+      static int16_t last_tail_cnt = 0x1;
+      if (last_tail_cnt != tail) {
+        mark16(MAX7219_DEBUG_PLANNER_TAIL, last_tail_cnt, tail);
+        last_tail_cnt = tail;
+      }
+    #endif
+
+  #endif
+
+  #ifdef MAX7219_DEBUG_PLANNER_QUEUE
+    static int16_t last_depth = 0;
+    const int16_t current_depth = (head - tail + BLOCK_BUFFER_SIZE) & (BLOCK_BUFFER_SIZE - 1) & 0xF;
+    if (current_depth != last_depth) {
+      quantity16(MAX7219_DEBUG_PLANNER_QUEUE, last_depth, current_depth);
+      last_depth = current_depth;
+    }
+  #endif
+
+  // After resume() automatically do a refresh()
+  if (suspended == 0x80) {
+    suspended = 0;
+    refresh();
+  }
+}
+
+#endif // MAX7219_DEBUG
diff --git a/Marlin/src/feature/max7219.h b/Marlin/src/feature/max7219.h
new file mode 100644
index 0000000..8e98c94
--- /dev/null
+++ b/Marlin/src/feature/max7219.h
@@ -0,0 +1,152 @@
+/**
+ * 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 module is off by default, but can be enabled to facilitate the display of
+ * extra debug information during code development.
+ *
+ * Just connect up 5V and GND to give it power, then connect up the pins assigned
+ * in Configuration_adv.h. For example, on the Re-ARM you could use:
+ *
+ *   #define MAX7219_CLK_PIN   77
+ *   #define MAX7219_DIN_PIN   78
+ *   #define MAX7219_LOAD_PIN  79
+ *
+ * max7219.init() is called automatically at startup, and then there are a number of
+ * support functions available to control the LEDs in the 8x8 grid.
+ *
+ * If you are using the Max7219 matrix for firmware debug purposes in time sensitive
+ * areas of the code, please be aware that the orientation (rotation) of the display can
+ * affect the speed.   The Max7219 can update a single column fairly fast.  It is much
+ * faster to do a Max7219_Set_Column() with a rotation of 90 or 270 degrees than to do
+ * a Max7219_Set_Row().    The opposite is true for rotations of 0 or 180 degrees.
+ */
+
+#ifndef MAX7219_ROTATE
+  #define MAX7219_ROTATE 0
+#endif
+#define _ROT ((MAX7219_ROTATE + 360) % 360)
+
+#ifndef MAX7219_NUMBER_UNITS
+  #define MAX7219_NUMBER_UNITS 1
+#endif
+#define MAX7219_LINES (8 * (MAX7219_NUMBER_UNITS))
+
+//
+// MAX7219 registers
+//
+#define max7219_reg_noop        0x00
+#define max7219_reg_digit0      0x01
+#define max7219_reg_digit1      0x02
+#define max7219_reg_digit2      0x03
+#define max7219_reg_digit3      0x04
+#define max7219_reg_digit4      0x05
+#define max7219_reg_digit5      0x06
+#define max7219_reg_digit6      0x07
+#define max7219_reg_digit7      0x08
+
+#define max7219_reg_decodeMode  0x09
+#define max7219_reg_intensity   0x0A
+#define max7219_reg_scanLimit   0x0B
+#define max7219_reg_shutdown    0x0C
+#define max7219_reg_displayTest 0x0F
+
+class Max7219 {
+public:
+  static uint8_t led_line[MAX7219_LINES];
+
+  Max7219() {}
+
+  static void init();
+  static void register_setup();
+  static void putbyte(uint8_t data);
+  static void pulse_load();
+
+  // Set a single register (e.g., a whole native row)
+  static void send(const uint8_t reg, const uint8_t data);
+
+  // Refresh all units
+  static inline void refresh() { for (uint8_t i = 0; i < 8; i++) refresh_line(i); }
+
+  // Suspend / resume updates to the LED unit
+  // Use these methods to speed up multiple changes
+  // or to apply updates from interrupt context.
+  static inline void suspend() { suspended++; }
+  static inline void resume() { suspended--; suspended |= 0x80; }
+
+  // Update a single native line on all units
+  static void refresh_line(const uint8_t line);
+
+  // Update a single native line on just one unit
+  static void refresh_unit_line(const uint8_t line);
+
+  // Set a single LED by XY coordinate
+  static void led_set(const uint8_t x, const uint8_t y, const bool on);
+  static void led_on(const uint8_t x, const uint8_t y);
+  static void led_off(const uint8_t x, const uint8_t y);
+  static void led_toggle(const uint8_t x, const uint8_t y);
+
+  // Set all LEDs in a single column
+  static void set_column(const uint8_t col, const uint32_t val);
+  static void clear_column(const uint8_t col);
+
+  // Set all LEDs in a single row
+  static void set_row(const uint8_t row, const uint32_t val);
+  static void clear_row(const uint8_t row);
+
+  // 16 and 32 bit versions of Row and Column functions
+  // Multiple rows and columns will be used to display the value if
+  // the array of matrix LED's is too narrow to accomplish the goal
+  static void set_rows_16bits(const uint8_t y, uint32_t val);
+  static void set_rows_32bits(const uint8_t y, uint32_t val);
+  static void set_columns_16bits(const uint8_t x, uint32_t val);
+  static void set_columns_32bits(const uint8_t x, uint32_t val);
+
+  // Quickly clear the whole matrix
+  static void clear();
+
+  // Quickly fill the whole matrix
+  static void fill();
+
+  // Apply custom code to update the matrix
+  static void idle_tasks();
+
+private:
+  static uint8_t suspended;
+  static void error(const char * const func, const int32_t v1, const int32_t v2=-1);
+  static void noop();
+  static void set(const uint8_t line, const uint8_t bits);
+  static void send_row(const uint8_t row);
+  static void send_column(const uint8_t col);
+  static void mark16(const uint8_t y, const uint8_t v1, const uint8_t v2);
+  static void range16(const uint8_t y, const uint8_t ot, const uint8_t nt, const uint8_t oh, const uint8_t nh);
+  static void quantity16(const uint8_t y, const uint8_t ov, const uint8_t nv);
+
+  #ifdef MAX7219_INIT_TEST
+    static void test_pattern();
+    static void run_test_pattern();
+    static void start_test_pattern();
+  #endif
+};
+
+extern Max7219 max7219;
diff --git a/Marlin/src/feature/meatpack.cpp b/Marlin/src/feature/meatpack.cpp
new file mode 100644
index 0000000..cd6d8ce
--- /dev/null
+++ b/Marlin/src/feature/meatpack.cpp
@@ -0,0 +1,228 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+/**
+ * MeatPack G-code Compression
+ *
+ * Algorithm & Implementation: Scott Mudge - mail@scottmudge.com
+ * Date: Dec. 2020
+ *
+ * Character Frequencies from ~30 MB of comment-stripped gcode:
+ *  '1' -> 4451136    '4' -> 1353273   '\n' -> 1087683    '-' ->   90242
+ *  '0' -> 4253577    '9' -> 1352147    'G' -> 1075806    'Z' ->   34109
+ *  ' ' -> 3053297    '3' -> 1262929    'X' ->  975742    'M' ->   11879
+ *  '.' -> 3035310    '5' -> 1189871    'E' ->  965275    'S' ->    9910
+ *  '2' -> 1523296    '6' -> 1127900    'Y' ->  965274
+ *  '8' -> 1366812    '7' -> 1112908    'F' ->   99416
+ *
+ * When space is omitted the letter 'E' is used in its place
+ */
+
+#include "../inc/MarlinConfig.h"
+
+#if ENABLED(MEATPACK)
+
+#include "meatpack.h"
+MeatPack meatpack;
+
+#define MeatPack_ProtocolVersion "PV01"
+//#define MP_DEBUG
+
+#define DEBUG_OUT ENABLED(MP_DEBUG)
+#include "../core/debug_out.h"
+
+bool MeatPack::cmd_is_next = false;       // A command is pending
+uint8_t MeatPack::state = 0;              // Configuration state OFF
+uint8_t MeatPack::second_char = 0;        // The unpacked 2nd character from an out-of-sequence packed pair
+uint8_t MeatPack::cmd_count = 0,          // Counts how many command bytes are received (need 2)
+        MeatPack::full_char_count = 0,    // Counts how many full-width characters are to be received
+        MeatPack::char_out_count = 0;     // Stores number of characters to be read out.
+uint8_t MeatPack::char_out_buf[2];        // Output buffer for caching up to 2 characters
+
+// The 15 most-common characters used in G-code, ~90-95% of all G-code uses these characters
+// Stored in SRAM for performance.
+uint8_t meatPackLookupTable[16] = {
+  '0', '1', '2', '3', '4', '5', '6', '7', '8', '9',
+  '.', ' ', '\n', 'G', 'X',
+  '\0' // Unused. 0b1111 indicates a literal character
+};
+
+TERN_(MP_DEBUG, uint8_t chars_decoded = 0); // Log the first 64 bytes after each reset
+
+void MeatPack::reset_state() {
+  state = 0;
+  cmd_is_next = false;
+  second_char = 0;
+  cmd_count = full_char_count = char_out_count = 0;
+  TERN_(MP_DEBUG, chars_decoded = 0);
+}
+
+/**
+ * Unpack one or two characters from a packed byte into a buffer.
+ * Return flags indicating whether any literal bytes follow.
+ */
+uint8_t MeatPack::unpack_chars(const uint8_t pk, uint8_t* __restrict const chars_out) {
+  uint8_t out = 0;
+
+  // If lower nybble is 1111, the higher nybble is unused, and next char is full.
+  if ((pk & kFirstNotPacked) == kFirstNotPacked)
+    out = kFirstCharIsLiteral;
+  else {
+    const uint8_t chr = pk & 0x0F;
+    chars_out[0] = meatPackLookupTable[chr]; // Set the first char
+  }
+
+  // Check if upper nybble is 1111... if so, we don't need the second char.
+  if ((pk & kSecondNotPacked) == kSecondNotPacked)
+    out |= kSecondCharIsLiteral;
+  else {
+    const uint8_t chr = (pk >> 4) & 0x0F;
+    chars_out[1] = meatPackLookupTable[chr]; // Set the second char
+  }
+
+  return out;
+}
+
+/**
+ * Interpret a single (non-command) character
+ * according to the current MeatPack state.
+ */
+void MeatPack::handle_rx_char_inner(const uint8_t c) {
+  if (TEST(state, MPConfig_Bit_Active)) {                   // Is MeatPack active?
+    if (!full_char_count) {                                 // No literal characters to fetch?
+      uint8_t buf[2] = { 0, 0 };
+      register const uint8_t res = unpack_chars(c, buf);    // Decode the byte into one or two characters.
+      if (res & kFirstCharIsLiteral) {                      // The 1st character couldn't be packed.
+        ++full_char_count;                                  // So the next stream byte is a full character.
+        if (res & kSecondCharIsLiteral) ++full_char_count;  // The 2nd character couldn't be packed. Another stream byte is a full character.
+        else second_char = buf[1];                          // Retain the unpacked second character.
+      }
+      else {
+        handle_output_char(buf[0]);                           // Send the unpacked first character out.
+        if (buf[0] != '\n') {                                 // After a newline the next char won't be set
+          if (res & kSecondCharIsLiteral) ++full_char_count;  // The 2nd character couldn't be packed. The next stream byte is a full character.
+          else handle_output_char(buf[1]);                    // Send the unpacked second character out.
+        }
+      }
+    }
+    else {
+      handle_output_char(c);                                // Pass through the character that couldn't be packed...
+      if (second_char) {
+        handle_output_char(second_char);                    // ...and send an unpacked 2nd character, if set.
+        second_char = 0;
+      }
+      --full_char_count;                                    // One literal character was consumed
+    }
+  }
+  else // Packing not enabled, just copy character to output
+    handle_output_char(c);
+}
+
+/**
+ * Buffer a single output character which will be picked up in
+ * GCodeQueue::get_serial_commands via calls to get_result_char
+ */
+void MeatPack::handle_output_char(const uint8_t c) {
+  char_out_buf[char_out_count++] = c;
+
+  #if ENABLED(MP_DEBUG)
+    if (chars_decoded < 1024) {
+      ++chars_decoded;
+      DEBUG_ECHOPGM("RB: ");
+      MYSERIAL.print((char)c);
+      DEBUG_EOL();
+    }
+  #endif
+}
+
+/**
+ * Process a MeatPack command byte to update the state.
+ * Report the new state to serial.
+ */
+void MeatPack::handle_command(const MeatPack_Command c) {
+  switch (c) {
+    case MPCommand_QueryConfig:     break;
+    case MPCommand_EnablePacking:   SBI(state, MPConfig_Bit_Active);   DEBUG_ECHOLNPGM("[MPDBG] ENA REC");   break;
+    case MPCommand_DisablePacking:  CBI(state, MPConfig_Bit_Active);   DEBUG_ECHOLNPGM("[MPDBG] DIS REC");   break;
+    case MPCommand_ResetAll:        reset_state();                     DEBUG_ECHOLNPGM("[MPDBG] RESET REC"); break;
+    case MPCommand_EnableNoSpaces:
+      SBI(state, MPConfig_Bit_NoSpaces);
+      meatPackLookupTable[kSpaceCharIdx] = kSpaceCharReplace;          DEBUG_ECHOLNPGM("[MPDBG] ENA NSP");   break;
+    case MPCommand_DisableNoSpaces:
+      CBI(state, MPConfig_Bit_NoSpaces);
+      meatPackLookupTable[kSpaceCharIdx] = ' ';                        DEBUG_ECHOLNPGM("[MPDBG] DIS NSP");   break;
+    default:                                                           DEBUG_ECHOLNPGM("[MPDBG] UNK CMD REC");
+  }
+  report_state();
+}
+
+void MeatPack::report_state() {
+  // NOTE: if any configuration vars are added below, the outgoing sync text for host plugin
+  // should not contain the "PV' substring, as this is used to indicate protocol version
+  SERIAL_ECHOPGM("[MP] ");
+  SERIAL_ECHOPGM(MeatPack_ProtocolVersion " ");
+  serialprint_onoff(TEST(state, MPConfig_Bit_Active));
+  serialprintPGM(TEST(state, MPConfig_Bit_NoSpaces) ? PSTR(" NSP\n") : PSTR(" ESP\n"));
+}
+
+/**
+ * Interpret a single character received from serial
+ * according to the current meatpack state.
+ */
+void MeatPack::handle_rx_char(const uint8_t c, const serial_index_t serial_ind) {
+  if (c == kCommandByte) {                // A command (0xFF) byte?
+    if (cmd_count) {                      // In fact, two in a row?
+      cmd_is_next = true;                 // Then a MeatPack command follows
+      cmd_count = 0;
+    }
+    else
+      ++cmd_count; // cmd_count = 1       // One command byte received so far...
+    return;
+  }
+
+  if (cmd_is_next) {                      // Were two command bytes received?
+    PORT_REDIRECT(serial_ind);
+    handle_command((MeatPack_Command)c);  // Then the byte is a MeatPack command
+    cmd_is_next = false;
+    return;
+  }
+
+  if (cmd_count) {                        // Only a single 0xFF was received
+    handle_rx_char_inner(kCommandByte);   // A single 0xFF is passed on literally so it can be interpreted as kFirstNotPacked|kSecondNotPacked
+    cmd_count = 0;
+  }
+
+  handle_rx_char_inner(c);                // Other characters are passed on for MeatPack decoding
+}
+
+uint8_t MeatPack::get_result_char(char* const __restrict out) {
+  uint8_t res = 0;
+  if (char_out_count) {
+    res = char_out_count;
+    char_out_count = 0;
+    for (register uint8_t i = 0; i < res; ++i)
+      out[i] = (char)char_out_buf[i];
+  }
+  return res;
+}
+
+#endif // MEATPACK
diff --git a/Marlin/src/feature/meatpack.h b/Marlin/src/feature/meatpack.h
new file mode 100644
index 0000000..2641130
--- /dev/null
+++ b/Marlin/src/feature/meatpack.h
@@ -0,0 +1,123 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+/*
+ * MeatPack G-code Compression
+ *
+ * Algorithm & Implementation: Scott Mudge - mail@scottmudge.com
+ * Date: Dec. 2020
+ *
+ * Specifically optimized for 3D printing G-Code, this is a zero-cost data compression method
+ * which packs ~180-190% more data into the same amount of bytes going to the CNC controller.
+ * As a majority of G-Code can be represented by a restricted alphabet, I performed histogram
+ * analysis on a wide variety of 3D printing gcode samples, and found ~93% of all gcode could
+ * be represented by the same 15-character alphabet.
+ *
+ * This allowed me to design a system of packing 2 8-bit characters into a single byte, assuming
+ * they fall within this limited 15-character alphabet. Using a 4-bit lookup table, these 8-bit
+ * characters can be represented by a 4-bit index.
+ *
+ * Combined with some logic to allow commingling of full-width characters outside of this 15-
+ * character alphabet (at the cost of an extra 8-bits per full-width character), and by stripping
+ * out unnecessary comments, the end result is gcode which is roughly half the original size.
+ *
+ * Why did I do this? I noticed micro-stuttering and other data-bottleneck issues while printing
+ * objects with high curvature, especially at high speeds. There is also the issue of the limited
+ * baud rate provided by Prusa's Atmega2560-based boards, over the USB serial connection. So soft-
+ * ware like OctoPrint would also suffer this same micro-stuttering and poor print quality issue.
+ *
+ */
+#pragma once
+
+#include <stdint.h>
+
+/**
+ * Commands sent to MeatPack to control its behavior.
+ * They are sent by first sending 2x MeatPack_CommandByte (0xFF) in sequence,
+ *      followed by one of the command bytes below.
+ * Provided that 0xFF is an exceedingly rare character that is virtually never
+ * present in G-code naturally, it is safe to assume 2 in sequence should never
+ * happen naturally, and so it is used as a signal here.
+ *
+ * 0xFF *IS* used in "packed" G-code (used to denote that the next 2 characters are
+ * full-width), however 2 in a row will never occur, as the next 2 bytes will always
+ * some non-0xFF character.
+ */
+enum MeatPack_Command : uint8_t {
+  MPCommand_None            = 0,
+  MPCommand_EnablePacking   = 0xFB,
+  MPCommand_DisablePacking  = 0xFA,
+  MPCommand_ResetAll        = 0xF9,
+  MPCommand_QueryConfig     = 0xF8,
+  MPCommand_EnableNoSpaces  = 0xF7,
+  MPCommand_DisableNoSpaces = 0xF6
+};
+
+enum MeatPack_ConfigStateBits : uint8_t {
+  MPConfig_Bit_Active   = 0,
+  MPConfig_Bit_NoSpaces = 1
+};
+
+class MeatPack {
+private:
+  friend class GCodeQueue;
+
+  // Utility definitions
+  static const uint8_t kCommandByte         = 0b11111111,
+                       kFirstNotPacked      = 0b00001111,
+                       kSecondNotPacked     = 0b11110000,
+                       kFirstCharIsLiteral  = 0b00000001,
+                       kSecondCharIsLiteral = 0b00000010;
+
+  static const uint8_t kSpaceCharIdx = 11;
+  static const char kSpaceCharReplace = 'E';
+
+  static bool cmd_is_next;        // A command is pending
+  static uint8_t state;           // Configuration state
+  static uint8_t second_char;     // Buffers a character if dealing with out-of-sequence pairs
+  static uint8_t cmd_count,       // Counter of command bytes received (need 2)
+                 full_char_count, // Counter for full-width characters to be received
+                 char_out_count;  // Stores number of characters to be read out.
+  static uint8_t char_out_buf[2]; // Output buffer for caching up to 2 characters
+
+  // Pass in a character rx'd by SD card or serial. Automatically parses command/ctrl sequences,
+  // and will control state internally.
+  static void handle_rx_char(const uint8_t c, const serial_index_t serial_ind);
+
+  /**
+   * After passing in rx'd char using above method, call this to get characters out.
+   * Can return from 0 to 2 characters at once.
+   * @param out [in] Output pointer for unpacked/processed data.
+   * @return Number of characters returned. Range from 0 to 2.
+   */
+  static uint8_t get_result_char(char* const __restrict out);
+
+  static void reset_state();
+  static void report_state();
+  static uint8_t unpacked_char(register const uint8_t in);
+  static uint8_t unpack_chars(const uint8_t pk, uint8_t* __restrict const chars_out);
+  static void handle_command(const MeatPack_Command c);
+  static void handle_output_char(const uint8_t c);
+  static void handle_rx_char_inner(const uint8_t c);
+};
+
+extern MeatPack meatpack;
diff --git a/Marlin/src/feature/mixing.cpp b/Marlin/src/feature/mixing.cpp
new file mode 100644
index 0000000..b002e98
--- /dev/null
+++ b/Marlin/src/feature/mixing.cpp
@@ -0,0 +1,193 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../inc/MarlinConfig.h"
+
+#if ENABLED(MIXING_EXTRUDER)
+
+//#define MIXER_NORMALIZER_DEBUG
+
+#include "mixing.h"
+
+Mixer mixer;
+
+#ifdef MIXER_NORMALIZER_DEBUG
+  #include "../core/serial.h"
+#endif
+
+// Used up to Planner level
+uint_fast8_t  Mixer::selected_vtool = 0;
+float         Mixer::collector[MIXING_STEPPERS]; // mix proportion. 0.0 = off, otherwise <= COLOR_A_MASK.
+mixer_comp_t  Mixer::color[NR_MIXING_VIRTUAL_TOOLS][MIXING_STEPPERS];
+
+// Used in Stepper
+int_fast8_t   Mixer::runner = 0;
+mixer_comp_t  Mixer::s_color[MIXING_STEPPERS];
+mixer_accu_t  Mixer::accu[MIXING_STEPPERS] = { 0 };
+
+#if EITHER(HAS_DUAL_MIXING, GRADIENT_MIX)
+  mixer_perc_t Mixer::mix[MIXING_STEPPERS];
+#endif
+
+void Mixer::normalize(const uint8_t tool_index) {
+  float cmax = 0;
+  #ifdef MIXER_NORMALIZER_DEBUG
+    float csum = 0;
+  #endif
+  MIXER_STEPPER_LOOP(i) {
+    const float v = collector[i];
+    NOLESS(cmax, v);
+    #ifdef MIXER_NORMALIZER_DEBUG
+      csum += v;
+    #endif
+  }
+  #ifdef MIXER_NORMALIZER_DEBUG
+    SERIAL_ECHOPGM("Mixer: Old relation : [ ");
+    MIXER_STEPPER_LOOP(i) {
+      SERIAL_ECHO_F(collector[i] / csum, 3);
+      SERIAL_CHAR(' ');
+    }
+    SERIAL_ECHOLNPGM("]");
+  #endif
+
+  // Scale all values so their maximum is COLOR_A_MASK
+  const float scale = float(COLOR_A_MASK) / cmax;
+  MIXER_STEPPER_LOOP(i) color[tool_index][i] = collector[i] * scale;
+
+  #ifdef MIXER_NORMALIZER_DEBUG
+    csum = 0;
+    SERIAL_ECHOPGM("Mixer: Normalize to : [ ");
+    MIXER_STEPPER_LOOP(i) {
+      SERIAL_ECHO(uint16_t(color[tool_index][i]));
+      SERIAL_CHAR(' ');
+      csum += color[tool_index][i];
+    }
+    SERIAL_ECHOLNPGM("]");
+    SERIAL_ECHOPGM("Mixer: New relation : [ ");
+    MIXER_STEPPER_LOOP(i) {
+      SERIAL_ECHO_F(uint16_t(color[tool_index][i]) / csum, 3);
+      SERIAL_CHAR(' ');
+    }
+    SERIAL_ECHOLNPGM("]");
+  #endif
+
+  TERN_(GRADIENT_MIX, refresh_gradient());
+}
+
+void Mixer::reset_vtools() {
+  // Virtual Tools 0, 1, 2, 3 = Filament 1, 2, 3, 4, etc.
+  // Every virtual tool gets a pure filament
+  LOOP_L_N(t, _MIN(MIXING_VIRTUAL_TOOLS, MIXING_STEPPERS))
+    MIXER_STEPPER_LOOP(i)
+      color[t][i] = (t == i) ? COLOR_A_MASK : 0;
+
+  // Remaining virtual tools are 100% filament 1
+  #if MIXING_VIRTUAL_TOOLS > MIXING_STEPPERS
+    LOOP_S_L_N(t, MIXING_STEPPERS, MIXING_VIRTUAL_TOOLS)
+      MIXER_STEPPER_LOOP(i)
+        color[t][i] = (i == 0) ? COLOR_A_MASK : 0;
+  #endif
+}
+
+// called at boot
+void Mixer::init() {
+
+  reset_vtools();
+
+  #if HAS_MIXER_SYNC_CHANNEL
+    // AUTORETRACT_TOOL gets the same amount of all filaments
+    MIXER_STEPPER_LOOP(i)
+      color[MIXER_AUTORETRACT_TOOL][i] = COLOR_A_MASK;
+  #endif
+
+  ZERO(collector);
+
+  #if EITHER(HAS_DUAL_MIXING, GRADIENT_MIX)
+    update_mix_from_vtool();
+  #endif
+
+  TERN_(GRADIENT_MIX, update_gradient_for_planner_z());
+}
+
+void Mixer::refresh_collector(const float proportion/*=1.0*/, const uint8_t t/*=selected_vtool*/, float (&c)[MIXING_STEPPERS]/*=collector*/) {
+  float csum = 0, cmax = 0;
+  MIXER_STEPPER_LOOP(i) {
+    const float v = color[t][i];
+    cmax = _MAX(cmax, v);
+    csum += v;
+  }
+  //SERIAL_ECHOPAIR("Mixer::refresh_collector(", proportion, ", ", int(t), ") cmax=", cmax, "  csum=", csum, "  color");
+  const float inv_prop = proportion / csum;
+  MIXER_STEPPER_LOOP(i) {
+    c[i] = color[t][i] * inv_prop;
+    //SERIAL_ECHOPAIR(" [", int(t), "][", int(i), "] = ", int(color[t][i]), " (", c[i], ")  ");
+  }
+  //SERIAL_EOL();
+}
+
+#if ENABLED(GRADIENT_MIX)
+
+  #include "../module/motion.h"
+  #include "../module/planner.h"
+
+  gradient_t Mixer::gradient = {
+    false,    // enabled
+    {0},      // color (array)
+    0, 0,     // start_z, end_z
+    0, 1,     // start_vtool, end_vtool
+    {0}, {0}  // start_mix[], end_mix[]
+    #if ENABLED(GRADIENT_VTOOL)
+      , -1    // vtool_index
+    #endif
+  };
+
+  float Mixer::prev_z; // = 0
+
+  void Mixer::update_gradient_for_z(const float z) {
+    if (z == prev_z) return;
+    prev_z = z;
+
+    const float slice = gradient.end_z - gradient.start_z;
+
+    float pct = (z - gradient.start_z) / slice;
+    NOLESS(pct, 0.0f); NOMORE(pct, 1.0f);
+
+    MIXER_STEPPER_LOOP(i) {
+      const mixer_perc_t sm = gradient.start_mix[i];
+      mix[i] = sm + (gradient.end_mix[i] - sm) * pct;
+    }
+
+    copy_mix_to_color(gradient.color);
+  }
+
+  void Mixer::update_gradient_for_planner_z() {
+    #if ENABLED(DELTA)
+      get_cartesian_from_steppers();
+      update_gradient_for_z(cartes.z);
+    #else
+      update_gradient_for_z(planner.get_axis_position_mm(Z_AXIS));
+    #endif
+  }
+
+#endif // GRADIENT_MIX
+
+#endif // MIXING_EXTRUDER
diff --git a/Marlin/src/feature/mixing.h b/Marlin/src/feature/mixing.h
new file mode 100644
index 0000000..7fe7062
--- /dev/null
+++ b/Marlin/src/feature/mixing.h
@@ -0,0 +1,263 @@
+/**
+ * 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 MIXER_NORMALIZER_DEBUG
+
+#ifndef __AVR__ // || HAS_DUAL_MIXING
+  // Use 16-bit (or fastest) data for the integer mix factors
+  typedef uint_fast16_t mixer_comp_t;
+  typedef uint_fast16_t mixer_accu_t;
+  #define COLOR_A_MASK 0x8000
+  #define COLOR_MASK 0x7FFF
+#else
+  // Use 8-bit data for the integer mix factors
+  // Exactness is sacrificed for speed
+  #define MIXER_ACCU_SIGNED
+  typedef uint8_t mixer_comp_t;
+  typedef int8_t mixer_accu_t;
+  #define COLOR_A_MASK 0x80
+  #define COLOR_MASK 0x7F
+#endif
+
+typedef int8_t mixer_perc_t;
+
+#ifndef MIXING_VIRTUAL_TOOLS
+  #define MIXING_VIRTUAL_TOOLS 1
+#endif
+
+enum MixTool {
+    FIRST_USER_VIRTUAL_TOOL = 0
+  , LAST_USER_VIRTUAL_TOOL = MIXING_VIRTUAL_TOOLS - 1
+  , NR_USER_VIRTUAL_TOOLS
+  , MIXER_DIRECT_SET_TOOL = NR_USER_VIRTUAL_TOOLS
+  #if HAS_MIXER_SYNC_CHANNEL
+    , MIXER_AUTORETRACT_TOOL
+  #endif
+  , NR_MIXING_VIRTUAL_TOOLS
+};
+
+#define MAX_VTOOLS TERN(HAS_MIXER_SYNC_CHANNEL, 254, 255)
+static_assert(NR_MIXING_VIRTUAL_TOOLS <= MAX_VTOOLS, "MIXING_VIRTUAL_TOOLS must be <= " STRINGIFY(MAX_VTOOLS) "!");
+
+#define MIXER_BLOCK_FIELD       mixer_comp_t b_color[MIXING_STEPPERS]
+#define MIXER_POPULATE_BLOCK()  mixer.populate_block(block->b_color)
+#define MIXER_STEPPER_SETUP()   mixer.stepper_setup(current_block->b_color)
+#define MIXER_STEPPER_LOOP(VAR) for (uint_fast8_t VAR = 0; VAR < MIXING_STEPPERS; VAR++)
+
+#if ENABLED(GRADIENT_MIX)
+
+  typedef struct {
+    bool enabled;                         // This gradient is enabled
+    mixer_comp_t color[MIXING_STEPPERS];  // The current gradient color
+    float start_z, end_z;                 // Region for gradient
+    int8_t start_vtool, end_vtool;        // Start and end virtual tools
+    mixer_perc_t start_mix[MIXING_STEPPERS],   // Start and end mixes from those tools
+                 end_mix[MIXING_STEPPERS];
+    TERN_(GRADIENT_VTOOL, int8_t vtool_index); // Use this virtual tool number as index
+  } gradient_t;
+
+#endif
+
+/**
+ * @brief Mixer class
+ * @details Contains data and behaviors for a Mixing Extruder
+ */
+class Mixer {
+  public:
+
+  static float collector[MIXING_STEPPERS];    // M163 components, also editable from LCD
+
+  static void init(); // Populate colors at boot time
+
+  static void reset_vtools();
+  static void refresh_collector(const float proportion=1.0, const uint8_t t=selected_vtool, float (&c)[MIXING_STEPPERS]=collector);
+
+  // Used up to Planner level
+  FORCE_INLINE static void set_collector(const uint8_t c, const float f) { collector[c] = _MAX(f, 0.0f); }
+
+  static void normalize(const uint8_t tool_index);
+  FORCE_INLINE static void normalize() { normalize(selected_vtool); }
+
+  FORCE_INLINE static uint8_t get_current_vtool() { return selected_vtool; }
+
+  FORCE_INLINE static void T(const uint_fast8_t c) {
+    selected_vtool = c;
+    TERN_(GRADIENT_VTOOL, refresh_gradient());
+    TERN_(HAS_DUAL_MIXING, update_mix_from_vtool());
+  }
+
+  // Used when dealing with blocks
+  FORCE_INLINE static void populate_block(mixer_comp_t b_color[MIXING_STEPPERS]) {
+    #if ENABLED(GRADIENT_MIX)
+      if (gradient.enabled) {
+        MIXER_STEPPER_LOOP(i) b_color[i] = gradient.color[i];
+        return;
+      }
+    #endif
+    MIXER_STEPPER_LOOP(i) b_color[i] = color[selected_vtool][i];
+  }
+
+  FORCE_INLINE static void stepper_setup(mixer_comp_t b_color[MIXING_STEPPERS]) {
+    MIXER_STEPPER_LOOP(i) s_color[i] = b_color[i];
+  }
+
+  #if EITHER(HAS_DUAL_MIXING, GRADIENT_MIX)
+
+    static mixer_perc_t mix[MIXING_STEPPERS];  // Scratch array for the Mix in proportion to 100
+
+    static inline void copy_mix_to_color(mixer_comp_t (&tcolor)[MIXING_STEPPERS]) {
+      // Scale each component to the largest one in terms of COLOR_A_MASK
+      // So the largest component will be COLOR_A_MASK and the other will be in proportion to it
+      const float scale = (COLOR_A_MASK) * RECIPROCAL(_MAX(
+        LIST_N(MIXING_STEPPERS, mix[0], mix[1], mix[2], mix[3], mix[4], mix[5])
+      ));
+
+      // Scale all values so their maximum is COLOR_A_MASK
+      MIXER_STEPPER_LOOP(i) tcolor[i] = mix[i] * scale;
+
+      #ifdef MIXER_NORMALIZER_DEBUG
+        SERIAL_ECHOPGM("Mix [ ");
+        SERIAL_ECHOLIST_N(MIXING_STEPPERS, int(mix[0]), int(mix[1]), int(mix[2]), int(mix[3]), int(mix[4]), int(mix[5]));
+        SERIAL_ECHOPGM(" ] to Color [ ");
+        SERIAL_ECHOLIST_N(MIXING_STEPPERS, int(tcolor[0]), int(tcolor[1]), int(tcolor[2]), int(tcolor[3]), int(tcolor[4]), int(tcolor[5]));
+        SERIAL_ECHOLNPGM(" ]");
+      #endif
+    }
+
+    static inline void update_mix_from_vtool(const uint8_t j=selected_vtool) {
+      float ctot = 0;
+      MIXER_STEPPER_LOOP(i) ctot += color[j][i];
+      //MIXER_STEPPER_LOOP(i) mix[i] = 100.0f * color[j][i] / ctot;
+      MIXER_STEPPER_LOOP(i) mix[i] = mixer_perc_t(100.0f * color[j][i] / ctot);
+
+      #ifdef MIXER_NORMALIZER_DEBUG
+        SERIAL_ECHOPAIR("V-tool ", int(j), " [ ");
+        SERIAL_ECHOLIST_N(MIXING_STEPPERS, int(color[j][0]), int(color[j][1]), int(color[j][2]), int(color[j][3]), int(color[j][4]), int(color[j][5]));
+        SERIAL_ECHOPGM(" ] to Mix [ ");
+        SERIAL_ECHOLIST_N(MIXING_STEPPERS, int(mix[0]), int(mix[1]), int(mix[2]), int(mix[3]), int(mix[4]), int(mix[5]));
+        SERIAL_ECHOLNPGM(" ]");
+      #endif
+    }
+
+  #endif // HAS_DUAL_MIXING || GRADIENT_MIX
+
+  #if HAS_DUAL_MIXING
+
+    // Update the virtual tool from an edited mix
+    static inline void update_vtool_from_mix() {
+      copy_mix_to_color(color[selected_vtool]);
+      TERN_(GRADIENT_MIX, refresh_gradient());
+      // MIXER_STEPPER_LOOP(i) collector[i] = mix[i];
+      // normalize();
+    }
+
+  #endif // HAS_DUAL_MIXING
+
+  #if ENABLED(GRADIENT_MIX)
+
+    static gradient_t gradient;
+    static float prev_z;
+
+    // Update the current mix from the gradient for a given Z
+    static void update_gradient_for_z(const float z);
+    static void update_gradient_for_planner_z();
+    static inline void gradient_control(const float z) {
+      if (gradient.enabled) {
+        if (z >= gradient.end_z)
+          T(gradient.end_vtool);
+        else
+          update_gradient_for_z(z);
+      }
+    }
+
+    static inline void update_mix_from_gradient() {
+      float ctot = 0;
+      MIXER_STEPPER_LOOP(i) ctot += gradient.color[i];
+      MIXER_STEPPER_LOOP(i) mix[i] = (mixer_perc_t)CEIL(100.0f * gradient.color[i] / ctot);
+
+      #ifdef MIXER_NORMALIZER_DEBUG
+        SERIAL_ECHOPGM("Gradient [ ");
+        SERIAL_ECHOLIST_N(MIXING_STEPPERS, int(gradient.color[0]), int(gradient.color[1]), int(gradient.color[2]), int(gradient.color[3]), int(gradient.color[4]), int(gradient.color[5]));
+        SERIAL_ECHOPGM(" ] to Mix [ ");
+        SERIAL_ECHOLIST_N(MIXING_STEPPERS, int(mix[0]), int(mix[1]), int(mix[2]), int(mix[3]), int(mix[4]), int(mix[5]));
+        SERIAL_ECHOLNPGM(" ]");
+      #endif
+    }
+
+    // Refresh the gradient after a change
+    static void refresh_gradient() {
+      #if ENABLED(GRADIENT_VTOOL)
+        const bool is_grd = (gradient.vtool_index == -1 || selected_vtool == (uint8_t)gradient.vtool_index);
+      #else
+        constexpr bool is_grd = true;
+      #endif
+      gradient.enabled = is_grd && gradient.start_vtool != gradient.end_vtool && gradient.start_z < gradient.end_z;
+      if (gradient.enabled) {
+        mixer_perc_t mix_bak[MIXING_STEPPERS];
+        COPY(mix_bak, mix);
+        update_mix_from_vtool(gradient.start_vtool);
+        COPY(gradient.start_mix, mix);
+        update_mix_from_vtool(gradient.end_vtool);
+        COPY(gradient.end_mix, mix);
+        update_gradient_for_planner_z();
+        COPY(mix, mix_bak);
+        prev_z = -1;
+      }
+    }
+
+  #endif // GRADIENT_MIX
+
+  // Used in Stepper
+  FORCE_INLINE static uint8_t get_stepper() { return runner; }
+  FORCE_INLINE static uint8_t get_next_stepper() {
+    for (;;) {
+      if (--runner < 0) runner = MIXING_STEPPERS - 1;
+      accu[runner] += s_color[runner];
+      if (
+        #ifdef MIXER_ACCU_SIGNED
+          accu[runner] < 0
+        #else
+          accu[runner] & COLOR_A_MASK
+        #endif
+      ) {
+        accu[runner] &= COLOR_MASK;
+        return runner;
+      }
+    }
+  }
+
+  private:
+
+  // Used up to Planner level
+  static uint_fast8_t selected_vtool;
+  static mixer_comp_t color[NR_MIXING_VIRTUAL_TOOLS][MIXING_STEPPERS];
+
+  // Used in Stepper
+  static int_fast8_t  runner;
+  static mixer_comp_t s_color[MIXING_STEPPERS];
+  static mixer_accu_t accu[MIXING_STEPPERS];
+};
+
+extern Mixer mixer;
diff --git a/Marlin/src/feature/mmu/mmu.cpp b/Marlin/src/feature/mmu/mmu.cpp
new file mode 100644
index 0000000..9a44829
--- /dev/null
+++ b/Marlin/src/feature/mmu/mmu.cpp
@@ -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/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if HAS_PRUSA_MMU1
+
+#include "../module/stepper.h"
+
+void select_multiplexed_stepper(const uint8_t e) {
+  planner.synchronize();
+  disable_e_steppers();
+  WRITE(E_MUX0_PIN, TEST(e, 0) ? HIGH : LOW);
+  WRITE(E_MUX1_PIN, TEST(e, 1) ? HIGH : LOW);
+  WRITE(E_MUX2_PIN, TEST(e, 2) ? HIGH : LOW);
+  safe_delay(100);
+}
+
+#endif // HAS_PRUSA_MMU1
diff --git a/Marlin/src/feature/mmu/mmu.h b/Marlin/src/feature/mmu/mmu.h
new file mode 100644
index 0000000..10805c8
--- /dev/null
+++ b/Marlin/src/feature/mmu/mmu.h
@@ -0,0 +1,24 @@
+/**
+ * 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
+
+void select_multiplexed_stepper(const uint8_t e);
diff --git a/Marlin/src/feature/mmu/mmu2-serial-protocol.md b/Marlin/src/feature/mmu/mmu2-serial-protocol.md
new file mode 100644
index 0000000..7ff0901
--- /dev/null
+++ b/Marlin/src/feature/mmu/mmu2-serial-protocol.md
@@ -0,0 +1,94 @@
+Startup sequence
+================
+
+When initialized, MMU sends
+
+- MMU => 'start\n'
+
+We follow with
+
+- MMU <= 'S1\n'
+- MMU => 'ok*Firmware version*\n'
+- MMU <= 'S2\n'
+- MMU => 'ok*Build number*\n'
+
+#if (12V_mode)
+
+- MMU <= 'M1\n'
+- MMU => 'ok\n'
+
+#endif
+
+- MMU <= 'P0\n'
+- MMU => '*FINDA status*\n'
+
+Now we are sure MMU is available and ready. If there was a timeout or other communication problem somewhere, printer will be killed.
+
+- *Firmware version* is an integer value, but we don't care about it
+- *Build number* is an integer value and has to be >=126, or =>132 if 12V mode is enabled
+- *FINDA status* is 1 if the filament is loaded to the extruder, 0 otherwise
+
+
+*Build number* is checked against the required value, if it does not match, printer is halted.
+
+
+
+Toolchange
+==========
+
+- MMU <= 'T*Filament index*\n'
+
+MMU sends
+
+- MMU => 'ok\n'
+
+as soon as the filament is fed down to the extruder. We follow with
+
+- MMU <= 'C0\n'
+
+MMU will feed a few more millimeters of filament for the extruder gears to grab.
+When done, the MMU sends
+
+- MMU => 'ok\n'
+
+We don't wait for a response here but immediately continue with the next gcode which should
+be one or more extruder moves to feed the filament into the hotend.
+
+
+FINDA status
+============
+
+- MMU <= 'P0\n'
+- MMU => '*FINDA status*\n'
+
+*FINDA status* is 1 if the is filament loaded to the extruder, 0 otherwise. This could be used as filament runout sensor if probed regularly.
+
+
+
+Load filament
+=============
+
+- MMU <= 'L*Filament index*\n'
+
+MMU will feed filament down to the extruder, when done
+
+- MMU => 'ok\n'
+
+
+Unload filament
+=============
+
+- MMU <= 'U0\n'
+
+MMU will retract current filament from the extruder, when done
+
+- MMU => 'ok\n'
+
+
+
+Eject filament
+==============
+
+- MMU <= 'E*Filament index*\n'
+- MMU => 'ok\n'
+
diff --git a/Marlin/src/feature/mmu/mmu2.cpp b/Marlin/src/feature/mmu/mmu2.cpp
new file mode 100644
index 0000000..e303694
--- /dev/null
+++ b/Marlin/src/feature/mmu/mmu2.cpp
@@ -0,0 +1,1061 @@
+/**
+ * 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 HAS_PRUSA_MMU2
+
+#include "mmu2.h"
+#include "../../lcd/menu/menu_mmu2.h"
+
+MMU2 mmu2;
+
+#include "../../gcode/gcode.h"
+#include "../../lcd/marlinui.h"
+#include "../../libs/buzzer.h"
+#include "../../libs/nozzle.h"
+#include "../../module/temperature.h"
+#include "../../module/planner.h"
+#include "../../module/stepper/indirection.h"
+#include "../../MarlinCore.h"
+
+#if ENABLED(HOST_PROMPT_SUPPORT)
+  #include "../../feature/host_actions.h"
+#endif
+
+#if ENABLED(EXTENSIBLE_UI)
+  #include "../../lcd/extui/ui_api.h"
+#endif
+
+#define DEBUG_OUT ENABLED(MMU2_DEBUG)
+#include "../../core/debug_out.h"
+
+#define MMU_TODELAY 100
+#define MMU_TIMEOUT 10
+#define MMU_CMD_TIMEOUT 45000UL // 45s timeout for mmu commands (except P0)
+#define MMU_P0_TIMEOUT 3000UL   // Timeout for P0 command: 3seconds
+
+#define MMU2_COMMAND(S) tx_str_P(PSTR(S "\n"))
+
+#if ENABLED(MMU_EXTRUDER_SENSOR)
+  uint8_t mmu_idl_sens = 0;
+  static bool mmu_loading_flag = false;
+#endif
+
+#define MMU_CMD_NONE 0
+#define MMU_CMD_T0   0x10
+#define MMU_CMD_T1   0x11
+#define MMU_CMD_T2   0x12
+#define MMU_CMD_T3   0x13
+#define MMU_CMD_T4   0x14
+#define MMU_CMD_L0   0x20
+#define MMU_CMD_L1   0x21
+#define MMU_CMD_L2   0x22
+#define MMU_CMD_L3   0x23
+#define MMU_CMD_L4   0x24
+#define MMU_CMD_C0   0x30
+#define MMU_CMD_U0   0x40
+#define MMU_CMD_E0   0x50
+#define MMU_CMD_E1   0x51
+#define MMU_CMD_E2   0x52
+#define MMU_CMD_E3   0x53
+#define MMU_CMD_E4   0x54
+#define MMU_CMD_R0   0x60
+#define MMU_CMD_F0   0x70
+#define MMU_CMD_F1   0x71
+#define MMU_CMD_F2   0x72
+#define MMU_CMD_F3   0x73
+#define MMU_CMD_F4   0x74
+
+#define MMU_REQUIRED_FW_BUILDNR TERN(MMU2_MODE_12V, 132, 126)
+
+#define MMU2_NO_TOOL 99
+#define MMU_BAUD    115200
+
+bool MMU2::enabled, MMU2::ready, MMU2::mmu_print_saved;
+#if HAS_PRUSA_MMU2S
+  bool MMU2::mmu2s_triggered;
+#endif
+uint8_t MMU2::cmd, MMU2::cmd_arg, MMU2::last_cmd, MMU2::extruder;
+int8_t MMU2::state = 0;
+volatile int8_t MMU2::finda = 1;
+volatile bool MMU2::finda_runout_valid;
+int16_t MMU2::version = -1, MMU2::buildnr = -1;
+millis_t MMU2::prev_request, MMU2::prev_P0_request;
+char MMU2::rx_buffer[MMU_RX_SIZE], MMU2::tx_buffer[MMU_TX_SIZE];
+
+struct E_Step {
+  float extrude;        //!< extrude distance in mm
+  feedRate_t feedRate;  //!< feed rate in mm/s
+};
+
+static constexpr E_Step
+    ramming_sequence[] PROGMEM = { MMU2_RAMMING_SEQUENCE }
+  , load_to_nozzle_sequence[] PROGMEM = { MMU2_LOAD_TO_NOZZLE_SEQUENCE }
+  #if HAS_PRUSA_MMU2S
+    , can_load_sequence[] PROGMEM = { MMU2_CAN_LOAD_SEQUENCE }
+    , can_load_increment_sequence[] PROGMEM = { MMU2_CAN_LOAD_INCREMENT_SEQUENCE }
+  #endif
+;
+
+MMU2::MMU2() {
+  rx_buffer[0] = '\0';
+}
+
+void MMU2::init() {
+
+  set_runout_valid(false);
+
+  #if PIN_EXISTS(MMU2_RST)
+    WRITE(MMU2_RST_PIN, HIGH);
+    SET_OUTPUT(MMU2_RST_PIN);
+  #endif
+
+  MMU2_SERIAL.begin(MMU_BAUD);
+  extruder = MMU2_NO_TOOL;
+
+  safe_delay(10);
+  reset();
+  rx_buffer[0] = '\0';
+  state = -1;
+}
+
+void MMU2::reset() {
+  DEBUG_ECHOLNPGM("MMU <= reset");
+
+  #if PIN_EXISTS(MMU2_RST)
+    WRITE(MMU2_RST_PIN, LOW);
+    safe_delay(20);
+    WRITE(MMU2_RST_PIN, HIGH);
+  #else
+    MMU2_COMMAND("X0"); // Send soft reset
+  #endif
+}
+
+uint8_t MMU2::get_current_tool() {
+  return extruder == MMU2_NO_TOOL ? -1 : extruder;
+}
+
+#if EITHER(HAS_PRUSA_MMU2S, MMU_EXTRUDER_SENSOR)
+  #define FILAMENT_PRESENT() (READ(FIL_RUNOUT1_PIN) != FIL_RUNOUT1_STATE)
+#endif
+
+void MMU2::mmu_loop() {
+
+  switch (state) {
+
+    case 0: break;
+
+    case -1:
+      if (rx_start()) {
+        prev_P0_request = millis();   // Initialize finda sensor timeout
+
+        DEBUG_ECHOLNPGM("MMU => 'start'");
+        DEBUG_ECHOLNPGM("MMU <= 'S1'");
+
+        MMU2_COMMAND("S1");   // Read Version
+        state = -2;
+      }
+      else if (millis() > 3000000) {
+        SERIAL_ECHOLNPGM("MMU not responding - DISABLED");
+        state = 0;
+      }
+      break;
+
+    case -2:
+      if (rx_ok()) {
+        sscanf(rx_buffer, "%huok\n", &version);
+
+        DEBUG_ECHOLNPAIR("MMU => ", version, "\nMMU <= 'S2'");
+
+        MMU2_COMMAND("S2");   // Read Build Number
+        state = -3;
+      }
+      break;
+
+    case -3:
+      if (rx_ok()) {
+        sscanf(rx_buffer, "%huok\n", &buildnr);
+
+        DEBUG_ECHOLNPAIR("MMU => ", buildnr);
+
+        check_version();
+
+        #if ENABLED(MMU2_MODE_12V)
+          DEBUG_ECHOLNPGM("MMU <= 'M1'");
+
+          MMU2_COMMAND("M1");   // Stealth Mode
+          state = -5;
+
+        #else
+          DEBUG_ECHOLNPGM("MMU <= 'P0'");
+
+          MMU2_COMMAND("P0");   // Read FINDA
+          state = -4;
+        #endif
+      }
+      break;
+
+    #if ENABLED(MMU2_MODE_12V)
+    case -5:
+      // response to M1
+      if (rx_ok()) {
+        DEBUG_ECHOLNPGM("MMU => ok");
+
+        DEBUG_ECHOLNPGM("MMU <= 'P0'");
+
+        MMU2_COMMAND("P0");   // Read FINDA
+        state = -4;
+      }
+      break;
+    #endif
+
+    case -4:
+      if (rx_ok()) {
+        sscanf(rx_buffer, "%hhuok\n", &finda);
+
+        DEBUG_ECHOLNPAIR("MMU => ", finda, "\nMMU - ENABLED");
+
+        enabled = true;
+        state = 1;
+        TERN_(HAS_PRUSA_MMU2S, mmu2s_triggered = false);
+      }
+      break;
+
+    case 1:
+      if (cmd) {
+        if (WITHIN(cmd, MMU_CMD_T0, MMU_CMD_T4)) {
+          // tool change
+          int filament = cmd - MMU_CMD_T0;
+          DEBUG_ECHOLNPAIR("MMU <= T", filament);
+          tx_printf_P(PSTR("T%d\n"), filament);
+          TERN_(MMU_EXTRUDER_SENSOR, mmu_idl_sens = 1); // enable idler sensor, if any
+          state = 3; // wait for response
+        }
+        else if (WITHIN(cmd, MMU_CMD_L0, MMU_CMD_L4)) {
+          // load
+          int filament = cmd - MMU_CMD_L0;
+          DEBUG_ECHOLNPAIR("MMU <= L", filament);
+          tx_printf_P(PSTR("L%d\n"), filament);
+          state = 3; // wait for response
+        }
+        else if (cmd == MMU_CMD_C0) {
+          // continue loading
+          DEBUG_ECHOLNPGM("MMU <= 'C0'");
+          MMU2_COMMAND("C0");
+          state = 3; // wait for response
+        }
+        else if (cmd == MMU_CMD_U0) {
+          // unload current
+          DEBUG_ECHOLNPGM("MMU <= 'U0'");
+
+          MMU2_COMMAND("U0");
+          state = 3; // wait for response
+        }
+        else if (WITHIN(cmd, MMU_CMD_E0, MMU_CMD_E4)) {
+          // eject filament
+          int filament = cmd - MMU_CMD_E0;
+          DEBUG_ECHOLNPAIR("MMU <= E", filament);
+          tx_printf_P(PSTR("E%d\n"), filament);
+          state = 3; // wait for response
+        }
+        else if (cmd == MMU_CMD_R0) {
+          // recover after eject
+          DEBUG_ECHOLNPGM("MMU <= 'R0'");
+          MMU2_COMMAND("R0");
+          state = 3; // wait for response
+        }
+        else if (WITHIN(cmd, MMU_CMD_F0, MMU_CMD_F4)) {
+          // filament type
+          int filament = cmd - MMU_CMD_F0;
+          DEBUG_ECHOPAIR("MMU <= F", filament, " ");
+          DEBUG_ECHO_F(cmd_arg, DEC);
+          DEBUG_EOL();
+          tx_printf_P(PSTR("F%d %d\n"), filament, cmd_arg);
+          state = 3; // wait for response
+        }
+
+        last_cmd = cmd;
+        cmd = MMU_CMD_NONE;
+      }
+      else if (ELAPSED(millis(), prev_P0_request + 300)) {
+        MMU2_COMMAND("P0"); // Read FINDA
+        state = 2; // wait for response
+      }
+
+      TERN_(HAS_PRUSA_MMU2S, check_filament());
+      break;
+
+    case 2:   // response to command P0
+      if (rx_ok()) {
+        sscanf(rx_buffer, "%hhuok\n", &finda);
+
+        // This is super annoying. Only activate if necessary
+        // if (finda_runout_valid) DEBUG_ECHOLNPAIR_F("MMU <= 'P0'\nMMU => ", finda, 6);
+
+        if (!finda && finda_runout_valid) filament_runout();
+        if (cmd == MMU_CMD_NONE) ready = true;
+        state = 1;
+      }
+      else if (ELAPSED(millis(), prev_request + MMU_P0_TIMEOUT)) // Resend request after timeout (3s)
+        state = 1;
+
+      TERN_(HAS_PRUSA_MMU2S, check_filament());
+      break;
+
+    case 3:   // response to mmu commands
+      #if ENABLED(MMU_EXTRUDER_SENSOR)
+        if (mmu_idl_sens) {
+          if (FILAMENT_PRESENT() && mmu_loading_flag) {
+            DEBUG_ECHOLNPGM("MMU <= 'A'");
+            MMU2_COMMAND("A"); // send 'abort' request
+            mmu_idl_sens = 0;
+            DEBUG_ECHOLNPGM("MMU IDLER_SENSOR = 0 - ABORT");
+          }
+        }
+      #endif
+
+      if (rx_ok()) {
+        #if HAS_PRUSA_MMU2S
+          // Respond to C0 MMU command in MMU2S model
+          const bool keep_trying = !mmu2s_triggered && last_cmd == MMU_CMD_C0;
+          if (keep_trying) {
+            // MMU ok received but filament sensor not triggered, retrying...
+            DEBUG_ECHOLNPGM("MMU => 'ok' (filament not present in gears)");
+            DEBUG_ECHOLNPGM("MMU <= 'C0' (keep trying)");
+            MMU2_COMMAND("C0");
+          }
+        #else
+          constexpr bool keep_trying = false;
+        #endif
+
+        if (!keep_trying) {
+          DEBUG_ECHOLNPGM("MMU => 'ok'");
+          ready = true;
+          state = 1;
+          last_cmd = MMU_CMD_NONE;
+        }
+      }
+      else if (ELAPSED(millis(), prev_request + MMU_CMD_TIMEOUT)) {
+        // resend request after timeout
+        if (last_cmd) {
+          DEBUG_ECHOLNPGM("MMU retry");
+          cmd = last_cmd;
+          last_cmd = MMU_CMD_NONE;
+        }
+        state = 1;
+      }
+      TERN_(HAS_PRUSA_MMU2S, check_filament());
+      break;
+  }
+}
+
+/**
+ * Check if MMU was started
+ */
+bool MMU2::rx_start() {
+  // check for start message
+  return rx_str_P(PSTR("start\n"));
+}
+
+/**
+ * Check if the data received ends with the given string.
+ */
+bool MMU2::rx_str_P(const char* str) {
+  uint8_t i = strlen(rx_buffer);
+
+  while (MMU2_SERIAL.available()) {
+    rx_buffer[i++] = MMU2_SERIAL.read();
+
+    if (i == sizeof(rx_buffer) - 1) {
+      DEBUG_ECHOLNPGM("rx buffer overrun");
+      break;
+    }
+  }
+  rx_buffer[i] = '\0';
+
+  uint8_t len = strlen_P(str);
+
+  if (i < len) return false;
+
+  str += len;
+
+  while (len--) {
+    char c0 = pgm_read_byte(str--), c1 = rx_buffer[i--];
+    if (c0 == c1) continue;
+    if (c0 == '\r' && c1 == '\n') continue;  // match cr as lf
+    if (c0 == '\n' && c1 == '\r') continue;  // match lf as cr
+    return false;
+  }
+  return true;
+}
+
+/**
+ * Transfer data to MMU, no argument
+ */
+void MMU2::tx_str_P(const char* str) {
+  clear_rx_buffer();
+  uint8_t len = strlen_P(str);
+  LOOP_L_N(i, len) MMU2_SERIAL.write(pgm_read_byte(str++));
+  prev_request = millis();
+}
+
+/**
+ * Transfer data to MMU, single argument
+ */
+void MMU2::tx_printf_P(const char* format, int argument = -1) {
+  clear_rx_buffer();
+  uint8_t len = sprintf_P(tx_buffer, format, argument);
+  LOOP_L_N(i, len) MMU2_SERIAL.write(tx_buffer[i]);
+  prev_request = millis();
+}
+
+/**
+ * Transfer data to MMU, two arguments
+ */
+void MMU2::tx_printf_P(const char* format, int argument1, int argument2) {
+  clear_rx_buffer();
+  uint8_t len = sprintf_P(tx_buffer, format, argument1, argument2);
+  LOOP_L_N(i, len) MMU2_SERIAL.write(tx_buffer[i]);
+  prev_request = millis();
+}
+
+/**
+ * Empty the rx buffer
+ */
+void MMU2::clear_rx_buffer() {
+  while (MMU2_SERIAL.available()) MMU2_SERIAL.read();
+  rx_buffer[0] = '\0';
+}
+
+/**
+ * Check if we received 'ok' from MMU
+ */
+bool MMU2::rx_ok() {
+  if (rx_str_P(PSTR("ok\n"))) {
+    prev_P0_request = millis();
+    return true;
+  }
+  return false;
+}
+
+/**
+ * Check if MMU has compatible firmware
+ */
+void MMU2::check_version() {
+  if (buildnr < MMU_REQUIRED_FW_BUILDNR) {
+    SERIAL_ERROR_MSG("Invalid MMU2 firmware. Version >= " STRINGIFY(MMU_REQUIRED_FW_BUILDNR) " required.");
+    kill(GET_TEXT(MSG_KILL_MMU2_FIRMWARE));
+  }
+}
+
+static void mmu2_not_responding() {
+  LCD_MESSAGEPGM(MSG_MMU2_NOT_RESPONDING);
+  BUZZ(100, 659);
+  BUZZ(200, 698);
+  BUZZ(100, 659);
+  BUZZ(300, 440);
+  BUZZ(100, 659);
+}
+
+#if HAS_PRUSA_MMU2S
+
+  bool MMU2::load_to_gears() {
+    command(MMU_CMD_C0);
+    manage_response(true, true);
+    LOOP_L_N(i, MMU2_C0_RETRY) {  // Keep loading until filament reaches gears
+      if (mmu2s_triggered) break;
+      command(MMU_CMD_C0);
+      manage_response(true, true);
+      check_filament();
+    }
+    const bool success = mmu2s_triggered && can_load();
+    if (!success) mmu2_not_responding();
+    return success;
+  }
+
+  /**
+   * Handle tool change
+   */
+  void MMU2::tool_change(const uint8_t index) {
+
+    if (!enabled) return;
+
+    set_runout_valid(false);
+
+    if (index != extruder) {
+
+      DISABLE_AXIS_E0();
+      ui.status_printf_P(0, GET_TEXT(MSG_MMU2_LOADING_FILAMENT), int(index + 1));
+
+      command(MMU_CMD_T0 + index);
+      manage_response(true, true);
+
+      if (load_to_gears()) {
+        extruder = index; // filament change is finished
+        active_extruder = 0;
+        ENABLE_AXIS_E0();
+        SERIAL_ECHO_START();
+        SERIAL_ECHOLNPAIR(STR_ACTIVE_EXTRUDER, int(extruder));
+      }
+      ui.reset_status();
+    }
+
+    set_runout_valid(true);
+  }
+
+  /**
+   * Handle special T?/Tx/Tc commands
+   *
+   * T? Gcode to extrude shouldn't have to follow, load to extruder wheels is done automatically
+   * Tx Same as T?, except nozzle doesn't have to be preheated. Tc must be placed after extruder nozzle is preheated to finish filament load.
+   * Tc Load to nozzle after filament was prepared by Tx and extruder nozzle is already heated.
+   */
+  void MMU2::tool_change(const char* special) {
+      if (!enabled) return;
+
+      set_runout_valid(false);
+
+      switch (*special) {
+        case '?': {
+          #if ENABLED(MMU2_MENUS)
+            const uint8_t index = mmu2_choose_filament();
+            while (!thermalManager.wait_for_hotend(active_extruder, false)) safe_delay(100);
+            load_filament_to_nozzle(index);
+          #else
+            BUZZ(400, 40);
+          #endif
+        } break;
+
+        case 'x': {
+          #if ENABLED(MMU2_MENUS)
+            planner.synchronize();
+            const uint8_t index = mmu2_choose_filament();
+            DISABLE_AXIS_E0();
+            command(MMU_CMD_T0 + index);
+            manage_response(true, true);
+
+            if (load_to_gears()) {
+              mmu_loop();
+              ENABLE_AXIS_E0();
+              extruder = index;
+              active_extruder = 0;
+            }
+          #else
+            BUZZ(400, 40);
+          #endif
+        } break;
+
+        case 'c': {
+          while (!thermalManager.wait_for_hotend(active_extruder, false)) safe_delay(100);
+          load_to_nozzle();
+        } break;
+      }
+
+      set_runout_valid(true);
+  }
+
+#elif ENABLED(MMU_EXTRUDER_SENSOR)
+
+  /**
+   * Handle tool change
+   */
+  void MMU2::tool_change(const uint8_t index) {
+    if (!enabled) return;
+
+    set_runout_valid(false);
+
+    if (index != extruder) {
+      DISABLE_AXIS_E0();
+      if (FILAMENT_PRESENT()) {
+        DEBUG_ECHOLNPGM("Unloading\n");
+        mmu_loading_flag = false;
+        command(MMU_CMD_U0);
+        manage_response(true, true);
+      }
+      ui.status_printf_P(0, GET_TEXT(MSG_MMU2_LOADING_FILAMENT), int(index + 1));
+      mmu_loading_flag = true;
+      command(MMU_CMD_T0 + index);
+      manage_response(true, true);
+      mmu_continue_loading();
+      command(MMU_CMD_C0);
+      extruder = index;
+      active_extruder = 0;
+
+      ENABLE_AXIS_E0();
+      SERIAL_ECHO_START();
+      SERIAL_ECHOLNPAIR(STR_ACTIVE_EXTRUDER, int(extruder));
+
+      ui.reset_status();
+    }
+
+    set_runout_valid(true);
+  }
+
+  /**
+   * Handle special T?/Tx/Tc commands
+   *
+   * T? Gcode to extrude shouldn't have to follow, load to extruder wheels is done automatically
+   * Tx Same as T?, except nozzle doesn't have to be preheated. Tc must be placed after extruder nozzle is preheated to finish filament load.
+   * Tc Load to nozzle after filament was prepared by Tx and extruder nozzle is already heated.
+   */
+  void MMU2::tool_change(const char* special) {
+    if (!enabled) return;
+
+    set_runout_valid(false);
+
+    switch (*special) {
+      case '?': {
+        DEBUG_ECHOLNPGM("case ?\n");
+        #if ENABLED(MMU2_MENUS)
+          uint8_t index = mmu2_choose_filament();
+          while (!thermalManager.wait_for_hotend(active_extruder, false)) safe_delay(100);
+          load_filament_to_nozzle(index);
+        #else
+          BUZZ(400, 40);
+        #endif
+      } break;
+
+      case 'x': {
+        DEBUG_ECHOLNPGM("case x\n");
+        #if ENABLED(MMU2_MENUS)
+          planner.synchronize();
+          uint8_t index = mmu2_choose_filament();
+          DISABLE_AXIS_E0();
+          command(MMU_CMD_T0 + index);
+          manage_response(true, true);
+          mmu_continue_loading();
+          command(MMU_CMD_C0);
+          mmu_loop();
+
+          ENABLE_AXIS_E0();
+          extruder = index;
+          active_extruder = 0;
+        #else
+          BUZZ(400, 40);
+        #endif
+      } break;
+
+      case 'c': {
+        DEBUG_ECHOLNPGM("case c\n");
+        while (!thermalManager.wait_for_hotend(active_extruder, false)) safe_delay(100);
+        execute_extruder_sequence((const E_Step *)load_to_nozzle_sequence, COUNT(load_to_nozzle_sequence));
+      } break;
+    }
+
+    set_runout_valid(true);
+  }
+
+  void MMU2::mmu_continue_loading() {
+    for (uint8_t i = 0; i < MMU_LOADING_ATTEMPTS_NR; i++) {
+      DEBUG_ECHOLNPAIR("Additional load attempt #", i);
+      if (FILAMENT_PRESENT()) break;
+      command(MMU_CMD_C0);
+      manage_response(true, true);
+    }
+    if (!FILAMENT_PRESENT()) {
+      DEBUG_ECHOLNPGM("Filament never reached sensor, runout");
+      filament_runout();
+    }
+    mmu_idl_sens = 0;
+  }
+
+#else // !HAS_PRUSA_MMU2S && !MMU_EXTRUDER_SENSOR
+
+  /**
+   * Handle tool change
+   */
+  void MMU2::tool_change(const uint8_t index) {
+    if (!enabled) return;
+
+    set_runout_valid(false);
+
+    if (index != extruder) {
+      DISABLE_AXIS_E0();
+      ui.status_printf_P(0, GET_TEXT(MSG_MMU2_LOADING_FILAMENT), int(index + 1));
+      command(MMU_CMD_T0 + index);
+      manage_response(true, true);
+      command(MMU_CMD_C0);
+      extruder = index; //filament change is finished
+      active_extruder = 0;
+      ENABLE_AXIS_E0();
+      SERIAL_ECHO_START();
+      SERIAL_ECHOLNPAIR(STR_ACTIVE_EXTRUDER, int(extruder));
+      ui.reset_status();
+    }
+
+    set_runout_valid(true);
+  }
+
+  /**
+   * Handle special T?/Tx/Tc commands
+   *
+   * T? Gcode to extrude shouldn't have to follow, load to extruder wheels is done automatically
+   * Tx Same as T?, except nozzle doesn't have to be preheated. Tc must be placed after extruder nozzle is preheated to finish filament load.
+   * Tc Load to nozzle after filament was prepared by Tx and extruder nozzle is already heated.
+   */
+  void MMU2::tool_change(const char* special) {
+    if (!enabled) return;
+
+    set_runout_valid(false);
+
+    switch (*special) {
+      case '?': {
+        DEBUG_ECHOLNPGM("case ?\n");
+        #if ENABLED(MMU2_MENUS)
+          uint8_t index = mmu2_choose_filament();
+          while (!thermalManager.wait_for_hotend(active_extruder, false)) safe_delay(100);
+          load_filament_to_nozzle(index);
+        #else
+          BUZZ(400, 40);
+        #endif
+      } break;
+
+      case 'x': {
+        DEBUG_ECHOLNPGM("case x\n");
+        #if ENABLED(MMU2_MENUS)
+          planner.synchronize();
+          uint8_t index = mmu2_choose_filament();
+          DISABLE_AXIS_E0();
+          command(MMU_CMD_T0 + index);
+          manage_response(true, true);
+          command(MMU_CMD_C0);
+          mmu_loop();
+
+          ENABLE_AXIS_E0();
+          extruder = index;
+          active_extruder = 0;
+        #else
+          BUZZ(400, 40);
+        #endif
+      } break;
+
+      case 'c': {
+        DEBUG_ECHOLNPGM("case c\n");
+        while (!thermalManager.wait_for_hotend(active_extruder, false)) safe_delay(100);
+        execute_extruder_sequence((const E_Step *)load_to_nozzle_sequence, COUNT(load_to_nozzle_sequence));
+      } break;
+    }
+
+    set_runout_valid(true);
+  }
+
+#endif // HAS_PRUSA_MMU2S
+
+/**
+ * Set next command
+ */
+void MMU2::command(const uint8_t mmu_cmd) {
+  if (!enabled) return;
+  cmd = mmu_cmd;
+  ready = false;
+}
+
+/**
+ * Wait for response from MMU
+ */
+bool MMU2::get_response() {
+  while (cmd != MMU_CMD_NONE) idle();
+
+  while (!ready) {
+    idle();
+    if (state != 3) break;
+  }
+
+  const bool ret = ready;
+  ready = false;
+
+  return ret;
+}
+
+/**
+ * Wait for response and deal with timeout if necessary
+ */
+void MMU2::manage_response(const bool move_axes, const bool turn_off_nozzle) {
+
+  constexpr xyz_pos_t park_point = NOZZLE_PARK_POINT;
+  bool response = false;
+  mmu_print_saved = false;
+  xyz_pos_t resume_position;
+  int16_t resume_hotend_temp = thermalManager.degTargetHotend(active_extruder);
+
+  KEEPALIVE_STATE(PAUSED_FOR_USER);
+
+  while (!response) {
+
+    response = get_response(); // wait for "ok" from mmu
+
+    if (!response) {          // No "ok" was received in reserved time frame, user will fix the issue on mmu unit
+      if (!mmu_print_saved) { // First occurrence. Save current position, park print head, disable nozzle heater.
+
+        planner.synchronize();
+
+        mmu_print_saved = true;
+
+        SERIAL_ECHOLNPGM("MMU not responding");
+
+        resume_hotend_temp = thermalManager.degTargetHotend(active_extruder);
+        resume_position = current_position;
+
+        if (move_axes && all_axes_homed())
+          nozzle.park(0, park_point /*= NOZZLE_PARK_POINT*/);
+
+        if (turn_off_nozzle) thermalManager.setTargetHotend(0, active_extruder);
+
+        mmu2_not_responding();
+      }
+    }
+    else if (mmu_print_saved) {
+      SERIAL_ECHOLNPGM("MMU starts responding\n");
+
+      if (turn_off_nozzle && resume_hotend_temp) {
+        thermalManager.setTargetHotend(resume_hotend_temp, active_extruder);
+        LCD_MESSAGEPGM(MSG_HEATING);
+        BUZZ(200, 40);
+
+        while (!thermalManager.wait_for_hotend(active_extruder, false)) safe_delay(1000);
+      }
+
+      if (move_axes && all_axes_homed()) {
+        LCD_MESSAGEPGM(MSG_MMU2_RESUMING);
+        BUZZ(198, 404); BUZZ(4, 0); BUZZ(198, 404);
+
+        // Move XY to starting position, then Z
+        do_blocking_move_to_xy(resume_position, feedRate_t(NOZZLE_PARK_XY_FEEDRATE));
+
+        // Move Z_AXIS to saved position
+        do_blocking_move_to_z(resume_position.z, feedRate_t(NOZZLE_PARK_Z_FEEDRATE));
+      }
+      else {
+        BUZZ(198, 404); BUZZ(4, 0); BUZZ(198, 404);
+        LCD_MESSAGEPGM(MSG_MMU2_RESUMING);
+      }
+    }
+  }
+}
+
+void MMU2::set_filament_type(const uint8_t index, const uint8_t filamentType) {
+  if (!enabled) return;
+
+  cmd_arg = filamentType;
+  command(MMU_CMD_F0 + index);
+
+  manage_response(true, true);
+}
+
+void MMU2::filament_runout() {
+  queue.inject_P(PSTR(MMU2_FILAMENT_RUNOUT_SCRIPT));
+  planner.synchronize();
+}
+
+#if HAS_PRUSA_MMU2S
+
+  void MMU2::check_filament() {
+    const bool present = FILAMENT_PRESENT();
+    if (cmd == MMU_CMD_NONE && last_cmd == MMU_CMD_C0) {
+      if (present && !mmu2s_triggered) {
+        DEBUG_ECHOLNPGM("MMU <= 'A'");
+        tx_str_P(PSTR("A\n"));
+      }
+      // Slowly spin the extruder during C0
+      else {
+        while (planner.movesplanned() < 3) {
+          current_position.e += 0.25;
+          line_to_current_position(MMM_TO_MMS(120));
+        }
+      }
+    }
+    mmu2s_triggered = present;
+  }
+
+  bool MMU2::can_load() {
+    execute_extruder_sequence((const E_Step *)can_load_sequence, COUNT(can_load_sequence));
+
+    int filament_detected_count = 0;
+    const int steps = (MMU2_CAN_LOAD_RETRACT) / (MMU2_CAN_LOAD_INCREMENT);
+    DEBUG_ECHOLNPGM("MMU can_load:");
+    LOOP_L_N(i, steps) {
+      execute_extruder_sequence((const E_Step *)can_load_increment_sequence, COUNT(can_load_increment_sequence));
+      check_filament(); // Don't trust the idle function
+      DEBUG_CHAR(mmu2s_triggered ? 'O' : 'o');
+      if (mmu2s_triggered) ++filament_detected_count;
+    }
+
+    if (filament_detected_count <= steps - (MMU2_CAN_LOAD_DEVIATION) / (MMU2_CAN_LOAD_INCREMENT)) {
+      DEBUG_ECHOLNPGM(" failed.");
+      return false;
+    }
+
+    DEBUG_ECHOLNPGM(" succeeded.");
+    return true;
+  }
+
+#endif
+
+// Load filament into MMU2
+void MMU2::load_filament(const uint8_t index) {
+  if (!enabled) return;
+
+  command(MMU_CMD_L0 + index);
+  manage_response(false, false);
+  BUZZ(200, 404);
+}
+
+/**
+ * Switch material and load to nozzle
+ */
+bool MMU2::load_filament_to_nozzle(const uint8_t index) {
+
+  if (!enabled) return false;
+
+  if (thermalManager.tooColdToExtrude(active_extruder)) {
+    BUZZ(200, 404);
+    LCD_ALERTMESSAGEPGM(MSG_HOTEND_TOO_COLD);
+    return false;
+  }
+
+  DISABLE_AXIS_E0();
+  command(MMU_CMD_T0 + index);
+  manage_response(true, true);
+
+  const bool success = load_to_gears();
+  if (success) {
+    mmu_loop();
+    extruder = index;
+    active_extruder = 0;
+    load_to_nozzle();
+    BUZZ(200, 404);
+  }
+  return success;
+}
+
+/**
+ * Load filament to nozzle of multimaterial printer
+ *
+ * This function is used only after T? (user select filament) and M600 (change filament).
+ * It is not used after T0 .. T4 command (select filament), in such case, gcode is responsible for loading
+ * filament to nozzle.
+ */
+void MMU2::load_to_nozzle() {
+  execute_extruder_sequence((const E_Step *)load_to_nozzle_sequence, COUNT(load_to_nozzle_sequence));
+}
+
+bool MMU2::eject_filament(const uint8_t index, const bool recover) {
+
+  if (!enabled) return false;
+
+  if (thermalManager.tooColdToExtrude(active_extruder)) {
+    BUZZ(200, 404);
+    LCD_ALERTMESSAGEPGM(MSG_HOTEND_TOO_COLD);
+    return false;
+  }
+
+  LCD_MESSAGEPGM(MSG_MMU2_EJECTING_FILAMENT);
+
+  ENABLE_AXIS_E0();
+  current_position.e -= MMU2_FILAMENTCHANGE_EJECT_FEED;
+  line_to_current_position(MMM_TO_MMS(2500));
+  planner.synchronize();
+  command(MMU_CMD_E0 + index);
+  manage_response(false, false);
+
+  if (recover)  {
+    LCD_MESSAGEPGM(MSG_MMU2_EJECT_RECOVER);
+    BUZZ(200, 404);
+    TERN_(HOST_PROMPT_SUPPORT, host_prompt_do(PROMPT_USER_CONTINUE, PSTR("MMU2 Eject Recover"), CONTINUE_STR));
+    TERN_(EXTENSIBLE_UI, ExtUI::onUserConfirmRequired_P(PSTR("MMU2 Eject Recover")));
+    wait_for_user_response();
+    BUZZ(200, 404);
+    BUZZ(200, 404);
+
+    command(MMU_CMD_R0);
+    manage_response(false, false);
+  }
+
+  ui.reset_status();
+
+  // no active tool
+  extruder = MMU2_NO_TOOL;
+
+  set_runout_valid(false);
+
+  BUZZ(200, 404);
+
+  DISABLE_AXIS_E0();
+
+  return true;
+}
+
+/**
+ * Unload from hotend and retract to MMU
+ */
+bool MMU2::unload() {
+
+  if (!enabled) return false;
+
+  if (thermalManager.tooColdToExtrude(active_extruder)) {
+    BUZZ(200, 404);
+    LCD_ALERTMESSAGEPGM(MSG_HOTEND_TOO_COLD);
+    return false;
+  }
+
+  // Unload sequence to optimize shape of the tip of the unloaded filament
+  execute_extruder_sequence((const E_Step *)ramming_sequence, sizeof(ramming_sequence) / sizeof(E_Step));
+
+  command(MMU_CMD_U0);
+  manage_response(false, true);
+
+  BUZZ(200, 404);
+
+  // no active tool
+  extruder = MMU2_NO_TOOL;
+
+  set_runout_valid(false);
+
+  return true;
+}
+
+void MMU2::execute_extruder_sequence(const E_Step * sequence, int steps) {
+
+  planner.synchronize();
+  ENABLE_AXIS_E0();
+
+  const E_Step* step = sequence;
+
+  LOOP_L_N(i, steps) {
+    const float es = pgm_read_float(&(step->extrude));
+    const feedRate_t fr_mm_m = pgm_read_float(&(step->feedRate));
+
+    DEBUG_ECHO_START();
+    DEBUG_ECHOLNPAIR("E step ", es, "/", fr_mm_m);
+
+    current_position.e += es;
+    line_to_current_position(MMM_TO_MMS(fr_mm_m));
+    planner.synchronize();
+
+    step++;
+  }
+
+  DISABLE_AXIS_E0();
+}
+
+#endif // HAS_PRUSA_MMU2
diff --git a/Marlin/src/feature/mmu/mmu2.h b/Marlin/src/feature/mmu/mmu2.h
new file mode 100644
index 0000000..4326989
--- /dev/null
+++ b/Marlin/src/feature/mmu/mmu2.h
@@ -0,0 +1,110 @@
+/**
+ * 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"
+
+#if HAS_FILAMENT_SENSOR
+  #include "../runout.h"
+#endif
+
+#if SERIAL_USB
+  #define MMU_RX_SIZE 256
+  #define MMU_TX_SIZE 256
+#else
+  #define MMU_RX_SIZE  16
+  #define MMU_TX_SIZE  16
+#endif
+
+struct E_Step;
+
+class MMU2 {
+public:
+  MMU2();
+
+  static void init();
+  static void reset();
+  static void mmu_loop();
+  static void tool_change(const uint8_t index);
+  static void tool_change(const char* special);
+  static uint8_t get_current_tool();
+  static void set_filament_type(const uint8_t index, const uint8_t type);
+
+  static bool unload();
+  static void load_filament(uint8_t);
+  static void load_all();
+  static bool load_filament_to_nozzle(const uint8_t index);
+  static bool eject_filament(const uint8_t index, const bool recover);
+
+private:
+  static bool rx_str_P(const char* str);
+  static void tx_str_P(const char* str);
+  static void tx_printf_P(const char* format, const int argument);
+  static void tx_printf_P(const char* format, const int argument1, const int argument2);
+  static void clear_rx_buffer();
+
+  static bool rx_ok();
+  static bool rx_start();
+  static void check_version();
+
+  static void command(const uint8_t cmd);
+  static bool get_response();
+  static void manage_response(const bool move_axes, const bool turn_off_nozzle);
+
+  static void load_to_nozzle();
+  static void execute_extruder_sequence(const E_Step * sequence, int steps);
+
+  static void filament_runout();
+
+  #if HAS_PRUSA_MMU2S
+    static bool mmu2s_triggered;
+    static void check_filament();
+    static bool can_load();
+    static bool load_to_gears();
+  #else
+    FORCE_INLINE static bool load_to_gears() { return true; }
+  #endif
+
+  #if ENABLED(MMU_EXTRUDER_SENSOR)
+    static void mmu_continue_loading();
+  #endif
+
+  static bool enabled, ready, mmu_print_saved;
+
+  static uint8_t cmd, cmd_arg, last_cmd, extruder;
+  static int8_t state;
+  static volatile int8_t finda;
+  static volatile bool finda_runout_valid;
+  static int16_t version, buildnr;
+  static millis_t prev_request, prev_P0_request;
+  static char rx_buffer[MMU_RX_SIZE], tx_buffer[MMU_TX_SIZE];
+
+  static inline void set_runout_valid(const bool valid) {
+    finda_runout_valid = valid;
+    #if HAS_FILAMENT_SENSOR
+      if (valid) runout.reset();
+    #endif
+  }
+
+};
+
+extern MMU2 mmu2;
diff --git a/Marlin/src/feature/password/password.cpp b/Marlin/src/feature/password/password.cpp
new file mode 100644
index 0000000..90bb647
--- /dev/null
+++ b/Marlin/src/feature/password/password.cpp
@@ -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/>.
+ *
+ */
+
+#include "../../inc/MarlinConfigPre.h"
+
+#if ENABLED(PASSWORD_FEATURE)
+
+#include "password.h"
+#include "../../gcode/gcode.h"
+#include "../../core/serial.h"
+
+Password password;
+
+// public:
+bool     Password::is_set, Password::is_locked;
+uint32_t Password::value, Password::value_entry;
+
+//
+// Authenticate user with password.
+// Called from Setup, after SD Prinitng Stops/Aborts, and M510
+//
+void Password::lock_machine() {
+  is_locked = true;
+  TERN_(HAS_LCD_MENU, authenticate_user(ui.status_screen, screen_password_entry));
+}
+
+//
+// Authentication check
+//
+void Password::authentication_check() {
+  if (value_entry == value)
+    is_locked = false;
+  else
+    SERIAL_ECHOLNPGM(STR_WRONG_PASSWORD);
+
+  TERN_(HAS_LCD_MENU, authentication_done());
+}
+
+#endif // PASSWORD_FEATURE
diff --git a/Marlin/src/feature/password/password.h b/Marlin/src/feature/password/password.h
new file mode 100644
index 0000000..1382d6d
--- /dev/null
+++ b/Marlin/src/feature/password/password.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
+
+#include "../../lcd/marlinui.h"
+
+class Password {
+public:
+  static bool is_set, is_locked;
+  static uint32_t value, value_entry;
+
+  Password() { is_locked = false; }
+
+  static void lock_machine();
+  static void authentication_check();
+
+  #if HAS_LCD_MENU
+    static void access_menu_password();
+    static void authentication_done();
+    static void media_gatekeeper();
+
+    private:
+    static void authenticate_user(const screenFunc_t, const screenFunc_t);
+    static void menu_password();
+    static void menu_password_entry();
+    static void screen_password_entry();
+    static void screen_set_password();
+    static void start_over();
+
+    static void digit_entered();
+    static void set_password_done(const bool with_set=true);
+    static void menu_password_report();
+
+    static void remove_password();
+  #endif
+};
+
+extern Password password;
diff --git a/Marlin/src/feature/pause.cpp b/Marlin/src/feature/pause.cpp
new file mode 100644
index 0000000..5ab4f2b
--- /dev/null
+++ b/Marlin/src/feature/pause.cpp
@@ -0,0 +1,664 @@
+/**
+ * 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/>.
+ *
+ */
+
+/**
+ * feature/pause.cpp - Pause feature support functions
+ * This may be combined with related G-codes if features are consolidated.
+ */
+
+#include "../inc/MarlinConfigPre.h"
+
+#if ENABLED(ADVANCED_PAUSE_FEATURE)
+
+//#define DEBUG_PAUSE_RESUME
+
+#include "../MarlinCore.h"
+#include "../gcode/gcode.h"
+#include "../module/motion.h"
+#include "../module/planner.h"
+#include "../module/stepper.h"
+#include "../module/printcounter.h"
+#include "../module/temperature.h"
+
+#if ENABLED(FWRETRACT)
+  #include "fwretract.h"
+#endif
+
+#if HAS_FILAMENT_SENSOR
+  #include "runout.h"
+#endif
+
+#if ENABLED(HOST_ACTION_COMMANDS)
+  #include "host_actions.h"
+#endif
+
+#if ENABLED(EXTENSIBLE_UI)
+  #include "../lcd/extui/ui_api.h"
+#endif
+
+#include "../lcd/marlinui.h"
+
+#if HAS_BUZZER
+  #include "../libs/buzzer.h"
+#endif
+
+#if ENABLED(POWER_LOSS_RECOVERY)
+  #include "powerloss.h"
+#endif
+
+#include "../libs/nozzle.h"
+#include "pause.h"
+
+#define DEBUG_OUT ENABLED(DEBUG_PAUSE_RESUME)
+#include "../core/debug_out.h"
+
+// private:
+
+static xyze_pos_t resume_position;
+
+#if HAS_LCD_MENU
+  PauseMenuResponse pause_menu_response;
+  PauseMode pause_mode = PAUSE_MODE_PAUSE_PRINT;
+#endif
+
+fil_change_settings_t fc_settings[EXTRUDERS];
+
+#if ENABLED(SDSUPPORT)
+  #include "../sd/cardreader.h"
+#endif
+
+#if ENABLED(EMERGENCY_PARSER)
+  #define _PMSG(L) L##_M108
+#else
+  #define _PMSG(L) L##_LCD
+#endif
+
+#if HAS_BUZZER
+  static void impatient_beep(const int8_t max_beep_count, const bool restart=false) {
+
+    if (TERN0(HAS_LCD_MENU, pause_mode == PAUSE_MODE_PAUSE_PRINT)) return;
+
+    static millis_t next_buzz = 0;
+    static int8_t runout_beep = 0;
+
+    if (restart) next_buzz = runout_beep = 0;
+
+    const bool always = max_beep_count < 0;
+
+    const millis_t ms = millis();
+    if (ELAPSED(ms, next_buzz)) {
+      if (always || runout_beep < max_beep_count + 5) { // Only beep as long as we're supposed to
+        next_buzz = ms + ((always || runout_beep < max_beep_count) ? 1000 : 500);
+        BUZZ(50, 880 - (runout_beep & 1) * 220);
+        runout_beep++;
+      }
+    }
+  }
+  inline void first_impatient_beep(const int8_t max_beep_count) { impatient_beep(max_beep_count, true); }
+#else
+  inline void impatient_beep(const int8_t, const bool=false) {}
+  inline void first_impatient_beep(const int8_t) {}
+#endif
+
+/**
+ * Ensure a safe temperature for extrusion
+ *
+ * - Fail if the TARGET temperature is too low
+ * - Display LCD placard with temperature status
+ * - Return when heating is done or aborted
+ *
+ * Returns 'true' if heating was completed, 'false' for abort
+ */
+static bool ensure_safe_temperature(const bool wait=true, const PauseMode mode=PAUSE_MODE_SAME) {
+  DEBUG_SECTION(est, "ensure_safe_temperature", true);
+  DEBUG_ECHOLNPAIR("... wait:", int(wait), " mode:", int(mode));
+
+  #if ENABLED(PREVENT_COLD_EXTRUSION)
+    if (!DEBUGGING(DRYRUN) && thermalManager.targetTooColdToExtrude(active_extruder))
+      thermalManager.setTargetHotend(thermalManager.extrude_min_temp, active_extruder);
+  #endif
+
+  ui.pause_show_message(PAUSE_MESSAGE_HEATING, mode); UNUSED(mode);
+
+  if (wait) return thermalManager.wait_for_hotend(active_extruder);
+
+  // Allow interruption by Emergency Parser M108
+  wait_for_heatup = TERN1(PREVENT_COLD_EXTRUSION, !thermalManager.allow_cold_extrude);
+  while (wait_for_heatup && ABS(thermalManager.degHotend(active_extruder) - thermalManager.degTargetHotend(active_extruder)) > TEMP_WINDOW)
+    idle();
+  wait_for_heatup = false;
+
+  #if ENABLED(PREVENT_COLD_EXTRUSION)
+    // A user can cancel wait-for-heating with M108
+    if (!DEBUGGING(DRYRUN) && thermalManager.targetTooColdToExtrude(active_extruder)) {
+      SERIAL_ECHO_MSG(STR_ERR_HOTEND_TOO_COLD);
+      return false;
+    }
+  #endif
+
+  return true;
+}
+
+/**
+ * Load filament into the hotend
+ *
+ * - Fail if the a safe temperature was not reached
+ * - If pausing for confirmation, wait for a click or M108
+ * - Show "wait for load" placard
+ * - Load and purge filament
+ * - Show "Purge more" / "Continue" menu
+ * - Return when "Continue" is selected
+ *
+ * Returns 'true' if load was completed, 'false' for abort
+ */
+bool load_filament(const float &slow_load_length/*=0*/, const float &fast_load_length/*=0*/, const float &purge_length/*=0*/, const int8_t max_beep_count/*=0*/,
+                   const bool show_lcd/*=false*/, const bool pause_for_user/*=false*/,
+                   const PauseMode mode/*=PAUSE_MODE_PAUSE_PRINT*/
+                   DXC_ARGS
+) {
+  DEBUG_SECTION(lf, "load_filament", true);
+  DEBUG_ECHOLNPAIR("... slowlen:", slow_load_length, " fastlen:", fast_load_length, " purgelen:", purge_length, " maxbeep:", int(max_beep_count), " showlcd:", int(show_lcd), " pauseforuser:", int(pause_for_user), " pausemode:", int(mode) DXC_SAY);
+
+  if (!ensure_safe_temperature(false, mode)) {
+    if (show_lcd) ui.pause_show_message(PAUSE_MESSAGE_STATUS, mode);
+    return false;
+  }
+
+  if (pause_for_user) {
+    if (show_lcd) ui.pause_show_message(PAUSE_MESSAGE_INSERT, mode);
+    SERIAL_ECHO_MSG(_PMSG(STR_FILAMENT_CHANGE_INSERT));
+
+    first_impatient_beep(max_beep_count);
+
+    KEEPALIVE_STATE(PAUSED_FOR_USER);
+    wait_for_user = true;    // LCD click or M108 will clear this
+    #if ENABLED(HOST_PROMPT_SUPPORT)
+      const char tool = '0'
+        #if NUM_RUNOUT_SENSORS > 1
+          + active_extruder
+        #endif
+      ;
+      host_prompt_do(PROMPT_USER_CONTINUE, PSTR("Load Filament T"), tool, CONTINUE_STR);
+    #endif
+
+    TERN_(EXTENSIBLE_UI, ExtUI::onUserConfirmRequired_P(PSTR("Load Filament")));
+
+    while (wait_for_user) {
+      impatient_beep(max_beep_count);
+      idle_no_sleep();
+    }
+  }
+
+  if (show_lcd) ui.pause_show_message(PAUSE_MESSAGE_LOAD, mode);
+
+  #if ENABLED(DUAL_X_CARRIAGE)
+    const int8_t saved_ext        = active_extruder;
+    const bool saved_ext_dup_mode = extruder_duplication_enabled;
+    set_duplication_enabled(false, DXC_ext);
+  #endif
+
+  // Slow Load filament
+  if (slow_load_length) unscaled_e_move(slow_load_length, FILAMENT_CHANGE_SLOW_LOAD_FEEDRATE);
+
+  // Fast Load Filament
+  if (fast_load_length) {
+    #if FILAMENT_CHANGE_FAST_LOAD_ACCEL > 0
+      const float saved_acceleration = planner.settings.retract_acceleration;
+      planner.settings.retract_acceleration = FILAMENT_CHANGE_FAST_LOAD_ACCEL;
+    #endif
+
+    unscaled_e_move(fast_load_length, FILAMENT_CHANGE_FAST_LOAD_FEEDRATE);
+
+    #if FILAMENT_CHANGE_FAST_LOAD_ACCEL > 0
+      planner.settings.retract_acceleration = saved_acceleration;
+    #endif
+  }
+
+  #if ENABLED(DUAL_X_CARRIAGE)      // Tie the two extruders movement back together.
+    set_duplication_enabled(saved_ext_dup_mode, saved_ext);
+  #endif
+
+  #if ENABLED(ADVANCED_PAUSE_CONTINUOUS_PURGE)
+
+    if (show_lcd) ui.pause_show_message(PAUSE_MESSAGE_PURGE);
+
+    TERN_(HOST_PROMPT_SUPPORT, host_prompt_do(PROMPT_USER_CONTINUE, PSTR("Filament Purging..."), CONTINUE_STR));
+    TERN_(EXTENSIBLE_UI, ExtUI::onUserConfirmRequired_P(PSTR("Filament Purging...")));
+    wait_for_user = true; // A click or M108 breaks the purge_length loop
+    for (float purge_count = purge_length; purge_count > 0 && wait_for_user; --purge_count)
+      unscaled_e_move(1, ADVANCED_PAUSE_PURGE_FEEDRATE);
+    wait_for_user = false;
+
+  #else
+
+    do {
+      if (purge_length > 0) {
+        // "Wait for filament purge"
+        if (show_lcd) ui.pause_show_message(PAUSE_MESSAGE_PURGE);
+
+        // Extrude filament to get into hotend
+        unscaled_e_move(purge_length, ADVANCED_PAUSE_PURGE_FEEDRATE);
+      }
+
+      TERN_(HOST_PROMPT_SUPPORT, filament_load_host_prompt()); // Initiate another host prompt. (NOTE: host_response_handler may also do this!)
+
+      #if HAS_LCD_MENU
+        if (show_lcd) {
+          // Show "Purge More" / "Resume" menu and wait for reply
+          KEEPALIVE_STATE(PAUSED_FOR_USER);
+          wait_for_user = false;
+          ui.pause_show_message(PAUSE_MESSAGE_OPTION);
+          while (pause_menu_response == PAUSE_RESPONSE_WAIT_FOR) idle_no_sleep();
+        }
+      #endif
+
+      // Keep looping if "Purge More" was selected
+    } while (TERN0(HAS_LCD_MENU, show_lcd && pause_menu_response == PAUSE_RESPONSE_EXTRUDE_MORE));
+
+  #endif
+  TERN_(HOST_PROMPT_SUPPORT, host_action_prompt_end());
+
+  return true;
+}
+
+/**
+ * Disabling E steppers for manual filament change should be fine
+ * as long as users don't spin the E motor ridiculously fast and
+ * send current back to their board, potentially frying it.
+ */
+inline void disable_active_extruder() {
+  #if HAS_E_STEPPER_ENABLE
+    disable_e_stepper(active_extruder);
+    safe_delay(100);
+  #endif
+}
+
+/**
+ * Unload filament from the hotend
+ *
+ * - Fail if the a safe temperature was not reached
+ * - Show "wait for unload" placard
+ * - Retract, pause, then unload filament
+ * - Disable E stepper (on most machines)
+ *
+ * Returns 'true' if unload was completed, 'false' for abort
+ */
+bool unload_filament(const float &unload_length, const bool show_lcd/*=false*/,
+                     const PauseMode mode/*=PAUSE_MODE_PAUSE_PRINT*/
+                     #if BOTH(FILAMENT_UNLOAD_ALL_EXTRUDERS, MIXING_EXTRUDER)
+                       , const float &mix_multiplier/*=1.0*/
+                     #endif
+) {
+  DEBUG_SECTION(uf, "unload_filament", true);
+  DEBUG_ECHOLNPAIR("... unloadlen:", unload_length, " showlcd:", int(show_lcd), " mode:", int(mode)
+    #if BOTH(FILAMENT_UNLOAD_ALL_EXTRUDERS, MIXING_EXTRUDER)
+      , " mixmult:", mix_multiplier
+    #endif
+  );
+
+  #if !BOTH(FILAMENT_UNLOAD_ALL_EXTRUDERS, MIXING_EXTRUDER)
+    constexpr float mix_multiplier = 1.0;
+  #endif
+
+  if (!ensure_safe_temperature(false, mode)) {
+    if (show_lcd) ui.pause_show_message(PAUSE_MESSAGE_STATUS);
+    return false;
+  }
+
+  if (show_lcd) ui.pause_show_message(PAUSE_MESSAGE_UNLOAD, mode);
+
+  // Retract filament
+  unscaled_e_move(-(FILAMENT_UNLOAD_PURGE_RETRACT) * mix_multiplier, (PAUSE_PARK_RETRACT_FEEDRATE) * mix_multiplier);
+
+  // Wait for filament to cool
+  safe_delay(FILAMENT_UNLOAD_PURGE_DELAY);
+
+  // Quickly purge
+  unscaled_e_move((FILAMENT_UNLOAD_PURGE_RETRACT + FILAMENT_UNLOAD_PURGE_LENGTH) * mix_multiplier,
+                  (FILAMENT_UNLOAD_PURGE_FEEDRATE) * mix_multiplier);
+
+  // Unload filament
+  #if FILAMENT_CHANGE_UNLOAD_ACCEL > 0
+    const float saved_acceleration = planner.settings.retract_acceleration;
+    planner.settings.retract_acceleration = FILAMENT_CHANGE_UNLOAD_ACCEL;
+  #endif
+
+  unscaled_e_move(unload_length * mix_multiplier, (FILAMENT_CHANGE_UNLOAD_FEEDRATE) * mix_multiplier);
+
+  #if FILAMENT_CHANGE_FAST_LOAD_ACCEL > 0
+    planner.settings.retract_acceleration = saved_acceleration;
+  #endif
+
+  // Disable the Extruder for manual change
+  disable_active_extruder();
+
+  return true;
+}
+
+// public:
+
+/**
+ * Pause procedure
+ *
+ * - Abort if already paused
+ * - Send host action for pause, if configured
+ * - Abort if TARGET temperature is too low
+ * - Display "wait for start of filament change" (if a length was specified)
+ * - Initial retract, if current temperature is hot enough
+ * - Park the nozzle at the given position
+ * - Call unload_filament (if a length was specified)
+ *
+ * Return 'true' if pause was completed, 'false' for abort
+ */
+uint8_t did_pause_print = 0;
+
+bool pause_print(const float &retract, const xyz_pos_t &park_point, const float &unload_length/*=0*/, const bool show_lcd/*=false*/ DXC_ARGS) {
+  DEBUG_SECTION(pp, "pause_print", true);
+  DEBUG_ECHOLNPAIR("... park.x:", park_point.x, " y:", park_point.y, " z:", park_point.z, " unloadlen:", unload_length, " showlcd:", int(show_lcd) DXC_SAY);
+
+  UNUSED(show_lcd);
+
+  if (did_pause_print) return false; // already paused
+
+  #if ENABLED(HOST_ACTION_COMMANDS)
+    #ifdef ACTION_ON_PAUSED
+      host_action_paused();
+    #elif defined(ACTION_ON_PAUSE)
+      host_action_pause();
+    #endif
+  #endif
+
+  TERN_(HOST_PROMPT_SUPPORT, host_prompt_open(PROMPT_INFO, PSTR("Pause"), DISMISS_STR));
+
+  // Indicate that the printer is paused
+  ++did_pause_print;
+
+  // Pause the print job and timer
+  #if ENABLED(SDSUPPORT)
+    if (IS_SD_PRINTING()) {
+      card.pauseSDPrint();
+      ++did_pause_print; // Indicate SD pause also
+    }
+  #endif
+
+  print_job_timer.pause();
+
+  // Save current position
+  resume_position = current_position;
+
+  // Wait for buffered blocks to complete
+  planner.synchronize();
+
+  #if ENABLED(ADVANCED_PAUSE_FANS_PAUSE) && HAS_FAN
+    thermalManager.set_fans_paused(true);
+  #endif
+
+  // Initial retract before move to filament change position
+  if (retract && thermalManager.hotEnoughToExtrude(active_extruder)) {
+    DEBUG_ECHOLNPAIR("... retract:", retract);
+    unscaled_e_move(retract, PAUSE_PARK_RETRACT_FEEDRATE);
+  }
+
+  // Park the nozzle by moving up by z_lift and then moving to (x_pos, y_pos)
+  if (!axes_should_home())
+    nozzle.park(0, park_point);
+
+  #if ENABLED(DUAL_X_CARRIAGE)
+    const int8_t saved_ext        = active_extruder;
+    const bool saved_ext_dup_mode = extruder_duplication_enabled;
+    set_duplication_enabled(false, DXC_ext);
+  #endif
+
+  if (unload_length)   // Unload the filament
+    unload_filament(unload_length, show_lcd, PAUSE_MODE_CHANGE_FILAMENT);
+
+  #if ENABLED(DUAL_X_CARRIAGE)
+    set_duplication_enabled(saved_ext_dup_mode, saved_ext);
+  #endif
+
+  // Disable the Extruder for manual change
+  disable_active_extruder();
+
+  return true;
+}
+
+/**
+ * For Paused Print:
+ * - Show "Press button (or M108) to resume"
+ *
+ * For Filament Change:
+ * - Show "Insert filament and press button to continue"
+ *
+ * - Wait for a click before returning
+ * - Heaters can time out and must reheat before continuing
+ *
+ * Used by M125 and M600
+ */
+
+void show_continue_prompt(const bool is_reload) {
+  DEBUG_SECTION(scp, "pause_print", true);
+  DEBUG_ECHOLNPAIR("... is_reload:", int(is_reload));
+
+  ui.pause_show_message(is_reload ? PAUSE_MESSAGE_INSERT : PAUSE_MESSAGE_WAITING);
+  SERIAL_ECHO_START();
+  serialprintPGM(is_reload ? PSTR(_PMSG(STR_FILAMENT_CHANGE_INSERT) "\n") : PSTR(_PMSG(STR_FILAMENT_CHANGE_WAIT) "\n"));
+}
+
+void wait_for_confirmation(const bool is_reload/*=false*/, const int8_t max_beep_count/*=0*/ DXC_ARGS) {
+  DEBUG_SECTION(wfc, "wait_for_confirmation", true);
+  DEBUG_ECHOLNPAIR("... is_reload:", is_reload, " maxbeep:", int(max_beep_count) DXC_SAY);
+
+  bool nozzle_timed_out = false;
+
+  show_continue_prompt(is_reload);
+
+  first_impatient_beep(max_beep_count);
+
+  // Start the heater idle timers
+  const millis_t nozzle_timeout = SEC_TO_MS(PAUSE_PARK_NOZZLE_TIMEOUT);
+
+  HOTEND_LOOP() thermalManager.heater_idle[e].start(nozzle_timeout);
+
+  #if ENABLED(DUAL_X_CARRIAGE)
+    const int8_t saved_ext        = active_extruder;
+    const bool saved_ext_dup_mode = extruder_duplication_enabled;
+    set_duplication_enabled(false, DXC_ext);
+  #endif
+
+  // Wait for filament insert by user and press button
+  KEEPALIVE_STATE(PAUSED_FOR_USER);
+  TERN_(HOST_PROMPT_SUPPORT, host_prompt_do(PROMPT_USER_CONTINUE, GET_TEXT(MSG_NOZZLE_PARKED), CONTINUE_STR));
+  TERN_(EXTENSIBLE_UI, ExtUI::onUserConfirmRequired_P(GET_TEXT(MSG_NOZZLE_PARKED)));
+  wait_for_user = true;    // LCD click or M108 will clear this
+  while (wait_for_user) {
+    impatient_beep(max_beep_count);
+
+    // If the nozzle has timed out...
+    if (!nozzle_timed_out)
+      HOTEND_LOOP() nozzle_timed_out |= thermalManager.heater_idle[e].timed_out;
+
+    // Wait for the user to press the button to re-heat the nozzle, then
+    // re-heat the nozzle, re-show the continue prompt, restart idle timers, start over
+    if (nozzle_timed_out) {
+      ui.pause_show_message(PAUSE_MESSAGE_HEAT);
+      SERIAL_ECHO_MSG(_PMSG(STR_FILAMENT_CHANGE_HEAT));
+
+      TERN_(HOST_PROMPT_SUPPORT, host_prompt_do(PROMPT_USER_CONTINUE, GET_TEXT(MSG_HEATER_TIMEOUT), GET_TEXT(MSG_REHEAT)));
+
+      TERN_(EXTENSIBLE_UI, ExtUI::onUserConfirmRequired_P(GET_TEXT(MSG_HEATER_TIMEOUT)));
+
+      wait_for_user_response(0, true); // Wait for LCD click or M108
+
+      TERN_(HOST_PROMPT_SUPPORT, host_prompt_do(PROMPT_INFO, GET_TEXT(MSG_REHEATING)));
+
+      TERN_(EXTENSIBLE_UI, ExtUI::onStatusChanged_P(GET_TEXT(MSG_REHEATING)));
+
+      // Re-enable the heaters if they timed out
+      HOTEND_LOOP() thermalManager.reset_hotend_idle_timer(e);
+
+      // Wait for the heaters to reach the target temperatures
+      ensure_safe_temperature(false);
+
+      // Show the prompt to continue
+      show_continue_prompt(is_reload);
+
+      // Start the heater idle timers
+      const millis_t nozzle_timeout = SEC_TO_MS(PAUSE_PARK_NOZZLE_TIMEOUT);
+
+      HOTEND_LOOP() thermalManager.heater_idle[e].start(nozzle_timeout);
+      TERN_(HOST_PROMPT_SUPPORT, host_prompt_do(PROMPT_USER_CONTINUE, PSTR("Reheat Done"), CONTINUE_STR));
+      TERN_(EXTENSIBLE_UI, ExtUI::onUserConfirmRequired_P(PSTR("Reheat finished.")));
+      wait_for_user = true;
+      nozzle_timed_out = false;
+
+      first_impatient_beep(max_beep_count);
+    }
+    idle_no_sleep();
+  }
+  #if ENABLED(DUAL_X_CARRIAGE)
+    set_duplication_enabled(saved_ext_dup_mode, saved_ext);
+  #endif
+}
+
+/**
+ * Resume or Start print procedure
+ *
+ * - If not paused, do nothing and return
+ * - Reset heater idle timers
+ * - Load filament if specified, but only if:
+ *   - a nozzle timed out, or
+ *   - the nozzle is already heated.
+ * - Display "wait for print to resume"
+ * - Retract to prevent oozing
+ * - Move the nozzle back to resume_position
+ * - Unretract
+ * - Re-prime the nozzle...
+ *   -  FWRETRACT: Recover/prime from the prior G10.
+ *   - !FWRETRACT: Retract by resume_position.e, if negative.
+ *                 Not sure how this logic comes into use.
+ * - Sync the planner E to resume_position.e
+ * - Send host action for resume, if configured
+ * - Resume the current SD print job, if any
+ */
+void resume_print(const float &slow_load_length/*=0*/, const float &fast_load_length/*=0*/, const float &purge_length/*=ADVANCED_PAUSE_PURGE_LENGTH*/, const int8_t max_beep_count/*=0*/, int16_t targetTemp/*=0*/ DXC_ARGS) {
+  DEBUG_SECTION(rp, "resume_print", true);
+  DEBUG_ECHOLNPAIR("... slowlen:", slow_load_length, " fastlen:", fast_load_length, " purgelen:", purge_length, " maxbeep:", int(max_beep_count), " targetTemp:", targetTemp DXC_SAY);
+
+  /*
+  SERIAL_ECHOLNPAIR(
+    "start of resume_print()\ndual_x_carriage_mode:", dual_x_carriage_mode,
+    "\nextruder_duplication_enabled:", extruder_duplication_enabled,
+    "\nactive_extruder:", active_extruder,
+    "\n"
+  );
+  //*/
+
+  if (!did_pause_print) return;
+
+  // Re-enable the heaters if they timed out
+  bool nozzle_timed_out = false;
+  HOTEND_LOOP() {
+    nozzle_timed_out |= thermalManager.heater_idle[e].timed_out;
+    thermalManager.reset_hotend_idle_timer(e);
+  }
+
+  if (targetTemp > thermalManager.degTargetHotend(active_extruder)) {
+    thermalManager.setTargetHotend(targetTemp, active_extruder);
+  }
+
+  // Load the new filament
+  load_filament(slow_load_length, fast_load_length, purge_length, max_beep_count, true, nozzle_timed_out, PAUSE_MODE_SAME DXC_PASS);
+
+  if (targetTemp > 0) {
+    thermalManager.setTargetHotend(targetTemp, active_extruder);
+    thermalManager.wait_for_hotend(active_extruder, false);
+  }
+
+  ui.pause_show_message(PAUSE_MESSAGE_RESUME);
+
+  // Check Temperature before moving hotend
+  ensure_safe_temperature();
+
+  // Retract to prevent oozing
+  unscaled_e_move(-(PAUSE_PARK_RETRACT_LENGTH), feedRate_t(PAUSE_PARK_RETRACT_FEEDRATE));
+
+  if (!axes_should_home()) {
+    // Move XY to starting position, then Z
+    do_blocking_move_to_xy(resume_position, feedRate_t(NOZZLE_PARK_XY_FEEDRATE));
+
+    // Move Z_AXIS to saved position
+    do_blocking_move_to_z(resume_position.z, feedRate_t(NOZZLE_PARK_Z_FEEDRATE));
+  }
+
+  // Unretract
+  unscaled_e_move(PAUSE_PARK_RETRACT_LENGTH, feedRate_t(PAUSE_PARK_RETRACT_FEEDRATE));
+
+  // Intelligent resuming
+  #if ENABLED(FWRETRACT)
+    // If retracted before goto pause
+    if (fwretract.retracted[active_extruder])
+      unscaled_e_move(-fwretract.settings.retract_length, fwretract.settings.retract_feedrate_mm_s);
+  #endif
+
+  // If resume_position is negative
+  if (resume_position.e < 0) unscaled_e_move(resume_position.e, feedRate_t(PAUSE_PARK_RETRACT_FEEDRATE));
+  #if ADVANCED_PAUSE_RESUME_PRIME != 0
+    unscaled_e_move(ADVANCED_PAUSE_RESUME_PRIME, feedRate_t(ADVANCED_PAUSE_PURGE_FEEDRATE));
+  #endif
+
+  // Now all extrusion positions are resumed and ready to be confirmed
+  // Set extruder to saved position
+  planner.set_e_position_mm((destination.e = current_position.e = resume_position.e));
+
+  // Write PLR now to update the z axis value
+  TERN_(POWER_LOSS_RECOVERY, if (recovery.enabled) recovery.save(true));
+
+  ui.pause_show_message(PAUSE_MESSAGE_STATUS);
+
+  #ifdef ACTION_ON_RESUMED
+    host_action_resumed();
+  #elif defined(ACTION_ON_RESUME)
+    host_action_resume();
+  #endif
+
+  --did_pause_print;
+
+  TERN_(HOST_PROMPT_SUPPORT, host_prompt_open(PROMPT_INFO, PSTR("Resuming"), DISMISS_STR));
+
+  #if ENABLED(SDSUPPORT)
+    if (did_pause_print) { card.startFileprint(); --did_pause_print; }
+  #endif
+
+  #if ENABLED(ADVANCED_PAUSE_FANS_PAUSE) && HAS_FAN
+    thermalManager.set_fans_paused(false);
+  #endif
+
+  TERN_(HAS_FILAMENT_SENSOR, runout.reset());
+
+  // Resume the print job timer if it was running
+  if (print_job_timer.isPaused()) print_job_timer.start();
+
+  TERN_(HAS_DISPLAY, ui.reset_status());
+  TERN_(HAS_LCD_MENU, ui.return_to_status());
+}
+
+#endif // ADVANCED_PAUSE_FEATURE
diff --git a/Marlin/src/feature/pause.h b/Marlin/src/feature/pause.h
new file mode 100644
index 0000000..7e58d45
--- /dev/null
+++ b/Marlin/src/feature/pause.h
@@ -0,0 +1,108 @@
+/**
+ * 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
+
+/**
+ * feature/pause.h - Pause feature support functions
+ * This may be combined with related G-codes if features are consolidated.
+ */
+
+typedef struct {
+  float unload_length, load_length;
+} fil_change_settings_t;
+
+#include "../inc/MarlinConfigPre.h"
+
+#if ENABLED(ADVANCED_PAUSE_FEATURE)
+
+#include "../libs/nozzle.h"
+
+enum PauseMode : char {
+  PAUSE_MODE_SAME,
+  PAUSE_MODE_PAUSE_PRINT,
+  PAUSE_MODE_CHANGE_FILAMENT,
+  PAUSE_MODE_LOAD_FILAMENT,
+  PAUSE_MODE_UNLOAD_FILAMENT
+};
+
+enum PauseMessage : char {
+  PAUSE_MESSAGE_PARKING,
+  PAUSE_MESSAGE_CHANGING,
+  PAUSE_MESSAGE_WAITING,
+  PAUSE_MESSAGE_UNLOAD,
+  PAUSE_MESSAGE_INSERT,
+  PAUSE_MESSAGE_LOAD,
+  PAUSE_MESSAGE_PURGE,
+  PAUSE_MESSAGE_OPTION,
+  PAUSE_MESSAGE_RESUME,
+  PAUSE_MESSAGE_STATUS,
+  PAUSE_MESSAGE_HEAT,
+  PAUSE_MESSAGE_HEATING
+};
+
+#if HAS_LCD_MENU
+  enum PauseMenuResponse : char {
+    PAUSE_RESPONSE_WAIT_FOR,
+    PAUSE_RESPONSE_EXTRUDE_MORE,
+    PAUSE_RESPONSE_RESUME_PRINT
+  };
+  extern PauseMenuResponse pause_menu_response;
+  extern PauseMode pause_mode;
+#endif
+
+extern fil_change_settings_t fc_settings[EXTRUDERS];
+
+extern uint8_t did_pause_print;
+
+#if ENABLED(DUAL_X_CARRIAGE)
+  #define DXC_PARAMS , const int8_t DXC_ext=-1
+  #define DXC_ARGS   , const int8_t DXC_ext
+  #define DXC_PASS   , DXC_ext
+  #define DXC_SAY    , " dxc:", int(DXC_ext)
+#else
+  #define DXC_PARAMS
+  #define DXC_ARGS
+  #define DXC_PASS
+  #define DXC_SAY
+#endif
+
+bool pause_print(const float &retract, const xyz_pos_t &park_point, const float &unload_length=0, const bool show_lcd=false DXC_PARAMS);
+
+void wait_for_confirmation(const bool is_reload=false, const int8_t max_beep_count=0 DXC_PARAMS);
+
+void resume_print(const float &slow_load_length=0, const float &fast_load_length=0, const float &extrude_length=ADVANCED_PAUSE_PURGE_LENGTH,
+                    const int8_t max_beep_count=0, int16_t targetTemp=0 DXC_PARAMS);
+
+bool load_filament(const float &slow_load_length=0, const float &fast_load_length=0, const float &extrude_length=0, const int8_t max_beep_count=0,
+                    const bool show_lcd=false, const bool pause_for_user=false, const PauseMode mode=PAUSE_MODE_PAUSE_PRINT DXC_PARAMS);
+
+bool unload_filament(const float &unload_length, const bool show_lcd=false, const PauseMode mode=PAUSE_MODE_PAUSE_PRINT
+  #if BOTH(FILAMENT_UNLOAD_ALL_EXTRUDERS, MIXING_EXTRUDER)
+    , const float &mix_multiplier=1.0
+  #endif
+);
+
+#else // !ADVANCED_PAUSE_FEATURE
+
+  constexpr uint8_t did_pause_print = 0;
+
+#endif // !ADVANCED_PAUSE_FEATURE
diff --git a/Marlin/src/feature/power.cpp b/Marlin/src/feature/power.cpp
new file mode 100644
index 0000000..d22247b
--- /dev/null
+++ b/Marlin/src/feature/power.cpp
@@ -0,0 +1,137 @@
+/**
+ * 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/>.
+ *
+ */
+
+/**
+ * power.cpp - power control
+ */
+
+#include "../inc/MarlinConfig.h"
+
+#if ENABLED(AUTO_POWER_CONTROL)
+
+#include "power.h"
+#include "../module/temperature.h"
+#include "../module/stepper/indirection.h"
+#include "../MarlinCore.h"
+
+#if defined(PSU_POWERUP_GCODE) || defined(PSU_POWEROFF_GCODE)
+  #include "../gcode/gcode.h"
+#endif
+
+#if BOTH(USE_CONTROLLER_FAN, AUTO_POWER_CONTROLLERFAN)
+  #include "controllerfan.h"
+#endif
+
+Power powerManager;
+
+millis_t Power::lastPowerOn;
+
+bool Power::is_power_needed() {
+  #if ENABLED(AUTO_POWER_FANS)
+    FANS_LOOP(i) if (thermalManager.fan_speed[i]) return true;
+  #endif
+
+  #if ENABLED(AUTO_POWER_E_FANS)
+    HOTEND_LOOP() if (thermalManager.autofan_speed[e]) return true;
+  #endif
+
+  #if BOTH(USE_CONTROLLER_FAN, AUTO_POWER_CONTROLLERFAN)
+    if (controllerFan.state()) return true;
+  #endif
+
+  if (TERN0(AUTO_POWER_CHAMBER_FAN, thermalManager.chamberfan_speed))
+    return true;
+
+  // If any of the drivers or the bed are enabled...
+  if (X_ENABLE_READ() == X_ENABLE_ON || Y_ENABLE_READ() == Y_ENABLE_ON || Z_ENABLE_READ() == Z_ENABLE_ON
+    #if HAS_X2_ENABLE
+      || X2_ENABLE_READ() == X_ENABLE_ON
+    #endif
+    #if HAS_Y2_ENABLE
+      || Y2_ENABLE_READ() == Y_ENABLE_ON
+    #endif
+    #if HAS_Z2_ENABLE
+      || Z2_ENABLE_READ() == Z_ENABLE_ON
+    #endif
+    #if E_STEPPERS
+      #define _OR_ENABLED_E(N) || E##N##_ENABLE_READ() == E_ENABLE_ON
+      REPEAT(E_STEPPERS, _OR_ENABLED_E)
+    #endif
+  ) return true;
+
+  HOTEND_LOOP() if (thermalManager.degTargetHotend(e) > 0 || thermalManager.temp_hotend[e].soft_pwm_amount > 0) return true;
+  if (TERN0(HAS_HEATED_BED, thermalManager.degTargetBed() > 0 || thermalManager.temp_bed.soft_pwm_amount > 0)) return true;
+
+  #if HAS_HOTEND && AUTO_POWER_E_TEMP
+    HOTEND_LOOP() if (thermalManager.degHotend(e) >= AUTO_POWER_E_TEMP) return true;
+  #endif
+
+  #if HAS_HEATED_CHAMBER && AUTO_POWER_CHAMBER_TEMP
+    if (thermalManager.degChamber() >= AUTO_POWER_CHAMBER_TEMP) return true;
+  #endif
+
+  return false;
+}
+
+void Power::check() {
+  static millis_t nextPowerCheck = 0;
+  millis_t ms = millis();
+  if (ELAPSED(ms, nextPowerCheck)) {
+    nextPowerCheck = ms + 2500UL;
+    if (is_power_needed())
+      power_on();
+    else if (!lastPowerOn || ELAPSED(ms, lastPowerOn + SEC_TO_MS(POWER_TIMEOUT)))
+      power_off();
+  }
+}
+
+void Power::power_on() {
+  lastPowerOn = millis();
+  if (!powersupply_on) {
+    PSU_PIN_ON();
+    safe_delay(PSU_POWERUP_DELAY);
+    restore_stepper_drivers();
+    TERN_(HAS_TRINAMIC_CONFIG, safe_delay(PSU_POWERUP_DELAY));
+    #ifdef PSU_POWERUP_GCODE
+      GcodeSuite::process_subcommands_now_P(PSTR(PSU_POWERUP_GCODE));
+    #endif
+  }
+}
+
+void Power::power_off() {
+  if (powersupply_on) {
+    #ifdef PSU_POWEROFF_GCODE
+      GcodeSuite::process_subcommands_now_P(PSTR(PSU_POWEROFF_GCODE));
+    #endif
+    PSU_PIN_OFF();
+  }
+}
+
+void Power::power_off_soon() {
+  #if POWER_OFF_DELAY
+    lastPowerOn = millis() - SEC_TO_MS(POWER_TIMEOUT) + SEC_TO_MS(POWER_OFF_DELAY);
+  #else
+    power_off();
+  #endif
+}
+
+#endif // AUTO_POWER_CONTROL
diff --git a/Marlin/src/feature/power.h b/Marlin/src/feature/power.h
new file mode 100644
index 0000000..2462b92
--- /dev/null
+++ b/Marlin/src/feature/power.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
+
+/**
+ * power.h - power control
+ */
+
+#include "../core/millis_t.h"
+
+class Power {
+  public:
+    static void check();
+    static void power_on();
+    static void power_off();
+    static void power_off_soon();
+  private:
+    static millis_t lastPowerOn;
+    static bool is_power_needed();
+};
+
+extern Power powerManager;
diff --git a/Marlin/src/feature/power_monitor.cpp b/Marlin/src/feature/power_monitor.cpp
new file mode 100644
index 0000000..97c4a93
--- /dev/null
+++ b/Marlin/src/feature/power_monitor.cpp
@@ -0,0 +1,75 @@
+/**
+ * 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"
+
+#if HAS_POWER_MONITOR
+
+#include "power_monitor.h"
+
+#include "../lcd/marlinui.h"
+#include "../lcd/lcdprint.h"
+#include "../libs/numtostr.h"
+
+uint8_t PowerMonitor::flags; // = 0
+
+#if ENABLED(POWER_MONITOR_CURRENT)
+  pm_lpf_t<PowerMonitor::amps_adc_scale, PM_K_VALUE, PM_K_SCALE> PowerMonitor::amps;
+#endif
+#if ENABLED(POWER_MONITOR_VOLTAGE)
+  pm_lpf_t<PowerMonitor::volts_adc_scale, PM_K_VALUE, PM_K_SCALE> PowerMonitor::volts;
+#endif
+
+millis_t PowerMonitor::display_item_ms;
+uint8_t PowerMonitor::display_item;
+
+PowerMonitor power_monitor; // Single instance - this calls the constructor
+
+#if HAS_MARLINUI_U8GLIB
+
+  #if ENABLED(POWER_MONITOR_CURRENT)
+    void PowerMonitor::draw_current() {
+      const float amps = getAmps();
+      lcd_put_u8str(amps < 100 ? ftostr31ns(amps) : ui16tostr4rj((uint16_t)amps));
+      lcd_put_wchar('A');
+    }
+  #endif
+
+  #if HAS_POWER_MONITOR_VREF
+    void PowerMonitor::draw_voltage() {
+      const float volts = getVolts();
+      lcd_put_u8str(volts < 100 ? ftostr31ns(volts) : ui16tostr4rj((uint16_t)volts));
+      lcd_put_wchar('V');
+    }
+  #endif
+
+  #if HAS_POWER_MONITOR_WATTS
+    void PowerMonitor::draw_power() {
+      const float power = getPower();
+      lcd_put_u8str(power < 100 ? ftostr31ns(power) : ui16tostr4rj((uint16_t)power));
+      lcd_put_wchar('W');
+    }
+  #endif
+
+#endif // HAS_MARLINUI_U8GLIB
+
+#endif // HAS_POWER_MONITOR
diff --git a/Marlin/src/feature/power_monitor.h b/Marlin/src/feature/power_monitor.h
new file mode 100644
index 0000000..f378ee2
--- /dev/null
+++ b/Marlin/src/feature/power_monitor.h
@@ -0,0 +1,142 @@
+/**
+ * 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 PM_SAMPLE_RANGE 1024
+#define PM_K_VALUE      6
+#define PM_K_SCALE      6
+
+template <const float & SCALE, int K_VALUE, int K_SCALE>
+struct pm_lpf_t {
+  uint32_t filter_buf;
+  float value;
+  void add_sample(const uint16_t sample) {
+    filter_buf = filter_buf - (filter_buf >> K_VALUE) + (uint32_t(sample) << K_SCALE);
+  }
+  void capture() {
+    value = filter_buf * (SCALE * (1.0f / (1UL << (PM_K_VALUE + PM_K_SCALE)))) + (POWER_MONITOR_CURRENT_OFFSET);
+  }
+  void reset(uint16_t reset_value = 0) {
+    filter_buf = uint32_t(reset_value) << (K_VALUE + K_SCALE);
+    capture();
+  }
+};
+
+class PowerMonitor {
+private:
+  #if ENABLED(POWER_MONITOR_CURRENT)
+    static constexpr float amps_adc_scale = float(ADC_VREF) / (POWER_MONITOR_VOLTS_PER_AMP * PM_SAMPLE_RANGE);
+    static pm_lpf_t<amps_adc_scale, PM_K_VALUE, PM_K_SCALE> amps;
+  #endif
+  #if ENABLED(POWER_MONITOR_VOLTAGE)
+    static constexpr float volts_adc_scale = float(ADC_VREF) / (POWER_MONITOR_VOLTS_PER_VOLT * PM_SAMPLE_RANGE);
+    static pm_lpf_t<volts_adc_scale, PM_K_VALUE, PM_K_SCALE> volts;
+  #endif
+
+public:
+  static uint8_t flags;  // M430 flags to display current
+
+  static millis_t display_item_ms;
+  static uint8_t display_item;
+
+  PowerMonitor() { reset(); }
+
+  enum PM_Display_Bit : uint8_t {
+    PM_DISP_BIT_I, // Current display enable bit
+    PM_DISP_BIT_V, // Voltage display enable bit
+    PM_DISP_BIT_P  // Power display enable bit
+  };
+
+  #if ENABLED(POWER_MONITOR_CURRENT)
+    FORCE_INLINE static float getAmps() { return amps.value; }
+    void add_current_sample(const uint16_t value) { amps.add_sample(value); }
+  #endif
+
+  #if HAS_POWER_MONITOR_VREF
+    #if ENABLED(POWER_MONITOR_VOLTAGE)
+      FORCE_INLINE static float getVolts() { return volts.value; }
+    #else
+      FORCE_INLINE static float getVolts() { return POWER_MONITOR_FIXED_VOLTAGE; }  // using a specified fixed valtage as the voltage measurement
+    #endif
+    #if ENABLED(POWER_MONITOR_VOLTAGE)
+      void add_voltage_sample(const uint16_t value) { volts.add_sample(value); }
+    #endif
+  #endif
+
+  #if HAS_POWER_MONITOR_WATTS
+    FORCE_INLINE static float getPower() { return getAmps() * getVolts(); }
+  #endif
+
+  #if HAS_WIRED_LCD
+    #if HAS_MARLINUI_U8GLIB && DISABLED(LIGHTWEIGHT_UI)
+      FORCE_INLINE static bool display_enabled() { return flags != 0x00; }
+    #endif
+    #if ENABLED(POWER_MONITOR_CURRENT)
+      static void draw_current();
+      FORCE_INLINE static bool current_display_enabled() { return TEST(flags, PM_DISP_BIT_I); }
+      FORCE_INLINE static void set_current_display(const bool b) { SET_BIT_TO(flags, PM_DISP_BIT_I, b); }
+      FORCE_INLINE static void toggle_current_display() { TBI(flags, PM_DISP_BIT_I); }
+    #endif
+    #if HAS_POWER_MONITOR_VREF
+      static void draw_voltage();
+      FORCE_INLINE static bool voltage_display_enabled() { return TEST(flags, PM_DISP_BIT_V); }
+      FORCE_INLINE static void set_voltage_display(const bool b) { SET_BIT_TO(flags, PM_DISP_BIT_V, b); }
+      FORCE_INLINE static void toggle_voltage_display() { TBI(flags, PM_DISP_BIT_V); }
+    #endif
+    #if HAS_POWER_MONITOR_WATTS
+      static void draw_power();
+      FORCE_INLINE static bool power_display_enabled() { return TEST(flags, PM_DISP_BIT_P); }
+      FORCE_INLINE static void set_power_display(const bool b) { SET_BIT_TO(flags, PM_DISP_BIT_P, b); }
+      FORCE_INLINE static void toggle_power_display() { TBI(flags, PM_DISP_BIT_P); }
+    #endif
+  #endif
+
+  static void reset() {
+    flags = 0x00;
+
+    #if ENABLED(POWER_MONITOR_CURRENT)
+      amps.reset();
+    #endif
+
+    #if ENABLED(POWER_MONITOR_VOLTAGE)
+      volts.reset();
+    #endif
+
+    #if ENABLED(SDSUPPORT)
+      display_item_ms = 0;
+      display_item = 0;
+    #endif
+  }
+
+  static void capture_values() {
+    #if ENABLED(POWER_MONITOR_CURRENT)
+     amps.capture();
+    #endif
+    #if ENABLED(POWER_MONITOR_VOLTAGE)
+      volts.capture();
+    #endif
+  }
+};
+
+extern PowerMonitor power_monitor;
diff --git a/Marlin/src/feature/powerloss.cpp b/Marlin/src/feature/powerloss.cpp
new file mode 100644
index 0000000..be35ff8
--- /dev/null
+++ b/Marlin/src/feature/powerloss.cpp
@@ -0,0 +1,619 @@
+/**
+ * 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/>.
+ *
+ */
+
+/**
+ * feature/powerloss.cpp - Resume an SD print after power-loss
+ */
+
+#include "../inc/MarlinConfigPre.h"
+
+#if ENABLED(POWER_LOSS_RECOVERY)
+
+#include "powerloss.h"
+#include "../core/macros.h"
+
+bool PrintJobRecovery::enabled; // Initialized by settings.load()
+
+SdFile PrintJobRecovery::file;
+job_recovery_info_t PrintJobRecovery::info;
+const char PrintJobRecovery::filename[5] = "/PLR";
+uint8_t PrintJobRecovery::queue_index_r;
+uint32_t PrintJobRecovery::cmd_sdpos, // = 0
+         PrintJobRecovery::sdpos[BUFSIZE];
+
+#if ENABLED(DWIN_CREALITY_LCD)
+  bool PrintJobRecovery::dwin_flag; // = false
+#endif
+
+#include "../sd/cardreader.h"
+#include "../lcd/marlinui.h"
+#include "../gcode/queue.h"
+#include "../gcode/gcode.h"
+#include "../module/motion.h"
+#include "../module/planner.h"
+#include "../module/printcounter.h"
+#include "../module/temperature.h"
+#include "../core/serial.h"
+
+#if ENABLED(FWRETRACT)
+  #include "fwretract.h"
+#endif
+
+#define DEBUG_OUT ENABLED(DEBUG_POWER_LOSS_RECOVERY)
+#include "../core/debug_out.h"
+
+PrintJobRecovery recovery;
+
+#ifndef POWER_LOSS_PURGE_LEN
+  #define POWER_LOSS_PURGE_LEN 0
+#endif
+#ifndef POWER_LOSS_ZRAISE
+  #define POWER_LOSS_ZRAISE 2     // Move on loss with backup power, or on resume without it
+#endif
+
+#if DISABLED(BACKUP_POWER_SUPPLY)
+  #undef POWER_LOSS_RETRACT_LEN   // No retract at outage without backup power
+#endif
+#ifndef POWER_LOSS_RETRACT_LEN
+  #define POWER_LOSS_RETRACT_LEN 0
+#endif
+
+/**
+ * Clear the recovery info
+ */
+void PrintJobRecovery::init() { memset(&info, 0, sizeof(info)); }
+
+/**
+ * Enable or disable then call changed()
+ */
+void PrintJobRecovery::enable(const bool onoff) {
+  enabled = onoff;
+  changed();
+}
+
+/**
+ * The enabled state was changed:
+ *  - Enabled: Purge the job recovery file
+ *  - Disabled: Write the job recovery file
+ */
+void PrintJobRecovery::changed() {
+  if (!enabled)
+    purge();
+  else if (IS_SD_PRINTING())
+    save(true);
+}
+
+/**
+ * Check for Print Job Recovery during setup()
+ *
+ * If a saved state exists send 'M1000 S' to initiate job recovery.
+ */
+void PrintJobRecovery::check() {
+  //if (!card.isMounted()) card.mount();
+  if (card.isMounted()) {
+    load();
+    if (!valid()) return cancel();
+    queue.inject_P(PSTR("M1000S"));
+  }
+}
+
+/**
+ * Delete the recovery file and clear the recovery data
+ */
+void PrintJobRecovery::purge() {
+  init();
+  card.removeJobRecoveryFile();
+}
+
+/**
+ * Load the recovery data, if it exists
+ */
+void PrintJobRecovery::load() {
+  if (exists()) {
+    open(true);
+    (void)file.read(&info, sizeof(info));
+    close();
+  }
+  debug(PSTR("Load"));
+}
+
+/**
+ * Set info fields that won't change
+ */
+void PrintJobRecovery::prepare() {
+  card.getAbsFilename(info.sd_filename);  // SD filename
+  cmd_sdpos = 0;
+}
+
+/**
+ * Save the current machine state to the power-loss recovery file
+ */
+void PrintJobRecovery::save(const bool force/*=false*/, const float zraise/*=0*/) {
+
+  #if SAVE_INFO_INTERVAL_MS > 0
+    static millis_t next_save_ms; // = 0
+    millis_t ms = millis();
+  #endif
+
+  #ifndef POWER_LOSS_MIN_Z_CHANGE
+    #define POWER_LOSS_MIN_Z_CHANGE 0.05  // Vase-mode-friendly out of the box
+  #endif
+
+  // Did Z change since the last call?
+  if (force
+    #if DISABLED(SAVE_EACH_CMD_MODE)      // Always save state when enabled
+      #if SAVE_INFO_INTERVAL_MS > 0       // Save if interval is elapsed
+        || ELAPSED(ms, next_save_ms)
+      #endif
+      // Save if Z is above the last-saved position by some minimum height
+      || current_position.z > info.current_position.z + POWER_LOSS_MIN_Z_CHANGE
+    #endif
+  ) {
+
+    #if SAVE_INFO_INTERVAL_MS > 0
+      next_save_ms = ms + SAVE_INFO_INTERVAL_MS;
+    #endif
+
+    // Set Head and Foot to matching non-zero values
+    if (!++info.valid_head) ++info.valid_head; // non-zero in sequence
+    //if (!IS_SD_PRINTING()) info.valid_head = 0;
+    info.valid_foot = info.valid_head;
+
+    // Machine state
+    info.current_position = current_position;
+    info.feedrate = uint16_t(MMS_TO_MMM(feedrate_mm_s));
+    info.zraise = zraise;
+
+    TERN_(GCODE_REPEAT_MARKERS, info.stored_repeat = repeat);
+    TERN_(HAS_HOME_OFFSET, info.home_offset = home_offset);
+    TERN_(HAS_POSITION_SHIFT, info.position_shift = position_shift);
+
+    #if HAS_MULTI_EXTRUDER
+      info.active_extruder = active_extruder;
+    #endif
+
+    #if DISABLED(NO_VOLUMETRICS)
+      info.volumetric_enabled = parser.volumetric_enabled;
+      #if HAS_MULTI_EXTRUDER
+        for (int8_t e = 0; e < EXTRUDERS; e++) info.filament_size[e] = planner.filament_size[e];
+      #else
+        if (parser.volumetric_enabled) info.filament_size[0] = planner.filament_size[active_extruder];
+      #endif
+    #endif
+
+    #if EXTRUDERS
+      HOTEND_LOOP() info.target_temperature[e] = thermalManager.temp_hotend[e].target;
+    #endif
+
+    TERN_(HAS_HEATED_BED, info.target_temperature_bed = thermalManager.temp_bed.target);
+
+    #if HAS_FAN
+      COPY(info.fan_speed, thermalManager.fan_speed);
+    #endif
+
+    #if HAS_LEVELING
+      info.flag.leveling = planner.leveling_active;
+      info.fade = TERN0(ENABLE_LEVELING_FADE_HEIGHT, planner.z_fade_height);
+    #endif
+
+    TERN_(GRADIENT_MIX, memcpy(&info.gradient, &mixer.gradient, sizeof(info.gradient)));
+
+    #if ENABLED(FWRETRACT)
+      COPY(info.retract, fwretract.current_retract);
+      info.retract_hop = fwretract.current_hop;
+    #endif
+
+    // Elapsed print job time
+    info.print_job_elapsed = print_job_timer.duration();
+
+    // Relative axis modes
+    info.axis_relative = gcode.axis_relative;
+
+    // Misc. Marlin flags
+    info.flag.dryrun = !!(marlin_debug_flags & MARLIN_DEBUG_DRYRUN);
+    info.flag.allow_cold_extrusion = TERN0(PREVENT_COLD_EXTRUSION, thermalManager.allow_cold_extrude);
+
+    write();
+  }
+}
+
+#if PIN_EXISTS(POWER_LOSS)
+
+  #if ENABLED(BACKUP_POWER_SUPPLY)
+
+    void PrintJobRecovery::retract_and_lift(const float &zraise) {
+      #if POWER_LOSS_RETRACT_LEN || POWER_LOSS_ZRAISE
+
+        gcode.set_relative_mode(true);  // Use relative coordinates
+
+        #if POWER_LOSS_RETRACT_LEN
+          // Retract filament now
+          gcode.process_subcommands_now_P(PSTR("G1 F3000 E-" STRINGIFY(POWER_LOSS_RETRACT_LEN)));
+        #endif
+
+        #if POWER_LOSS_ZRAISE
+          // Raise the Z axis now
+          if (zraise) {
+            char cmd[20], str_1[16];
+            sprintf_P(cmd, PSTR("G0 Z%s"), dtostrf(zraise, 1, 3, str_1));
+            gcode.process_subcommands_now(cmd);
+          }
+        #else
+          UNUSED(zraise);
+        #endif
+
+        //gcode.axis_relative = info.axis_relative;
+        planner.synchronize();
+      #endif
+    }
+
+  #endif
+
+  /**
+   * An outage was detected by a sensor pin.
+   *  - If not SD printing, let the machine turn off on its own with no "KILL" screen
+   *  - Disable all heaters first to save energy
+   *  - Save the recovery data for the current instant
+   *  - If backup power is available Retract E and Raise Z
+   *  - Go to the KILL screen
+   */
+  void PrintJobRecovery::_outage() {
+    #if ENABLED(BACKUP_POWER_SUPPLY)
+      static bool lock = false;
+      if (lock) return; // No re-entrance from idle() during retract_and_lift()
+      lock = true;
+    #endif
+
+    #if POWER_LOSS_ZRAISE
+      // Get the limited Z-raise to do now or on resume
+      const float zraise = _MAX(0, _MIN(current_position.z + POWER_LOSS_ZRAISE, Z_MAX_POS - 1) - current_position.z);
+    #else
+      constexpr float zraise = 0;
+    #endif
+
+    // Save, including the limited Z raise
+    if (IS_SD_PRINTING()) save(true, zraise);
+
+    // Disable all heaters to reduce power loss
+    thermalManager.disable_all_heaters();
+
+    #if ENABLED(BACKUP_POWER_SUPPLY)
+      // Do a hard-stop of the steppers (with possibly a loud thud)
+      quickstop_stepper();
+      // With backup power a retract and raise can be done now
+      retract_and_lift(zraise);
+    #endif
+
+    kill(GET_TEXT(MSG_OUTAGE_RECOVERY));
+  }
+
+#endif
+
+/**
+ * Save the recovery info the recovery file
+ */
+void PrintJobRecovery::write() {
+
+  debug(PSTR("Write"));
+
+  open(false);
+  file.seekSet(0);
+  const int16_t ret = file.write(&info, sizeof(info));
+  if (ret == -1) DEBUG_ECHOLNPGM("Power-loss file write failed.");
+  if (!file.close()) DEBUG_ECHOLNPGM("Power-loss file close failed.");
+}
+
+/**
+ * Resume the saved print job
+ */
+void PrintJobRecovery::resume() {
+
+  char cmd[MAX_CMD_SIZE+16], str_1[16], str_2[16];
+
+  const uint32_t resume_sdpos = info.sdpos; // Get here before the stepper ISR overwrites it
+
+  // Apply the dry-run flag if enabled
+  if (info.flag.dryrun) marlin_debug_flags |= MARLIN_DEBUG_DRYRUN;
+
+  // Restore cold extrusion permission
+  TERN_(PREVENT_COLD_EXTRUSION, thermalManager.allow_cold_extrude = info.flag.allow_cold_extrusion);
+
+  #if HAS_LEVELING
+    // Make sure leveling is off before any G92 and G28
+    gcode.process_subcommands_now_P(PSTR("M420 S0 Z0"));
+  #endif
+
+  #if HAS_HEATED_BED
+    const int16_t bt = info.target_temperature_bed;
+    if (bt) {
+      // Restore the bed temperature
+      sprintf_P(cmd, PSTR("M190 S%i"), bt);
+      gcode.process_subcommands_now(cmd);
+    }
+  #endif
+
+  // Restore all hotend temperatures
+  #if HAS_HOTEND
+    HOTEND_LOOP() {
+      const int16_t et = info.target_temperature[e];
+      if (et) {
+        #if HAS_MULTI_HOTEND
+          sprintf_P(cmd, PSTR("T%i S"), e);
+          gcode.process_subcommands_now(cmd);
+        #endif
+        sprintf_P(cmd, PSTR("M109 S%i"), et);
+        gcode.process_subcommands_now(cmd);
+      }
+    }
+  #endif
+
+  // Reset E, raise Z, home XY...
+  #if Z_HOME_DIR > 0
+
+    // If Z homing goes to max, just reset E and home all
+    gcode.process_subcommands_now_P(PSTR(
+      "G92.9 E0\n"
+      "G28R0"
+    ));
+
+  #else // "G92.9 E0 ..."
+
+    // If a Z raise occurred at outage restore Z, otherwise raise Z now
+    sprintf_P(cmd, PSTR("G92.9 E0 " TERN(BACKUP_POWER_SUPPLY, "Z%s", "Z0\nG1Z%s")), dtostrf(info.zraise, 1, 3, str_1));
+    gcode.process_subcommands_now(cmd);
+
+    // Home safely with no Z raise
+    gcode.process_subcommands_now_P(PSTR(
+      "G28R0"                               // No raise during G28
+      #if IS_CARTESIAN && DISABLED(POWER_LOSS_RECOVER_ZHOME)
+        "XY"                                // Don't home Z on Cartesian unless overridden
+      #endif
+    ));
+
+  #endif
+
+  // Pretend that all axes are homed
+  set_all_homed();
+
+  #if ENABLED(POWER_LOSS_RECOVER_ZHOME)
+    // Z has been homed so restore Z to ZsavedPos + POWER_LOSS_ZRAISE
+    sprintf_P(cmd, PSTR("G1 F500 Z%s"), dtostrf(info.current_position.z + POWER_LOSS_ZRAISE, 1, 3, str_1));
+    gcode.process_subcommands_now(cmd);
+  #endif
+
+  // Recover volumetric extrusion state
+  #if DISABLED(NO_VOLUMETRICS)
+    #if HAS_MULTI_EXTRUDER
+      for (int8_t e = 0; e < EXTRUDERS; e++) {
+        sprintf_P(cmd, PSTR("M200 T%i D%s"), e, dtostrf(info.filament_size[e], 1, 3, str_1));
+        gcode.process_subcommands_now(cmd);
+      }
+      if (!info.volumetric_enabled) {
+        sprintf_P(cmd, PSTR("M200 T%i D0"), info.active_extruder);
+        gcode.process_subcommands_now(cmd);
+      }
+    #else
+      if (info.volumetric_enabled) {
+        sprintf_P(cmd, PSTR("M200 D%s"), dtostrf(info.filament_size[0], 1, 3, str_1));
+        gcode.process_subcommands_now(cmd);
+      }
+    #endif
+  #endif
+
+  // Select the previously active tool (with no_move)
+  #if HAS_MULTI_EXTRUDER
+    sprintf_P(cmd, PSTR("T%i S"), info.active_extruder);
+    gcode.process_subcommands_now(cmd);
+  #endif
+
+  // Restore print cooling fan speeds
+  #if HAS_FAN
+    FANS_LOOP(i) {
+      const int f = info.fan_speed[i];
+      if (f) {
+        sprintf_P(cmd, PSTR("M106 P%i S%i"), i, f);
+        gcode.process_subcommands_now(cmd);
+      }
+    }
+  #endif
+
+  // Restore retract and hop state
+  #if ENABLED(FWRETRACT)
+    LOOP_L_N(e, EXTRUDERS) {
+      if (info.retract[e] != 0.0) {
+        fwretract.current_retract[e] = info.retract[e];
+        fwretract.retracted[e] = true;
+      }
+    }
+    fwretract.current_hop = info.retract_hop;
+  #endif
+
+  #if HAS_LEVELING
+    // Restore leveling state before 'G92 Z' to ensure
+    // the Z stepper count corresponds to the native Z.
+    if (info.fade || info.flag.leveling) {
+      sprintf_P(cmd, PSTR("M420 S%i Z%s"), int(info.flag.leveling), dtostrf(info.fade, 1, 1, str_1));
+      gcode.process_subcommands_now(cmd);
+    }
+  #endif
+
+  #if ENABLED(GRADIENT_MIX)
+    memcpy(&mixer.gradient, &info.gradient, sizeof(info.gradient));
+  #endif
+
+  // Un-retract if there was a retract at outage
+  #if POWER_LOSS_RETRACT_LEN
+    gcode.process_subcommands_now_P(PSTR("G1 E" STRINGIFY(POWER_LOSS_RETRACT_LEN) " F3000"));
+  #endif
+
+  // Additional purge if configured
+  #if POWER_LOSS_PURGE_LEN
+    sprintf_P(cmd, PSTR("G1 E%d F200"), (POWER_LOSS_PURGE_LEN) + (POWER_LOSS_RETRACT_LEN));
+    gcode.process_subcommands_now(cmd);
+  #endif
+
+  #if ENABLED(NOZZLE_CLEAN_FEATURE)
+    gcode.process_subcommands_now_P(PSTR("G12"));
+  #endif
+
+  // Move back to the saved XY
+  sprintf_P(cmd, PSTR("G1 X%s Y%s F3000"),
+    dtostrf(info.current_position.x, 1, 3, str_1),
+    dtostrf(info.current_position.y, 1, 3, str_2)
+  );
+  gcode.process_subcommands_now(cmd);
+
+  // Move back to the saved Z
+  dtostrf(info.current_position.z, 1, 3, str_1);
+  #if Z_HOME_DIR > 0 || ENABLED(POWER_LOSS_RECOVER_ZHOME)
+    sprintf_P(cmd, PSTR("G1 Z%s F200"), str_1);
+  #else
+    gcode.process_subcommands_now_P(PSTR("G1 Z0 F200"));
+    sprintf_P(cmd, PSTR("G92.9 Z%s"), str_1);
+  #endif
+  gcode.process_subcommands_now(cmd);
+
+  // Restore the feedrate
+  sprintf_P(cmd, PSTR("G1 F%d"), info.feedrate);
+  gcode.process_subcommands_now(cmd);
+
+  // Restore E position with G92.9
+  sprintf_P(cmd, PSTR("G92.9 E%s"), dtostrf(info.current_position.e, 1, 3, str_1));
+  gcode.process_subcommands_now(cmd);
+
+  TERN_(GCODE_REPEAT_MARKERS, repeat = info.stored_repeat);
+  TERN_(HAS_HOME_OFFSET, home_offset = info.home_offset);
+  TERN_(HAS_POSITION_SHIFT, position_shift = info.position_shift);
+  #if HAS_HOME_OFFSET || HAS_POSITION_SHIFT
+    LOOP_XYZ(i) update_workspace_offset((AxisEnum)i);
+  #endif
+
+  // Relative axis modes
+  gcode.axis_relative = info.axis_relative;
+
+  #if ENABLED(DEBUG_POWER_LOSS_RECOVERY)
+    const uint8_t old_flags = marlin_debug_flags;
+    marlin_debug_flags |= MARLIN_DEBUG_ECHO;
+  #endif
+
+  // Continue to apply PLR when a file is resumed!
+  enable(true);
+
+  // Resume the SD file from the last position
+  char *fn = info.sd_filename;
+  sprintf_P(cmd, M23_STR, fn);
+  gcode.process_subcommands_now(cmd);
+  sprintf_P(cmd, PSTR("M24 S%ld T%ld"), resume_sdpos, info.print_job_elapsed);
+  gcode.process_subcommands_now(cmd);
+
+  TERN_(DEBUG_POWER_LOSS_RECOVERY, marlin_debug_flags = old_flags);
+}
+
+#if ENABLED(DEBUG_POWER_LOSS_RECOVERY)
+
+  void PrintJobRecovery::debug(PGM_P const prefix) {
+    DEBUG_PRINT_P(prefix);
+    DEBUG_ECHOLNPAIR(" Job Recovery Info...\nvalid_head:", int(info.valid_head), " valid_foot:", int(info.valid_foot));
+    if (info.valid_head) {
+      if (info.valid_head == info.valid_foot) {
+        DEBUG_ECHOPGM("current_position: ");
+        LOOP_XYZE(i) {
+          if (i) DEBUG_CHAR(',');
+          DEBUG_DECIMAL(info.current_position[i]);
+        }
+        DEBUG_EOL();
+
+        DEBUG_ECHOLNPAIR("zraise: ", info.zraise);
+
+        #if HAS_HOME_OFFSET
+          DEBUG_ECHOPGM("home_offset: ");
+          LOOP_XYZ(i) {
+            if (i) DEBUG_CHAR(',');
+            DEBUG_DECIMAL(info.home_offset[i]);
+          }
+          DEBUG_EOL();
+        #endif
+
+        #if HAS_POSITION_SHIFT
+          DEBUG_ECHOPGM("position_shift: ");
+          LOOP_XYZ(i) {
+            if (i) DEBUG_CHAR(',');
+            DEBUG_DECIMAL(info.position_shift[i]);
+          }
+          DEBUG_EOL();
+        #endif
+
+        DEBUG_ECHOLNPAIR("feedrate: ", info.feedrate);
+
+        #if HAS_MULTI_EXTRUDER
+          DEBUG_ECHOLNPAIR("active_extruder: ", int(info.active_extruder));
+        #endif
+
+        #if HAS_HOTEND
+          DEBUG_ECHOPGM("target_temperature: ");
+          HOTEND_LOOP() {
+            DEBUG_ECHO(info.target_temperature[e]);
+            if (e < HOTENDS - 1) DEBUG_CHAR(',');
+          }
+          DEBUG_EOL();
+        #endif
+
+        #if HAS_HEATED_BED
+          DEBUG_ECHOLNPAIR("target_temperature_bed: ", info.target_temperature_bed);
+        #endif
+
+        #if HAS_FAN
+          DEBUG_ECHOPGM("fan_speed: ");
+          FANS_LOOP(i) {
+            DEBUG_ECHO(int(info.fan_speed[i]));
+            if (i < FAN_COUNT - 1) DEBUG_CHAR(',');
+          }
+          DEBUG_EOL();
+        #endif
+
+        #if HAS_LEVELING
+          DEBUG_ECHOLNPAIR("leveling: ", int(info.flag.leveling), " fade: ", info.fade);
+        #endif
+        #if ENABLED(FWRETRACT)
+          DEBUG_ECHOPGM("retract: ");
+          for (int8_t e = 0; e < EXTRUDERS; e++) {
+            DEBUG_ECHO(info.retract[e]);
+            if (e < EXTRUDERS - 1) DEBUG_CHAR(',');
+          }
+          DEBUG_EOL();
+          DEBUG_ECHOLNPAIR("retract_hop: ", info.retract_hop);
+        #endif
+        DEBUG_ECHOLNPAIR("sd_filename: ", info.sd_filename);
+        DEBUG_ECHOLNPAIR("sdpos: ", info.sdpos);
+        DEBUG_ECHOLNPAIR("print_job_elapsed: ", info.print_job_elapsed);
+        DEBUG_ECHOLNPAIR("dryrun: ", int(info.flag.dryrun));
+        DEBUG_ECHOLNPAIR("allow_cold_extrusion: ", int(info.flag.allow_cold_extrusion));
+      }
+      else
+        DEBUG_ECHOLNPGM("INVALID DATA");
+    }
+    DEBUG_ECHOLNPGM("---");
+  }
+
+#endif // DEBUG_POWER_LOSS_RECOVERY
+
+#endif // POWER_LOSS_RECOVERY
diff --git a/Marlin/src/feature/powerloss.h b/Marlin/src/feature/powerloss.h
new file mode 100644
index 0000000..25581e1
--- /dev/null
+++ b/Marlin/src/feature/powerloss.h
@@ -0,0 +1,191 @@
+/**
+ * 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
+
+/**
+ * feature/powerloss.h - Resume an SD print after power-loss
+ */
+
+#include "../sd/cardreader.h"
+#include "../gcode/gcode.h"
+
+#include "../inc/MarlinConfig.h"
+
+#if ENABLED(GCODE_REPEAT_MARKERS)
+  #include "../feature/repeat.h"
+#endif
+
+#if ENABLED(MIXING_EXTRUDER)
+  #include "../feature/mixing.h"
+#endif
+
+#if !defined(POWER_LOSS_STATE) && PIN_EXISTS(POWER_LOSS)
+  #define POWER_LOSS_STATE HIGH
+#endif
+
+//#define DEBUG_POWER_LOSS_RECOVERY
+//#define SAVE_EACH_CMD_MODE
+//#define SAVE_INFO_INTERVAL_MS 0
+
+typedef struct {
+  uint8_t valid_head;
+
+  // Machine state
+  xyze_pos_t current_position;
+  uint16_t feedrate;
+  float zraise;
+
+  // Repeat information
+  TERN_(GCODE_REPEAT_MARKERS, Repeat stored_repeat);
+
+  TERN_(HAS_HOME_OFFSET,    xyz_pos_t home_offset);
+  TERN_(HAS_POSITION_SHIFT, xyz_pos_t position_shift);
+  TERN_(HAS_MULTI_EXTRUDER, uint8_t active_extruder);
+
+  #if DISABLED(NO_VOLUMETRICS)
+    bool volumetric_enabled;
+    float filament_size[EXTRUDERS];
+  #endif
+
+  TERN_(HAS_HOTEND,     int16_t target_temperature[HOTENDS]);
+  TERN_(HAS_HEATED_BED, int16_t target_temperature_bed);
+  TERN_(HAS_FAN,        uint8_t fan_speed[FAN_COUNT]);
+
+  TERN_(HAS_LEVELING, float fade);
+
+  #if ENABLED(FWRETRACT)
+    float retract[EXTRUDERS], retract_hop;
+  #endif
+
+  // Mixing extruder and gradient
+  #if ENABLED(MIXING_EXTRUDER)
+    //uint_fast8_t selected_vtool;
+    //mixer_comp_t color[NR_MIXING_VIRTUAL_TOOLS][MIXING_STEPPERS];
+    TERN_(GRADIENT_MIX, gradient_t gradient);
+  #endif
+
+  // SD Filename and position
+  char sd_filename[MAXPATHNAMELENGTH];
+  volatile uint32_t sdpos;
+
+  // Job elapsed time
+  millis_t print_job_elapsed;
+
+  // Relative axis modes
+  uint8_t axis_relative;
+
+  // Misc. Marlin flags
+  struct {
+    bool dryrun:1;                // M111 S8
+    bool allow_cold_extrusion:1;  // M302 P1
+    TERN_(HAS_LEVELING, bool leveling:1);
+  } flag;
+
+  uint8_t valid_foot;
+
+  bool valid() { return valid_head && valid_head == valid_foot; }
+
+} job_recovery_info_t;
+
+class PrintJobRecovery {
+  public:
+    static const char filename[5];
+
+    static SdFile file;
+    static job_recovery_info_t info;
+
+    static uint8_t queue_index_r;     //!< Queue index of the active command
+    static uint32_t cmd_sdpos,        //!< SD position of the next command
+                    sdpos[BUFSIZE];   //!< SD positions of queued commands
+
+    #if ENABLED(DWIN_CREALITY_LCD)
+      static bool dwin_flag;
+    #endif
+
+    static void init();
+    static void prepare();
+
+    static inline void setup() {
+      #if PIN_EXISTS(POWER_LOSS)
+        #if ENABLED(POWER_LOSS_PULLUP)
+          SET_INPUT_PULLUP(POWER_LOSS_PIN);
+        #elif ENABLED(POWER_LOSS_PULLDOWN)
+          SET_INPUT_PULLDOWN(POWER_LOSS_PIN);
+        #else
+          SET_INPUT(POWER_LOSS_PIN);
+        #endif
+      #endif
+    }
+
+    // Track each command's file offsets
+    static inline uint32_t command_sdpos() { return sdpos[queue_index_r]; }
+    static inline void commit_sdpos(const uint8_t index_w) { sdpos[index_w] = cmd_sdpos; }
+
+    static bool enabled;
+    static void enable(const bool onoff);
+    static void changed();
+
+    static inline bool exists() { return card.jobRecoverFileExists(); }
+    static inline void open(const bool read) { card.openJobRecoveryFile(read); }
+    static inline void close() { file.close(); }
+
+    static void check();
+    static void resume();
+    static void purge();
+
+    static inline void cancel() { purge(); IF_DISABLED(NO_SD_AUTOSTART, card.autofile_begin()); }
+
+    static void load();
+    static void save(const bool force=ENABLED(SAVE_EACH_CMD_MODE), const float zraise=0);
+
+    #if PIN_EXISTS(POWER_LOSS)
+      static inline void outage() {
+        if (enabled && READ(POWER_LOSS_PIN) == POWER_LOSS_STATE)
+          _outage();
+      }
+    #endif
+
+    // The referenced file exists
+    static inline bool interrupted_file_exists() { return card.fileExists(info.sd_filename); }
+
+    static inline bool valid() { return info.valid() && interrupted_file_exists(); }
+
+    #if ENABLED(DEBUG_POWER_LOSS_RECOVERY)
+      static void debug(PGM_P const prefix);
+    #else
+      static inline void debug(PGM_P const) {}
+    #endif
+
+  private:
+    static void write();
+
+    #if ENABLED(BACKUP_POWER_SUPPLY)
+      static void retract_and_lift(const float &zraise);
+    #endif
+
+    #if PIN_EXISTS(POWER_LOSS)
+      friend class GcodeSuite;
+      static void _outage();
+    #endif
+};
+
+extern PrintJobRecovery recovery;
diff --git a/Marlin/src/feature/probe_temp_comp.cpp b/Marlin/src/feature/probe_temp_comp.cpp
new file mode 100644
index 0000000..af8039d
--- /dev/null
+++ b/Marlin/src/feature/probe_temp_comp.cpp
@@ -0,0 +1,240 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../inc/MarlinConfigPre.h"
+
+#if ENABLED(PROBE_TEMP_COMPENSATION)
+
+#include "probe_temp_comp.h"
+#include <math.h>
+
+ProbeTempComp temp_comp;
+
+int16_t ProbeTempComp::z_offsets_probe[cali_info_init[TSI_PROBE].measurements],  // = {0}
+        ProbeTempComp::z_offsets_bed[cali_info_init[TSI_BED].measurements];      // = {0}
+
+#if ENABLED(USE_TEMP_EXT_COMPENSATION)
+  int16_t ProbeTempComp::z_offsets_ext[cali_info_init[TSI_EXT].measurements];    // = {0}
+#endif
+
+int16_t *ProbeTempComp::sensor_z_offsets[TSI_COUNT] = {
+  ProbeTempComp::z_offsets_probe, ProbeTempComp::z_offsets_bed
+  #if ENABLED(USE_TEMP_EXT_COMPENSATION)
+    , ProbeTempComp::z_offsets_ext
+  #endif
+};
+
+const temp_calib_t ProbeTempComp::cali_info[TSI_COUNT] = {
+  cali_info_init[TSI_PROBE], cali_info_init[TSI_BED]
+  #if ENABLED(USE_TEMP_EXT_COMPENSATION)
+    , cali_info_init[TSI_EXT]
+  #endif
+};
+
+constexpr xyz_pos_t ProbeTempComp::park_point;
+constexpr xy_pos_t ProbeTempComp::measure_point;
+constexpr int ProbeTempComp::probe_calib_bed_temp;
+
+uint8_t ProbeTempComp::calib_idx; // = 0
+float ProbeTempComp::init_measurement; // = 0.0
+
+void ProbeTempComp::clear_offsets(const TempSensorID tsi) {
+  LOOP_L_N(i, cali_info[tsi].measurements)
+    sensor_z_offsets[tsi][i] = 0;
+  calib_idx = 0;
+}
+
+bool ProbeTempComp::set_offset(const TempSensorID tsi, const uint8_t idx, const int16_t offset) {
+  if (idx >= cali_info[tsi].measurements) return false;
+  sensor_z_offsets[tsi][idx] = offset;
+  return true;
+}
+
+void ProbeTempComp::print_offsets() {
+  LOOP_L_N(s, TSI_COUNT) {
+    float temp = cali_info[s].start_temp;
+    for (int16_t i = -1; i < cali_info[s].measurements; ++i) {
+      serialprintPGM(s == TSI_BED ? PSTR("Bed") :
+        #if ENABLED(USE_TEMP_EXT_COMPENSATION)
+          s == TSI_EXT ? PSTR("Extruder") :
+        #endif
+        PSTR("Probe")
+      );
+      SERIAL_ECHOLNPAIR(
+        " temp: ", temp,
+        "C; Offset: ", i < 0 ? 0.0f : sensor_z_offsets[s][i], " um"
+      );
+      temp += cali_info[s].temp_res;
+    }
+  }
+}
+
+void ProbeTempComp::prepare_new_calibration(const float &init_meas_z) {
+  calib_idx = 0;
+  init_measurement = init_meas_z;
+}
+
+void ProbeTempComp::push_back_new_measurement(const TempSensorID tsi, const float &meas_z) {
+  switch (tsi) {
+    case TSI_PROBE:
+    case TSI_BED:
+    //case TSI_EXT:
+      if (calib_idx >= cali_info[tsi].measurements) return;
+      sensor_z_offsets[tsi][calib_idx++] = static_cast<int16_t>(meas_z * 1000.0f - init_measurement * 1000.0f);
+    default: break;
+  }
+}
+
+bool ProbeTempComp::finish_calibration(const TempSensorID tsi) {
+  if (tsi != TSI_PROBE && tsi != TSI_BED) return false;
+
+  if (calib_idx < 3) {
+    SERIAL_ECHOLNPGM("!Insufficient measurements (min. 3).");
+    clear_offsets(tsi);
+    return false;
+  }
+
+  const uint8_t measurements = cali_info[tsi].measurements;
+  const float start_temp = cali_info[tsi].start_temp,
+                res_temp = cali_info[tsi].temp_res;
+  int16_t * const data = sensor_z_offsets[tsi];
+
+  // Extrapolate
+  float k, d;
+  if (calib_idx < measurements) {
+    SERIAL_ECHOLNPAIR("Got ", calib_idx, " measurements. ");
+    if (linear_regression(tsi, k, d)) {
+      SERIAL_ECHOPGM("Applying linear extrapolation");
+      calib_idx--;
+      for (; calib_idx < measurements; ++calib_idx) {
+        const float temp = start_temp + float(calib_idx) * res_temp;
+        data[calib_idx] = static_cast<int16_t>(k * temp + d);
+      }
+    }
+    else {
+      // Simply use the last measured value for higher temperatures
+      SERIAL_ECHOPGM("Failed to extrapolate");
+      const int16_t last_val = data[calib_idx];
+      for (; calib_idx < measurements; ++calib_idx)
+        data[calib_idx] = last_val;
+    }
+    SERIAL_ECHOLNPGM(" for higher temperatures.");
+  }
+
+  // Sanity check
+  for (calib_idx = 0; calib_idx < measurements; ++calib_idx) {
+    // Restrict the max. offset
+    if (abs(data[calib_idx]) > 2000) {
+      SERIAL_ECHOLNPGM("!Invalid Z-offset detected (0-2).");
+      clear_offsets(tsi);
+      return false;
+    }
+    // Restrict the max. offset difference between two probings
+    if (calib_idx > 0 && abs(data[calib_idx - 1] - data[calib_idx]) > 800) {
+      SERIAL_ECHOLNPGM("!Invalid Z-offset between two probings detected (0-0.8).");
+      clear_offsets(TSI_PROBE);
+      return false;
+    }
+  }
+
+  return true;
+}
+
+void ProbeTempComp::compensate_measurement(const TempSensorID tsi, const float &temp, float &meas_z) {
+  if (WITHIN(temp, cali_info[tsi].start_temp, cali_info[tsi].end_temp))
+    meas_z -= get_offset_for_temperature(tsi, temp);
+}
+
+float ProbeTempComp::get_offset_for_temperature(const TempSensorID tsi, const float &temp) {
+  const uint8_t measurements = cali_info[tsi].measurements;
+  const float start_temp = cali_info[tsi].start_temp,
+                res_temp = cali_info[tsi].temp_res;
+  const int16_t * const data = sensor_z_offsets[tsi];
+
+  auto point = [&](uint8_t i) {
+    return xy_float_t({start_temp + i*res_temp, static_cast<float>(data[i])});
+  };
+
+  auto linear_interp = [](float x, xy_float_t p1, xy_float_t p2) {
+    return (p2.y - p1.y) / (p2.x - p2.y) * (x - p1.x) + p1.y;
+  };
+
+  // Linear interpolation
+  uint8_t idx = static_cast<uint8_t>((temp - start_temp) / res_temp);
+
+  // offset in um
+  float offset = 0.0f;
+
+  #if !defined(PTC_LINEAR_EXTRAPOLATION) || PTC_LINEAR_EXTRAPOLATION <= 0
+    if (idx < 0)
+      offset = 0.0f;
+    else if (idx > measurements - 2)
+      offset = static_cast<float>(data[measurements - 1]);
+  #else
+    if (idx < 0)
+      offset = linear_interp(temp, point(0), point(PTC_LINEAR_EXTRAPOLATION));
+    else if (idx > measurements - 2)
+      offset = linear_interp(temp, point(measurements - PTC_LINEAR_EXTRAPOLATION - 1), point(measurements - 1));
+  #endif
+    else
+      offset = linear_interp(temp, point(idx), point(idx + 1));
+
+  // return offset in mm
+  return offset / 1000.0f;
+}
+
+bool ProbeTempComp::linear_regression(const TempSensorID tsi, float &k, float &d) {
+  if (tsi != TSI_PROBE && tsi != TSI_BED) return false;
+
+  if (!WITHIN(calib_idx, 2, cali_info[tsi].measurements)) return false;
+
+  const float start_temp = cali_info[tsi].start_temp,
+                res_temp = cali_info[tsi].temp_res;
+  const int16_t * const data = sensor_z_offsets[tsi];
+
+  float sum_x = start_temp,
+        sum_x2 = sq(start_temp),
+        sum_xy = 0, sum_y = 0;
+
+  LOOP_L_N(i, calib_idx) {
+    const float xi = start_temp + (i + 1) * res_temp,
+                yi = static_cast<float>(data[i]);
+    sum_x += xi;
+    sum_x2 += sq(xi);
+    sum_xy += xi * yi;
+    sum_y += yi;
+  }
+
+  const float denom = static_cast<float>(calib_idx + 1) * sum_x2 - sq(sum_x);
+  if (fabs(denom) <= 10e-5) {
+    // Singularity - unable to solve
+    k = d = 0.0;
+    return false;
+  }
+
+  k = (static_cast<float>(calib_idx + 1) * sum_xy - sum_x * sum_y) / denom;
+  d = (sum_y - k * sum_x) / static_cast<float>(calib_idx + 1);
+
+  return true;
+}
+
+#endif // PROBE_TEMP_COMPENSATION
diff --git a/Marlin/src/feature/probe_temp_comp.h b/Marlin/src/feature/probe_temp_comp.h
new file mode 100644
index 0000000..626dd87
--- /dev/null
+++ b/Marlin/src/feature/probe_temp_comp.h
@@ -0,0 +1,147 @@
+/**
+ * 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"
+
+enum TempSensorID : uint8_t {
+  TSI_PROBE,
+  TSI_BED,
+  #if ENABLED(USE_TEMP_EXT_COMPENSATION)
+    TSI_EXT,
+  #endif
+  TSI_COUNT
+};
+
+typedef struct {
+  uint8_t measurements; // Max. number of measurements to be stored (35 - 80°C)
+  float   temp_res,     // Resolution in °C between measurements
+          start_temp,   // Base measurement; z-offset == 0
+          end_temp;
+} temp_calib_t;
+
+/**
+ * Probe temperature compensation implementation.
+ * Z-probes like the P.I.N.D.A V2 allow for compensation of
+ * measurement errors/shifts due to changed temperature.
+ */
+
+// Probe temperature calibration constants
+#ifndef PTC_SAMPLE_COUNT
+  #define PTC_SAMPLE_COUNT 10U
+#endif
+#ifndef PTC_SAMPLE_RES
+  #define PTC_SAMPLE_RES 5.0f
+#endif
+#ifndef PTC_SAMPLE_START
+  #define PTC_SAMPLE_START 30.0f
+#endif
+#define PTC_SAMPLE_END ((PTC_SAMPLE_START) + (PTC_SAMPLE_COUNT) * (PTC_SAMPLE_RES))
+
+// Bed temperature calibration constants
+#ifndef BTC_PROBE_TEMP
+  #define BTC_PROBE_TEMP 30.0f
+#endif
+#ifndef BTC_SAMPLE_COUNT
+  #define BTC_SAMPLE_COUNT 10U
+#endif
+#ifndef BTC_SAMPLE_STEP
+  #define BTC_SAMPLE_RES 5.0f
+#endif
+#ifndef BTC_SAMPLE_START
+  #define BTC_SAMPLE_START 60.0f
+#endif
+#define BTC_SAMPLE_END ((BTC_SAMPLE_START) + (BTC_SAMPLE_COUNT) * (BTC_SAMPLE_RES))
+
+#ifndef PTC_PROBE_HEATING_OFFSET
+  #define PTC_PROBE_HEATING_OFFSET 0.5f
+#endif
+
+#ifndef PTC_PROBE_RAISE
+  #define PTC_PROBE_RAISE 10.0f
+#endif
+
+static constexpr temp_calib_t cali_info_init[TSI_COUNT] = {
+    {  PTC_SAMPLE_COUNT, PTC_SAMPLE_RES, PTC_SAMPLE_START, PTC_SAMPLE_END },       // Probe
+    {  BTC_SAMPLE_COUNT, BTC_SAMPLE_RES, BTC_SAMPLE_START, BTC_SAMPLE_END },       // Bed
+  #if ENABLED(USE_TEMP_EXT_COMPENSATION)
+    {  20,  5, 180, 180 +  5 * 20 }        // Extruder
+  #endif
+};
+
+class ProbeTempComp {
+  public:
+
+    static const temp_calib_t cali_info[TSI_COUNT];
+
+    // Where to park nozzle to wait for probe cooldown
+    static constexpr xyz_pos_t park_point = PTC_PARK_POS;
+
+    // XY coordinates of nozzle for probing the bed
+    static constexpr xy_pos_t measure_point    = PTC_PROBE_POS;     // Coordinates to probe
+                            //measure_point    = { 12.0f, 7.3f };   // Coordinates for the MK52 magnetic heatbed
+
+    static constexpr int  probe_calib_bed_temp = BED_MAX_TARGET,  // Bed temperature while calibrating probe
+                          bed_calib_probe_temp = BTC_PROBE_TEMP;  // Probe temperature while calibrating bed
+
+    static int16_t *sensor_z_offsets[TSI_COUNT],
+                   z_offsets_probe[cali_info_init[TSI_PROBE].measurements], // (µm)
+                   z_offsets_bed[cali_info_init[TSI_BED].measurements];     // (µm)
+
+    #if ENABLED(USE_TEMP_EXT_COMPENSATION)
+      static int16_t z_offsets_ext[cali_info_init[TSI_EXT].measurements];   // (µm)
+    #endif
+
+    static inline void reset_index() { calib_idx = 0; };
+    static inline uint8_t get_index() { return calib_idx; }
+    static void clear_offsets(const TempSensorID tsi);
+    static inline void clear_all_offsets() {
+      clear_offsets(TSI_BED);
+      clear_offsets(TSI_PROBE);
+      TERN_(USE_TEMP_EXT_COMPENSATION, clear_offsets(TSI_EXT));
+    }
+    static bool set_offset(const TempSensorID tsi, const uint8_t idx, const int16_t offset);
+    static void print_offsets();
+    static void prepare_new_calibration(const float &init_meas_z);
+    static void push_back_new_measurement(const TempSensorID tsi, const float &meas_z);
+    static bool finish_calibration(const TempSensorID tsi);
+    static void compensate_measurement(const TempSensorID tsi, const float &temp, float &meas_z);
+
+  private:
+    static uint8_t calib_idx;
+
+    /**
+     * Base value. Temperature compensation values will be deltas
+     * to this value, set at first probe.
+     */
+    static float init_measurement;
+
+    static float get_offset_for_temperature(const TempSensorID tsi, const float &temp);
+
+    /**
+     * Fit a linear function in measured temperature offsets
+     * to allow generating values of higher temperatures.
+     */
+    static bool linear_regression(const TempSensorID tsi, float &k, float &d);
+};
+
+extern ProbeTempComp temp_comp;
diff --git a/Marlin/src/feature/repeat.cpp b/Marlin/src/feature/repeat.cpp
new file mode 100644
index 0000000..d48157a
--- /dev/null
+++ b/Marlin/src/feature/repeat.cpp
@@ -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/>.
+ *
+ */
+#include "../inc/MarlinConfig.h"
+
+#if ENABLED(GCODE_REPEAT_MARKERS)
+
+//#define DEBUG_GCODE_REPEAT_MARKERS
+
+#include "repeat.h"
+
+#include "../gcode/gcode.h"
+#include "../sd/cardreader.h"
+
+#define DEBUG_OUT ENABLED(DEBUG_GCODE_REPEAT_MARKERS)
+#include "../core/debug_out.h"
+
+repeat_marker_t Repeat::marker[MAX_REPEAT_NESTING];
+uint8_t Repeat::index;
+
+void Repeat::add_marker(const uint32_t sdpos, const uint16_t count) {
+  if (index >= MAX_REPEAT_NESTING)
+    SERIAL_ECHO_MSG("!Too many markers.");
+  else {
+    marker[index].sdpos = sdpos;
+    marker[index].counter = count ?: -1;
+    index++;
+    DEBUG_ECHOLNPAIR("Add Marker ", int(index), " at ", sdpos, " (", count, ")");
+  }
+}
+
+void Repeat::loop() {
+  if (!index)                           // No marker?
+    SERIAL_ECHO_MSG("!No marker set."); //  Inform the user.
+  else {
+    const uint8_t ind = index - 1;      // Active marker's index
+    if (!marker[ind].counter) {         // Did its counter run out?
+      DEBUG_ECHOLNPAIR("Pass Marker ", int(index));
+      index--;                          //  Carry on. Previous marker on the next 'M808'.
+    }
+    else {
+      card.setIndex(marker[ind].sdpos); // Loop back to the marker.
+      if (marker[ind].counter > 0)      // Ignore a negative (or zero) counter.
+        --marker[ind].counter;          // Decrement the counter. If zero this 'M808' will be skipped next time.
+      DEBUG_ECHOLNPAIR("Goto Marker ", int(index), " at ", marker[ind].sdpos, " (", marker[ind].counter, ")");
+    }
+  }
+}
+
+void Repeat::cancel() { LOOP_L_N(i, index) marker[i].counter = 0; }
+
+void Repeat::early_parse_M808(char * const cmd) {
+  if (is_command_M808(cmd)) {
+    DEBUG_ECHOLNPAIR("Parsing \"", cmd, "\"");
+    parser.parse(cmd);
+    if (parser.seen('L'))
+      add_marker(card.getIndex(), parser.value_ushort());
+    else
+      Repeat::loop();
+  }
+}
+
+#endif // GCODE_REPEAT_MARKERS
diff --git a/Marlin/src/feature/repeat.h b/Marlin/src/feature/repeat.h
new file mode 100644
index 0000000..0f4d942
--- /dev/null
+++ b/Marlin/src/feature/repeat.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
+
+#include "../inc/MarlinConfigPre.h"
+#include "../gcode/parser.h"
+
+#include <stdint.h>
+
+#define MAX_REPEAT_NESTING 10
+
+typedef struct {
+  uint32_t sdpos;   // The repeat file position
+  int16_t counter;  // The counter for looping
+} repeat_marker_t;
+
+class Repeat {
+private:
+  static repeat_marker_t marker[MAX_REPEAT_NESTING];
+  static uint8_t index;
+public:
+  static inline void reset() { index = 0; }
+  static inline bool is_active() {
+    LOOP_L_N(i, index) if (marker[i].counter) return true;
+    return false;
+  }
+  static bool is_command_M808(char * const cmd) { return cmd[0] == 'M' && cmd[1] == '8' && cmd[2] == '0' && cmd[3] == '8' && !NUMERIC(cmd[4]); }
+  static void early_parse_M808(char * const cmd);
+  static void add_marker(const uint32_t sdpos, const uint16_t count);
+  static void loop();
+  static void cancel();
+};
+
+extern Repeat repeat;
diff --git a/Marlin/src/feature/runout.cpp b/Marlin/src/feature/runout.cpp
new file mode 100644
index 0000000..be769d2
--- /dev/null
+++ b/Marlin/src/feature/runout.cpp
@@ -0,0 +1,135 @@
+/**
+ * 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/>.
+ *
+ */
+
+/**
+ * feature/runout.cpp - Runout sensor support
+ */
+
+#include "../inc/MarlinConfigPre.h"
+
+#if HAS_FILAMENT_SENSOR
+
+#include "runout.h"
+
+FilamentMonitor runout;
+
+bool FilamentMonitorBase::enabled = true,
+     FilamentMonitorBase::filament_ran_out;  // = false
+
+#if ENABLED(HOST_ACTION_COMMANDS)
+  bool FilamentMonitorBase::host_handling; // = false
+#endif
+
+#if ENABLED(TOOLCHANGE_MIGRATION_FEATURE)
+  #include "../module/tool_change.h"
+  #define DEBUG_OUT ENABLED(DEBUG_TOOLCHANGE_MIGRATION_FEATURE)
+  #include "../core/debug_out.h"
+#endif
+
+#if HAS_FILAMENT_RUNOUT_DISTANCE
+  float RunoutResponseDelayed::runout_distance_mm = FILAMENT_RUNOUT_DISTANCE_MM;
+  volatile float RunoutResponseDelayed::runout_mm_countdown[EXTRUDERS];
+  #if ENABLED(FILAMENT_MOTION_SENSOR)
+    uint8_t FilamentSensorEncoder::motion_detected;
+  #endif
+#else
+  int8_t RunoutResponseDebounced::runout_count; // = 0
+#endif
+
+//
+// Filament Runout event handler
+//
+#include "../MarlinCore.h"
+#include "../feature/pause.h"
+#include "../gcode/queue.h"
+
+#if ENABLED(HOST_ACTION_COMMANDS)
+  #include "host_actions.h"
+#endif
+
+#if ENABLED(EXTENSIBLE_UI)
+  #include "../lcd/extui/ui_api.h"
+#endif
+
+void event_filament_runout() {
+
+  if (did_pause_print) return;  // Action already in progress. Purge triggered repeated runout.
+
+  #if ENABLED(TOOLCHANGE_MIGRATION_FEATURE)
+    if (migration.in_progress) {
+      DEBUG_ECHOLNPGM("Migration Already In Progress");
+      return;  // Action already in progress. Purge triggered repeated runout.
+    }
+    if (migration.automode) {
+      DEBUG_ECHOLNPGM("Migration Starting");
+      if (extruder_migration()) return;
+    }
+  #endif
+
+  TERN_(EXTENSIBLE_UI, ExtUI::onFilamentRunout(ExtUI::getActiveTool()));
+
+  #if EITHER(HOST_PROMPT_SUPPORT, HOST_ACTION_COMMANDS)
+    const char tool = '0'
+      #if NUM_RUNOUT_SENSORS > 1
+        + active_extruder
+      #endif
+    ;
+  #endif
+
+  //action:out_of_filament
+  #if ENABLED(HOST_PROMPT_SUPPORT)
+    host_action_prompt_begin(PROMPT_FILAMENT_RUNOUT, PSTR("FilamentRunout T"), tool);
+    host_action_prompt_show();
+  #endif
+
+  const bool run_runout_script = !runout.host_handling;
+
+  #if ENABLED(HOST_ACTION_COMMANDS)
+    if (run_runout_script
+      && ( strstr(FILAMENT_RUNOUT_SCRIPT, "M600")
+        || strstr(FILAMENT_RUNOUT_SCRIPT, "M125")
+        || TERN0(ADVANCED_PAUSE_FEATURE, strstr(FILAMENT_RUNOUT_SCRIPT, "M25"))
+      )
+    ) {
+      host_action_paused(false);
+    }
+    else {
+      // Legacy Repetier command for use until newer version supports standard dialog
+      // To be removed later when pause command also triggers dialog
+      #ifdef ACTION_ON_FILAMENT_RUNOUT
+        host_action(PSTR(ACTION_ON_FILAMENT_RUNOUT " T"), false);
+        SERIAL_CHAR(tool);
+        SERIAL_EOL();
+      #endif
+
+      host_action_pause(false);
+    }
+    SERIAL_ECHOPGM(" " ACTION_REASON_ON_FILAMENT_RUNOUT " ");
+    SERIAL_CHAR(tool);
+    SERIAL_EOL();
+  #endif // HOST_ACTION_COMMANDS
+
+  if (run_runout_script)
+    queue.inject_P(PSTR(FILAMENT_RUNOUT_SCRIPT));
+}
+
+#endif // HAS_FILAMENT_SENSOR
diff --git a/Marlin/src/feature/runout.h b/Marlin/src/feature/runout.h
new file mode 100644
index 0000000..60154c5
--- /dev/null
+++ b/Marlin/src/feature/runout.h
@@ -0,0 +1,367 @@
+/**
+ * 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
+
+/**
+ * feature/runout.h - Runout sensor support
+ */
+
+#include "../sd/cardreader.h"
+#include "../module/printcounter.h"
+#include "../module/planner.h"
+#include "../module/stepper.h" // for block_t
+#include "../gcode/queue.h"
+#include "../feature/pause.h"
+
+#include "../inc/MarlinConfig.h"
+
+#if ENABLED(EXTENSIBLE_UI)
+  #include "../lcd/extui/ui_api.h"
+#endif
+
+//#define FILAMENT_RUNOUT_SENSOR_DEBUG
+#ifndef FILAMENT_RUNOUT_THRESHOLD
+  #define FILAMENT_RUNOUT_THRESHOLD 5
+#endif
+
+void event_filament_runout();
+
+template<class RESPONSE_T, class SENSOR_T>
+class TFilamentMonitor;
+class FilamentSensorEncoder;
+class FilamentSensorSwitch;
+class RunoutResponseDelayed;
+class RunoutResponseDebounced;
+
+/********************************* TEMPLATE SPECIALIZATION *********************************/
+
+typedef TFilamentMonitor<
+          TERN(HAS_FILAMENT_RUNOUT_DISTANCE, RunoutResponseDelayed, RunoutResponseDebounced),
+          TERN(FILAMENT_MOTION_SENSOR, FilamentSensorEncoder, FilamentSensorSwitch)
+        > FilamentMonitor;
+
+extern FilamentMonitor runout;
+
+/*******************************************************************************************/
+
+class FilamentMonitorBase {
+  public:
+    static bool enabled, filament_ran_out;
+
+    #if ENABLED(HOST_ACTION_COMMANDS)
+      static bool host_handling;
+    #else
+      static constexpr bool host_handling = false;
+    #endif
+};
+
+template<class RESPONSE_T, class SENSOR_T>
+class TFilamentMonitor : public FilamentMonitorBase {
+  private:
+    typedef RESPONSE_T response_t;
+    typedef SENSOR_T   sensor_t;
+    static  response_t response;
+    static  sensor_t   sensor;
+
+  public:
+    static inline void setup() {
+      sensor.setup();
+      reset();
+    }
+
+    static inline void reset() {
+      filament_ran_out = false;
+      response.reset();
+    }
+
+    // Call this method when filament is present,
+    // so the response can reset its counter.
+    static inline void filament_present(const uint8_t extruder) {
+      response.filament_present(extruder);
+    }
+
+    #if HAS_FILAMENT_RUNOUT_DISTANCE
+      static inline float& runout_distance() { return response.runout_distance_mm; }
+      static inline void set_runout_distance(const float &mm) { response.runout_distance_mm = mm; }
+    #endif
+
+    // Handle a block completion. RunoutResponseDelayed uses this to
+    // add up the length of filament moved while the filament is out.
+    static inline void block_completed(const block_t* const b) {
+      if (enabled) {
+        response.block_completed(b);
+        sensor.block_completed(b);
+      }
+    }
+
+    // Give the response a chance to update its counter.
+    static inline void run() {
+      if (enabled && !filament_ran_out && (printingIsActive() || did_pause_print)) {
+        TERN_(HAS_FILAMENT_RUNOUT_DISTANCE, cli()); // Prevent RunoutResponseDelayed::block_completed from accumulating here
+        response.run();
+        sensor.run();
+        const bool ran_out = response.has_run_out();
+        TERN_(HAS_FILAMENT_RUNOUT_DISTANCE, sei());
+        if (ran_out) {
+          filament_ran_out = true;
+          event_filament_runout();
+          planner.synchronize();
+        }
+      }
+    }
+};
+
+/*************************** FILAMENT PRESENCE SENSORS ***************************/
+
+class FilamentSensorBase {
+  protected:
+    /**
+     * Called by FilamentSensorSwitch::run when filament is detected.
+     * Called by FilamentSensorEncoder::block_completed when motion is detected.
+     */
+    static inline void filament_present(const uint8_t extruder) {
+      runout.filament_present(extruder); // ...which calls response.filament_present(extruder)
+    }
+
+  public:
+    static inline void setup() {
+      #define _INIT_RUNOUT_PIN(P,S,U,D) do{ if (ENABLED(U)) SET_INPUT_PULLUP(P); else if (ENABLED(D)) SET_INPUT_PULLDOWN(P); else SET_INPUT(P); }while(0)
+      #define  INIT_RUNOUT_PIN(N) _INIT_RUNOUT_PIN(FIL_RUNOUT##N##_PIN, FIL_RUNOUT##N##_STATE, FIL_RUNOUT##N##_PULLUP, FIL_RUNOUT##N##_PULLDOWN)
+      #if NUM_RUNOUT_SENSORS >= 1
+        INIT_RUNOUT_PIN(1);
+      #endif
+      #if NUM_RUNOUT_SENSORS >= 2
+        INIT_RUNOUT_PIN(2);
+      #endif
+      #if NUM_RUNOUT_SENSORS >= 3
+        INIT_RUNOUT_PIN(3);
+      #endif
+      #if NUM_RUNOUT_SENSORS >= 4
+        INIT_RUNOUT_PIN(4);
+      #endif
+      #if NUM_RUNOUT_SENSORS >= 5
+        INIT_RUNOUT_PIN(5);
+      #endif
+      #if NUM_RUNOUT_SENSORS >= 6
+        INIT_RUNOUT_PIN(6);
+      #endif
+      #if NUM_RUNOUT_SENSORS >= 7
+        INIT_RUNOUT_PIN(7);
+      #endif
+      #if NUM_RUNOUT_SENSORS >= 8
+        INIT_RUNOUT_PIN(8);
+      #endif
+      #undef _INIT_RUNOUT_PIN
+      #undef  INIT_RUNOUT_PIN
+    }
+
+    // Return a bitmask of runout pin states
+    static inline uint8_t poll_runout_pins() {
+      #define _OR_RUNOUT(N) | (READ(FIL_RUNOUT##N##_PIN) ? _BV((N) - 1) : 0)
+      return (0 REPEAT_S(1, INCREMENT(NUM_RUNOUT_SENSORS), _OR_RUNOUT));
+      #undef _OR_RUNOUT
+    }
+
+    // Return a bitmask of runout flag states (1 bits always indicates runout)
+    static inline uint8_t poll_runout_states() {
+      return poll_runout_pins() ^ uint8_t(0
+        #if NUM_RUNOUT_SENSORS >= 1
+          | (FIL_RUNOUT1_STATE ? 0 : _BV(1 - 1))
+        #endif
+        #if NUM_RUNOUT_SENSORS >= 2
+          | (FIL_RUNOUT2_STATE ? 0 : _BV(2 - 1))
+        #endif
+        #if NUM_RUNOUT_SENSORS >= 3
+          | (FIL_RUNOUT3_STATE ? 0 : _BV(3 - 1))
+        #endif
+        #if NUM_RUNOUT_SENSORS >= 4
+          | (FIL_RUNOUT4_STATE ? 0 : _BV(4 - 1))
+        #endif
+        #if NUM_RUNOUT_SENSORS >= 5
+          | (FIL_RUNOUT5_STATE ? 0 : _BV(5 - 1))
+        #endif
+        #if NUM_RUNOUT_SENSORS >= 6
+          | (FIL_RUNOUT6_STATE ? 0 : _BV(6 - 1))
+        #endif
+        #if NUM_RUNOUT_SENSORS >= 7
+          | (FIL_RUNOUT7_STATE ? 0 : _BV(7 - 1))
+        #endif
+        #if NUM_RUNOUT_SENSORS >= 8
+          | (FIL_RUNOUT8_STATE ? 0 : _BV(8 - 1))
+        #endif
+      );
+    }
+};
+
+#if ENABLED(FILAMENT_MOTION_SENSOR)
+
+  /**
+   * This sensor uses a magnetic encoder disc and a Hall effect
+   * sensor (or a slotted disc and optical sensor). The state
+   * will toggle between 0 and 1 on filament movement. It can detect
+   * filament runout and stripouts or jams.
+   */
+  class FilamentSensorEncoder : public FilamentSensorBase {
+    private:
+      static uint8_t motion_detected;
+
+      static inline void poll_motion_sensor() {
+        static uint8_t old_state;
+        const uint8_t new_state = poll_runout_pins(),
+                      change    = old_state ^ new_state;
+        old_state = new_state;
+
+        #ifdef FILAMENT_RUNOUT_SENSOR_DEBUG
+          if (change) {
+            SERIAL_ECHOPGM("Motion detected:");
+            LOOP_L_N(e, NUM_RUNOUT_SENSORS)
+              if (TEST(change, e)) SERIAL_CHAR(' ', '0' + e);
+            SERIAL_EOL();
+          }
+        #endif
+
+        motion_detected |= change;
+      }
+
+    public:
+      static inline void block_completed(const block_t* const b) {
+        // If the sensor wheel has moved since the last call to
+        // this method reset the runout counter for the extruder.
+        if (TEST(motion_detected, b->extruder))
+          filament_present(b->extruder);
+
+        // Clear motion triggers for next block
+        motion_detected = 0;
+      }
+
+      static inline void run() { poll_motion_sensor(); }
+  };
+
+#else
+
+  /**
+   * This is a simple endstop switch in the path of the filament.
+   * It can detect filament runout, but not stripouts or jams.
+   */
+  class FilamentSensorSwitch : public FilamentSensorBase {
+    private:
+      static inline bool poll_runout_state(const uint8_t extruder) {
+        const uint8_t runout_states = poll_runout_states();
+        #if NUM_RUNOUT_SENSORS == 1
+          UNUSED(extruder);
+        #else
+          if ( !TERN0(DUAL_X_CARRIAGE, idex_is_duplicating())
+            && !TERN0(MULTI_NOZZLE_DUPLICATION, extruder_duplication_enabled)
+          ) return TEST(runout_states, extruder); // A specific extruder ran out
+        #endif
+        return !!runout_states;                   // Any extruder ran out
+      }
+
+    public:
+      static inline void block_completed(const block_t* const) {}
+
+      static inline void run() {
+        const bool out = poll_runout_state(active_extruder);
+        if (!out) filament_present(active_extruder);
+        #ifdef FILAMENT_RUNOUT_SENSOR_DEBUG
+          static bool was_out = false;
+          if (out != was_out) {
+            was_out = out;
+            SERIAL_ECHOPGM("Filament ");
+            serialprintPGM(out ? PSTR("OUT\n") : PSTR("IN\n"));
+          }
+        #endif
+      }
+  };
+
+
+#endif // !FILAMENT_MOTION_SENSOR
+
+/********************************* RESPONSE TYPE *********************************/
+
+#if HAS_FILAMENT_RUNOUT_DISTANCE
+
+  // RunoutResponseDelayed triggers a runout event only if the length
+  // of filament specified by FILAMENT_RUNOUT_DISTANCE_MM has been fed
+  // during a runout condition.
+  class RunoutResponseDelayed {
+    private:
+      static volatile float runout_mm_countdown[EXTRUDERS];
+
+    public:
+      static float runout_distance_mm;
+
+      static inline void reset() {
+        LOOP_L_N(i, EXTRUDERS) filament_present(i);
+      }
+
+      static inline void run() {
+        #ifdef FILAMENT_RUNOUT_SENSOR_DEBUG
+          static millis_t t = 0;
+          const millis_t ms = millis();
+          if (ELAPSED(ms, t)) {
+            t = millis() + 1000UL;
+            LOOP_L_N(i, EXTRUDERS) {
+              serialprintPGM(i ? PSTR(", ") : PSTR("Remaining mm: "));
+              SERIAL_ECHO(runout_mm_countdown[i]);
+            }
+            SERIAL_EOL();
+          }
+        #endif
+      }
+
+      static inline bool has_run_out() {
+        return runout_mm_countdown[active_extruder] < 0;
+      }
+
+      static inline void filament_present(const uint8_t extruder) {
+        runout_mm_countdown[extruder] = runout_distance_mm;
+      }
+
+      static inline void block_completed(const block_t* const b) {
+        if (b->steps.x || b->steps.y || b->steps.z || did_pause_print) { // Allow pause purge move to re-trigger runout state
+          // Only trigger on extrusion with XYZ movement to allow filament change and retract/recover.
+          const uint8_t e = b->extruder;
+          const int32_t steps = b->steps.e;
+          runout_mm_countdown[e] -= (TEST(b->direction_bits, E_AXIS) ? -steps : steps) * planner.steps_to_mm[E_AXIS_N(e)];
+        }
+      }
+  };
+
+#else // !HAS_FILAMENT_RUNOUT_DISTANCE
+
+  // RunoutResponseDebounced triggers a runout event after a runout
+  // condition has been detected runout_threshold times in a row.
+
+  class RunoutResponseDebounced {
+    private:
+      static constexpr int8_t runout_threshold = FILAMENT_RUNOUT_THRESHOLD;
+      static int8_t runout_count;
+    public:
+      static inline void reset()                                  { runout_count = runout_threshold; }
+      static inline void run()                                    { if (runout_count >= 0) runout_count--; }
+      static inline bool has_run_out()                            { return runout_count < 0; }
+      static inline void block_completed(const block_t* const)    { }
+      static inline void filament_present(const uint8_t)          { runout_count = runout_threshold; }
+  };
+
+#endif // !HAS_FILAMENT_RUNOUT_DISTANCE
diff --git a/Marlin/src/feature/solenoid.cpp b/Marlin/src/feature/solenoid.cpp
new file mode 100644
index 0000000..623f223
--- /dev/null
+++ b/Marlin/src/feature/solenoid.cpp
@@ -0,0 +1,91 @@
+/**
+ * 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 EITHER(EXT_SOLENOID, MANUAL_SOLENOID_CONTROL)
+
+#include "solenoid.h"
+
+#include "../module/motion.h" // for active_extruder
+
+// PARKING_EXTRUDER options alter the default behavior of solenoids, this ensures compliance of M380-381
+
+#if ENABLED(PARKING_EXTRUDER)
+  #include "../module/tool_change.h"
+#endif
+
+#define HAS_SOLENOID(N) (HAS_SOLENOID_##N && (ENABLED(MANUAL_SOLENOID_CONTROL) || N < EXTRUDERS))
+
+// Used primarily with MANUAL_SOLENOID_CONTROL
+static void set_solenoid(const uint8_t num, const bool active) {
+  const uint8_t value = active ? PE_MAGNET_ON_STATE : !PE_MAGNET_ON_STATE;
+  switch (num) {
+    case 0: OUT_WRITE(SOL0_PIN, value); break;
+    #if HAS_SOLENOID(1)
+      case 1: OUT_WRITE(SOL1_PIN, value); break;
+    #endif
+    #if HAS_SOLENOID(2)
+      case 2: OUT_WRITE(SOL2_PIN, value); break;
+    #endif
+    #if HAS_SOLENOID(3)
+      case 3: OUT_WRITE(SOL3_PIN, value); break;
+    #endif
+    #if HAS_SOLENOID(4)
+      case 4: OUT_WRITE(SOL4_PIN, value); break;
+    #endif
+    #if HAS_SOLENOID(5)
+      case 5: OUT_WRITE(SOL5_PIN, value); break;
+    #endif
+    default: SERIAL_ECHO_MSG(STR_INVALID_SOLENOID); break;
+  }
+
+  #if ENABLED(PARKING_EXTRUDER)
+    if (!active && active_extruder == num) // If active extruder's solenoid is disabled, carriage is considered parked
+      parking_extruder_set_parked(true);
+  #endif
+}
+
+void enable_solenoid(const uint8_t num) { set_solenoid(num, true); }
+void disable_solenoid(const uint8_t num) { set_solenoid(num, false); }
+void enable_solenoid_on_active_extruder() { enable_solenoid(active_extruder); }
+
+void disable_all_solenoids() {
+  disable_solenoid(0);
+  #if HAS_SOLENOID(1)
+    disable_solenoid(1);
+  #endif
+  #if HAS_SOLENOID(2)
+    disable_solenoid(2);
+  #endif
+  #if HAS_SOLENOID(3)
+    disable_solenoid(3);
+  #endif
+  #if HAS_SOLENOID(4)
+    disable_solenoid(4);
+  #endif
+  #if HAS_SOLENOID(5)
+    disable_solenoid(5);
+  #endif
+}
+
+#endif // EXT_SOLENOID || MANUAL_SOLENOID_CONTROL
diff --git a/Marlin/src/feature/solenoid.h b/Marlin/src/feature/solenoid.h
new file mode 100644
index 0000000..2ba4983
--- /dev/null
+++ b/Marlin/src/feature/solenoid.h
@@ -0,0 +1,27 @@
+/**
+ * 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
+
+void enable_solenoid_on_active_extruder();
+void disable_all_solenoids();
+void enable_solenoid(const uint8_t num);
+void disable_solenoid(const uint8_t num);
diff --git a/Marlin/src/feature/spindle_laser.cpp b/Marlin/src/feature/spindle_laser.cpp
new file mode 100644
index 0000000..66c04a0
--- /dev/null
+++ b/Marlin/src/feature/spindle_laser.cpp
@@ -0,0 +1,138 @@
+/**
+ * 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/>.
+ *
+ */
+
+/**
+ * feature/spindle_laser.cpp
+ */
+
+#include "../inc/MarlinConfig.h"
+
+#if HAS_CUTTER
+
+#include "spindle_laser.h"
+
+#if ENABLED(SPINDLE_SERVO)
+  #include "../module/servo.h"
+#endif
+
+SpindleLaser cutter;
+uint8_t SpindleLaser::power;
+#if ENABLED(LASER_FEATURE)
+  cutter_test_pulse_t SpindleLaser::testPulse = 50;                   // Test fire Pulse time ms value.
+#endif
+bool SpindleLaser::isReady;                                           // Ready to apply power setting from the UI to OCR
+cutter_power_t SpindleLaser::menuPower,                               // Power set via LCD menu in PWM, PERCENT, or RPM
+               SpindleLaser::unitPower;                               // LCD status power in PWM, PERCENT, or RPM
+
+#if ENABLED(MARLIN_DEV_MODE)
+  cutter_frequency_t SpindleLaser::frequency;                         // PWM frequency setting; range: 2K - 50K
+#endif
+#define SPINDLE_LASER_PWM_OFF TERN(SPINDLE_LASER_PWM_INVERT, 255, 0)
+
+//
+// Init the cutter to a safe OFF state
+//
+void SpindleLaser::init() {
+  #if ENABLED(SPINDLE_SERVO)
+    MOVE_SERVO(SPINDLE_SERVO_NR, SPINDLE_SERVO_MIN);
+  #else
+    OUT_WRITE(SPINDLE_LASER_ENA_PIN, !SPINDLE_LASER_ACTIVE_STATE);    // Init spindle to off
+  #endif
+  #if ENABLED(SPINDLE_CHANGE_DIR)
+    OUT_WRITE(SPINDLE_DIR_PIN, SPINDLE_INVERT_DIR ? 255 : 0);         // Init rotation to clockwise (M3)
+  #endif
+  #if ENABLED(SPINDLE_LASER_PWM)
+    SET_PWM(SPINDLE_LASER_PWM_PIN);
+    analogWrite(pin_t(SPINDLE_LASER_PWM_PIN), SPINDLE_LASER_PWM_OFF); // Set to lowest speed
+  #endif
+  #if ENABLED(HAL_CAN_SET_PWM_FREQ) && defined(SPINDLE_LASER_FREQUENCY)
+    set_pwm_frequency(pin_t(SPINDLE_LASER_PWM_PIN), SPINDLE_LASER_FREQUENCY);
+    TERN_(MARLIN_DEV_MODE, frequency = SPINDLE_LASER_FREQUENCY);
+  #endif
+}
+
+#if ENABLED(SPINDLE_LASER_PWM)
+  /**
+   * Set the cutter PWM directly to the given ocr value
+   */
+  void SpindleLaser::_set_ocr(const uint8_t ocr) {
+    #if NEEDS_HARDWARE_PWM && SPINDLE_LASER_FREQUENCY
+      set_pwm_frequency(pin_t(SPINDLE_LASER_PWM_PIN), TERN(MARLIN_DEV_MODE, frequency, SPINDLE_LASER_FREQUENCY));
+      set_pwm_duty(pin_t(SPINDLE_LASER_PWM_PIN), ocr ^ SPINDLE_LASER_PWM_OFF);
+    #else
+      analogWrite(pin_t(SPINDLE_LASER_PWM_PIN), ocr ^ SPINDLE_LASER_PWM_OFF);
+    #endif
+  }
+
+  void SpindleLaser::set_ocr(const uint8_t ocr) {
+    WRITE(SPINDLE_LASER_ENA_PIN,  SPINDLE_LASER_ACTIVE_STATE); // Cutter ON
+    _set_ocr(ocr);
+  }
+
+  void SpindleLaser::ocr_off() {
+    WRITE(SPINDLE_LASER_ENA_PIN, !SPINDLE_LASER_ACTIVE_STATE); // Cutter OFF
+    _set_ocr(0);
+  }
+#endif
+
+//
+// Set cutter ON/OFF state (and PWM) to the given cutter power value
+//
+void SpindleLaser::apply_power(const uint8_t opwr) {
+  static uint8_t last_power_applied = 0;
+  if (opwr == last_power_applied) return;
+  last_power_applied = opwr;
+  power = opwr;
+  #if ENABLED(SPINDLE_LASER_PWM)
+    if (cutter.unitPower == 0 && CUTTER_UNIT_IS(RPM)) {
+      ocr_off();
+      isReady = false;
+    }
+    else if (ENABLED(CUTTER_POWER_RELATIVE) || enabled()) {
+      set_ocr(power);
+      isReady = true;
+    }
+    else {
+      ocr_off();
+      isReady = false;
+    }
+  #elif ENABLED(SPINDLE_SERVO)
+    MOVE_SERVO(SPINDLE_SERVO_NR, power);
+  #else
+    WRITE(SPINDLE_LASER_ENA_PIN, enabled() ? SPINDLE_LASER_ACTIVE_STATE : !SPINDLE_LASER_ACTIVE_STATE);
+    isReady = true;
+  #endif
+}
+
+#if ENABLED(SPINDLE_CHANGE_DIR)
+  //
+  // Set the spindle direction and apply immediately
+  // Stop on direction change if SPINDLE_STOP_ON_DIR_CHANGE is enabled
+  //
+  void SpindleLaser::set_reverse(const bool reverse) {
+    const bool dir_state = (reverse == SPINDLE_INVERT_DIR); // Forward (M3) HIGH when not inverted
+    if (TERN0(SPINDLE_STOP_ON_DIR_CHANGE, enabled()) && READ(SPINDLE_DIR_PIN) != dir_state) disable();
+    WRITE(SPINDLE_DIR_PIN, dir_state);
+  }
+#endif
+
+#endif // HAS_CUTTER
diff --git a/Marlin/src/feature/spindle_laser.h b/Marlin/src/feature/spindle_laser.h
new file mode 100644
index 0000000..d50bc7e
--- /dev/null
+++ b/Marlin/src/feature/spindle_laser.h
@@ -0,0 +1,319 @@
+/**
+ * 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
+
+/**
+ * feature/spindle_laser.h
+ * Support for Laser Power or Spindle Power & Direction
+ */
+
+#include "../inc/MarlinConfig.h"
+
+#include "spindle_laser_types.h"
+
+#if USE_BEEPER
+  #include "../libs/buzzer.h"
+#endif
+
+#if ENABLED(LASER_POWER_INLINE)
+  #include "../module/planner.h"
+#endif
+
+#define PCT_TO_PWM(X) ((X) * 255 / 100)
+#define PCT_TO_SERVO(X) ((X) * 180 / 100)
+
+#ifndef SPEED_POWER_INTERCEPT
+  #define SPEED_POWER_INTERCEPT 0
+#endif
+
+// #define _MAP(N,S1,S2,D1,D2) ((N)*_MAX((D2)-(D1),0)/_MAX((S2)-(S1),1)+(D1))
+
+class SpindleLaser {
+public:
+  static constexpr float
+    min_pct = TERN(CUTTER_POWER_RELATIVE, 0, TERN(SPINDLE_FEATURE, round(100.0f * (SPEED_POWER_MIN) / (SPEED_POWER_MAX)), SPEED_POWER_MIN)),
+    max_pct = TERN(SPINDLE_FEATURE, 100, SPEED_POWER_MAX);
+
+  static const inline uint8_t pct_to_ocr(const float pct) { return uint8_t(PCT_TO_PWM(pct)); }
+
+  // cpower = configured values (e.g., SPEED_POWER_MAX)
+
+  // Convert configured power range to a percentage
+  static const inline uint8_t cpwr_to_pct(const cutter_cpower_t cpwr) {
+    constexpr cutter_cpower_t power_floor = TERN(CUTTER_POWER_RELATIVE, SPEED_POWER_MIN, 0),
+                              power_range = SPEED_POWER_MAX - power_floor;
+    return cpwr ? round(100.0f * (cpwr - power_floor) / power_range) : 0;
+  }
+
+  // Convert a cpower (e.g., SPEED_POWER_STARTUP) to unit power (upwr, upower),
+  // which can be PWM, Percent, Servo angle, or RPM (rel/abs).
+  static const inline cutter_power_t cpwr_to_upwr(const cutter_cpower_t cpwr) { // STARTUP power to Unit power
+    const cutter_power_t upwr = (
+      #if ENABLED(SPINDLE_FEATURE)
+        // Spindle configured values are in RPM
+        #if CUTTER_UNIT_IS(RPM)
+          cpwr                            // to RPM
+        #elif CUTTER_UNIT_IS(PERCENT)     // to PCT
+          cpwr_to_pct(cpwr)
+        #elif CUTTER_UNIT_IS(SERVO)       // to SERVO angle
+          PCT_TO_SERVO(cpwr_to_pct(cpwr))
+        #else                             // to PWM
+          PCT_TO_PWM(cpwr_to_pct(cpwr))
+        #endif
+      #else
+        // Laser configured values are in PCT
+        #if CUTTER_UNIT_IS(PWM255)
+          PCT_TO_PWM(cpwr)
+        #else
+          cpwr                            // to RPM/PCT
+        #endif
+      #endif
+    );
+    return upwr;
+  }
+
+  static const cutter_power_t mpower_min() { return cpwr_to_upwr(SPEED_POWER_MIN); }
+  static const cutter_power_t mpower_max() { return cpwr_to_upwr(SPEED_POWER_MAX); }
+
+  #if ENABLED(LASER_FEATURE)
+    static cutter_test_pulse_t testPulse; // Test fire Pulse ms value
+  #endif
+
+  static bool isReady;                    // Ready to apply power setting from the UI to OCR
+  static uint8_t power;
+
+  #if ENABLED(MARLIN_DEV_MODE)
+    static cutter_frequency_t frequency;  // Set PWM frequency; range: 2K-50K
+  #endif
+
+  static cutter_power_t menuPower,        // Power as set via LCD menu in PWM, Percentage or RPM
+                        unitPower;        // Power as displayed status in PWM, Percentage or RPM
+
+  static void init();
+
+  #if ENABLED(MARLIN_DEV_MODE)
+    static inline void refresh_frequency() { set_pwm_frequency(pin_t(SPINDLE_LASER_PWM_PIN), frequency); }
+  #endif
+
+  // Modifying this function should update everywhere
+  static inline bool enabled(const cutter_power_t opwr) { return opwr > 0; }
+  static inline bool enabled() { return enabled(power); }
+
+  static void apply_power(const uint8_t inpow);
+
+  FORCE_INLINE static void refresh() { apply_power(power); }
+  FORCE_INLINE static void set_power(const uint8_t upwr) { power = upwr; refresh(); }
+
+  #if ENABLED(SPINDLE_LASER_PWM)
+
+    private:
+
+    static void _set_ocr(const uint8_t ocr);
+
+    public:
+
+    static void set_ocr(const uint8_t ocr);
+    static inline void set_ocr_power(const uint8_t ocr) { power = ocr; set_ocr(ocr); }
+    static void ocr_off();
+    // Used to update output for power->OCR translation
+    static inline uint8_t upower_to_ocr(const cutter_power_t upwr) {
+      return (
+        #if CUTTER_UNIT_IS(PWM255)
+          uint8_t(upwr)
+        #elif CUTTER_UNIT_IS(PERCENT)
+          pct_to_ocr(upwr)
+        #else
+          uint8_t(pct_to_ocr(cpwr_to_pct(upwr)))
+        #endif
+      );
+    }
+
+    // Correct power to configured range
+    static inline cutter_power_t power_to_range(const cutter_power_t pwr) {
+      return power_to_range(pwr, (
+        #if CUTTER_UNIT_IS(PWM255)
+          0
+        #elif CUTTER_UNIT_IS(PERCENT)
+          1
+        #elif CUTTER_UNIT_IS(RPM)
+          2
+        #else
+          #error "CUTTER_UNIT_IS(unknown)"
+        #endif
+      ));
+    }
+    static inline cutter_power_t power_to_range(const cutter_power_t pwr, const uint8_t pwrUnit) {
+      if (pwr <= 0) return 0;
+      cutter_power_t upwr;
+      switch (pwrUnit) {
+        case 0:                                                 // PWM
+          upwr = cutter_power_t(
+              (pwr < pct_to_ocr(min_pct)) ? pct_to_ocr(min_pct) // Use minimum if set below
+            : (pwr > pct_to_ocr(max_pct)) ? pct_to_ocr(max_pct) // Use maximum if set above
+            :  pwr
+          );
+          break;
+        case 1:                                                 // PERCENT
+          upwr = cutter_power_t(
+              (pwr < min_pct) ? min_pct                         // Use minimum if set below
+            : (pwr > max_pct) ? max_pct                         // Use maximum if set above
+            :  pwr                                              // PCT
+          );
+          break;
+        case 2:                                                 // RPM
+          upwr = cutter_power_t(
+              (pwr < SPEED_POWER_MIN) ? SPEED_POWER_MIN         // Use minimum if set below
+            : (pwr > SPEED_POWER_MAX) ? SPEED_POWER_MAX         // Use maximum if set above
+            : pwr                                               // Calculate OCR value
+          );
+          break;
+        default: break;
+      }
+      return upwr;
+    }
+
+  #endif // SPINDLE_LASER_PWM
+
+  static inline void set_enabled(const bool enable) {
+    set_power(enable ? TERN(SPINDLE_LASER_PWM, (power ?: (unitPower ? upower_to_ocr(cpwr_to_upwr(SPEED_POWER_STARTUP)) : 0)), 255) : 0);
+  }
+
+  // Wait for spindle to spin up or spin down
+  static inline void power_delay(const bool on) {
+    #if DISABLED(LASER_POWER_INLINE)
+      safe_delay(on ? SPINDLE_LASER_POWERUP_DELAY : SPINDLE_LASER_POWERDOWN_DELAY);
+    #endif
+  }
+
+  #if ENABLED(SPINDLE_CHANGE_DIR)
+    static void set_reverse(const bool reverse);
+    static bool is_reverse() { return READ(SPINDLE_DIR_PIN) == SPINDLE_INVERT_DIR; }
+  #else
+    static inline void set_reverse(const bool) {}
+    static bool is_reverse() { return false; }
+  #endif
+
+  static inline void disable() { isReady = false; set_enabled(false); }
+
+  #if HAS_LCD_MENU
+
+    static inline void enable_with_dir(const bool reverse) {
+      isReady = true;
+      const uint8_t ocr = TERN(SPINDLE_LASER_PWM, upower_to_ocr(menuPower), 255);
+      if (menuPower)
+        power = ocr;
+      else
+        menuPower = cpwr_to_upwr(SPEED_POWER_STARTUP);
+      unitPower = menuPower;
+      set_reverse(reverse);
+      set_enabled(true);
+    }
+    FORCE_INLINE static void enable_forward() { enable_with_dir(false); }
+    FORCE_INLINE static void enable_reverse() { enable_with_dir(true); }
+    FORCE_INLINE static void enable_same_dir() { enable_with_dir(is_reverse()); }
+
+    #if ENABLED(SPINDLE_LASER_PWM)
+      static inline void update_from_mpower() {
+        if (isReady) power = upower_to_ocr(menuPower);
+        unitPower = menuPower;
+      }
+    #endif
+
+    #if ENABLED(LASER_FEATURE)
+      /**
+       * Test fire the laser using the testPulse ms duration
+       * Also fires with any PWM power that was previous set
+       * If not set defaults to 80% power
+       */
+      static inline void test_fire_pulse() {
+        enable_forward();                  // Turn Laser on (Spindle speak but same funct)
+        TERN_(USE_BEEPER, buzzer.tone(30, 3000));
+        delay(testPulse);                  // Delay for time set by user in pulse ms menu screen.
+        disable();                         // Turn laser off
+      }
+    #endif
+
+  #endif // HAS_LCD_MENU
+
+  #if ENABLED(LASER_POWER_INLINE)
+    /**
+     * Inline power adds extra fields to the planner block
+     * to handle laser power and scale to movement speed.
+     */
+
+    // Force disengage planner power control
+    static inline void inline_disable() {
+      isReady = false;
+      unitPower = 0;
+      planner.laser_inline.status.isPlanned = false;
+      planner.laser_inline.status.isEnabled = false;
+      planner.laser_inline.power = 0;
+    }
+
+    // Inline modes of all other functions; all enable planner inline power control
+    static inline void set_inline_enabled(const bool enable) {
+      if (enable)
+        inline_power(255);
+      else {
+        isReady = false;
+        unitPower = menuPower = 0;
+        planner.laser_inline.status.isPlanned = false;
+        TERN(SPINDLE_LASER_PWM, inline_ocr_power, inline_power)(0);
+      }
+    }
+
+    // Set the power for subsequent movement blocks
+    static void inline_power(const cutter_power_t upwr) {
+      unitPower = menuPower = upwr;
+      #if ENABLED(SPINDLE_LASER_PWM)
+        #if ENABLED(SPEED_POWER_RELATIVE) && !CUTTER_UNIT_IS(RPM) // relative mode does not turn laser off at 0, except for RPM
+          planner.laser_inline.status.isEnabled = true;
+          planner.laser_inline.power = upower_to_ocr(upwr);
+          isReady = true;
+        #else
+          inline_ocr_power(upower_to_ocr(upwr));
+        #endif
+      #else
+        planner.laser_inline.status.isEnabled = enabled(upwr);
+        planner.laser_inline.power = upwr;
+        isReady = enabled(upwr);
+      #endif
+    }
+
+    static inline void inline_direction(const bool) { /* never */ }
+
+    #if ENABLED(SPINDLE_LASER_PWM)
+      static inline void inline_ocr_power(const uint8_t ocrpwr) {
+        isReady = ocrpwr > 0;
+        planner.laser_inline.status.isEnabled = ocrpwr > 0;
+        planner.laser_inline.power = ocrpwr;
+      }
+    #endif
+  #endif  // LASER_POWER_INLINE
+
+  static inline void kill() {
+    TERN_(LASER_POWER_INLINE, inline_disable());
+    disable();
+  }
+};
+
+extern SpindleLaser cutter;
diff --git a/Marlin/src/feature/spindle_laser_types.h b/Marlin/src/feature/spindle_laser_types.h
new file mode 100644
index 0000000..0075e54
--- /dev/null
+++ b/Marlin/src/feature/spindle_laser_types.h
@@ -0,0 +1,63 @@
+/**
+ * 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
+
+/**
+ * feature/spindle_laser_types.h
+ * Support for Laser Power or Spindle Power & Direction
+ */
+
+#include "../inc/MarlinConfigPre.h"
+
+#if ENABLED(SPINDLE_FEATURE)
+  #define _MSG_CUTTER(M) MSG_SPINDLE_##M
+#else
+  #define _MSG_CUTTER(M) MSG_LASER_##M
+#endif
+#define MSG_CUTTER(M) _MSG_CUTTER(M)
+
+typedef IF<(SPEED_POWER_MAX > 255), uint16_t, uint8_t>::type cutter_cpower_t;
+
+#if CUTTER_UNIT_IS(RPM) && SPEED_POWER_MAX > 255
+  typedef uint16_t cutter_power_t;
+  #define CUTTER_MENU_POWER_TYPE   uint16_5
+  #define cutter_power2str         ui16tostr5rj
+#else
+  typedef uint8_t cutter_power_t;
+  #if CUTTER_UNIT_IS(PERCENT)
+    #define CUTTER_MENU_POWER_TYPE percent_3
+    #define cutter_power2str       pcttostrpctrj
+  #else
+    #define CUTTER_MENU_POWER_TYPE uint8
+    #define cutter_power2str       ui8tostr3rj
+  #endif
+#endif
+
+#if ENABLED(LASER_FEATURE)
+  typedef uint16_t cutter_test_pulse_t;
+  #define CUTTER_MENU_PULSE_TYPE uint16_3
+#endif
+
+#if ENABLED(MARLIN_DEV_MODE)
+  typedef uint16_t cutter_frequency_t;
+  #define CUTTER_MENU_FREQUENCY_TYPE uint16_5
+#endif
diff --git a/Marlin/src/feature/tmc_util.cpp b/Marlin/src/feature/tmc_util.cpp
new file mode 100644
index 0000000..8d01568
--- /dev/null
+++ b/Marlin/src/feature/tmc_util.cpp
@@ -0,0 +1,1283 @@
+/**
+ * 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 HAS_TRINAMIC_CONFIG
+
+#include "tmc_util.h"
+#include "../MarlinCore.h"
+
+#include "../module/stepper/indirection.h"
+#include "../module/printcounter.h"
+#include "../libs/duration_t.h"
+#include "../gcode/gcode.h"
+
+#if ENABLED(TMC_DEBUG)
+  #include "../module/planner.h"
+  #include "../libs/hex_print.h"
+  #if ENABLED(MONITOR_DRIVER_STATUS)
+    static uint16_t report_tmc_status_interval; // = 0
+  #endif
+#endif
+
+#if HAS_LCD_MENU
+  #include "../module/stepper.h"
+#endif
+
+/**
+ * Check for over temperature or short to ground error flags.
+ * Report and log warning of overtemperature condition.
+ * Reduce driver current in a persistent otpw condition.
+ * Keep track of otpw counter so we don't reduce current on a single instance,
+ * and so we don't repeatedly report warning before the condition is cleared.
+ */
+#if ENABLED(MONITOR_DRIVER_STATUS)
+
+  struct TMC_driver_data {
+    uint32_t drv_status;
+    bool is_otpw:1,
+         is_ot:1,
+         is_s2g:1,
+         is_error:1
+         #if ENABLED(TMC_DEBUG)
+           , is_stall:1
+           , is_stealth:1
+           , is_standstill:1
+           #if HAS_STALLGUARD
+             , sg_result_reasonable:1
+           #endif
+         #endif
+      ;
+    #if ENABLED(TMC_DEBUG)
+      #if HAS_TMCX1X0 || HAS_TMC220x
+        uint8_t cs_actual;
+      #endif
+      #if HAS_STALLGUARD
+        uint16_t sg_result;
+      #endif
+    #endif
+  };
+
+  #if HAS_TMCX1X0
+
+    #if ENABLED(TMC_DEBUG)
+      static uint32_t get_pwm_scale(TMC2130Stepper &st) { return st.PWM_SCALE(); }
+    #endif
+
+    static TMC_driver_data get_driver_data(TMC2130Stepper &st) {
+      constexpr uint8_t OT_bp = 25, OTPW_bp = 26;
+      constexpr uint32_t S2G_bm = 0x18000000;
+      #if ENABLED(TMC_DEBUG)
+        constexpr uint16_t SG_RESULT_bm = 0x3FF; // 0:9
+        constexpr uint8_t STEALTH_bp = 14;
+        constexpr uint32_t CS_ACTUAL_bm = 0x1F0000; // 16:20
+        constexpr uint8_t STALL_GUARD_bp = 24;
+        constexpr uint8_t STST_bp = 31;
+      #endif
+      TMC_driver_data data;
+      const auto ds = data.drv_status = st.DRV_STATUS();
+      #ifdef __AVR__
+
+        // 8-bit optimization saves up to 70 bytes of PROGMEM per axis
+        uint8_t spart;
+        #if ENABLED(TMC_DEBUG)
+          data.sg_result = ds & SG_RESULT_bm;
+          spart = ds >> 8;
+          data.is_stealth = TEST(spart, STEALTH_bp - 8);
+          spart = ds >> 16;
+          data.cs_actual = spart & (CS_ACTUAL_bm >> 16);
+        #endif
+        spart = ds >> 24;
+        data.is_ot = TEST(spart, OT_bp - 24);
+        data.is_otpw = TEST(spart, OTPW_bp - 24);
+        data.is_s2g = !!(spart & (S2G_bm >> 24));
+        #if ENABLED(TMC_DEBUG)
+          data.is_stall = TEST(spart, STALL_GUARD_bp - 24);
+          data.is_standstill = TEST(spart, STST_bp - 24);
+          data.sg_result_reasonable = !data.is_standstill; // sg_result has no reasonable meaning while standstill
+        #endif
+
+      #else // !__AVR__
+
+        data.is_ot = TEST(ds, OT_bp);
+        data.is_otpw = TEST(ds, OTPW_bp);
+        data.is_s2g = !!(ds & S2G_bm);
+        #if ENABLED(TMC_DEBUG)
+          constexpr uint8_t CS_ACTUAL_sb = 16;
+          data.sg_result = ds & SG_RESULT_bm;
+          data.is_stealth = TEST(ds, STEALTH_bp);
+          data.cs_actual = (ds & CS_ACTUAL_bm) >> CS_ACTUAL_sb;
+          data.is_stall = TEST(ds, STALL_GUARD_bp);
+          data.is_standstill = TEST(ds, STST_bp);
+          data.sg_result_reasonable = !data.is_standstill; // sg_result has no reasonable meaning while standstill
+        #endif
+
+      #endif // !__AVR__
+
+      return data;
+    }
+
+  #endif // HAS_TMCX1X0
+
+  #if HAS_TMC220x
+
+    #if ENABLED(TMC_DEBUG)
+      static uint32_t get_pwm_scale(TMC2208Stepper &st) { return st.pwm_scale_sum(); }
+    #endif
+
+    static TMC_driver_data get_driver_data(TMC2208Stepper &st) {
+      constexpr uint8_t OTPW_bp = 0, OT_bp = 1;
+      constexpr uint8_t S2G_bm = 0b111100; // 2..5
+      TMC_driver_data data;
+      const auto ds = data.drv_status = st.DRV_STATUS();
+      data.is_otpw = TEST(ds, OTPW_bp);
+      data.is_ot = TEST(ds, OT_bp);
+      data.is_s2g = !!(ds & S2G_bm);
+      #if ENABLED(TMC_DEBUG)
+        constexpr uint32_t CS_ACTUAL_bm = 0x1F0000; // 16:20
+        constexpr uint8_t STEALTH_bp = 30, STST_bp = 31;
+        #ifdef __AVR__
+          // 8-bit optimization saves up to 12 bytes of PROGMEM per axis
+          uint8_t spart = ds >> 16;
+          data.cs_actual = spart & (CS_ACTUAL_bm >> 16);
+          spart = ds >> 24;
+          data.is_stealth = TEST(spart, STEALTH_bp - 24);
+          data.is_standstill = TEST(spart, STST_bp - 24);
+        #else
+          constexpr uint8_t CS_ACTUAL_sb = 16;
+          data.cs_actual = (ds & CS_ACTUAL_bm) >> CS_ACTUAL_sb;
+          data.is_stealth = TEST(ds, STEALTH_bp);
+          data.is_standstill = TEST(ds, STST_bp);
+        #endif
+        TERN_(HAS_STALLGUARD, data.sg_result_reasonable = false);
+      #endif
+      return data;
+    }
+
+  #endif // TMC2208 || TMC2209
+
+  #if HAS_DRIVER(TMC2660)
+
+    #if ENABLED(TMC_DEBUG)
+      static uint32_t get_pwm_scale(TMC2660Stepper) { return 0; }
+    #endif
+
+    static TMC_driver_data get_driver_data(TMC2660Stepper &st) {
+      constexpr uint8_t OT_bp = 1, OTPW_bp = 2;
+      constexpr uint8_t S2G_bm = 0b11000;
+      TMC_driver_data data;
+      const auto ds = data.drv_status = st.DRVSTATUS();
+      uint8_t spart = ds & 0xFF;
+      data.is_otpw = TEST(spart, OTPW_bp);
+      data.is_ot = TEST(spart, OT_bp);
+      data.is_s2g = !!(ds & S2G_bm);
+      #if ENABLED(TMC_DEBUG)
+        constexpr uint8_t STALL_GUARD_bp = 0;
+        constexpr uint8_t STST_bp = 7, SG_RESULT_sp = 10;
+        constexpr uint32_t SG_RESULT_bm = 0xFFC00; // 10:19
+        data.is_stall = TEST(spart, STALL_GUARD_bp);
+        data.is_standstill = TEST(spart, STST_bp);
+        data.sg_result = (ds & SG_RESULT_bm) >> SG_RESULT_sp;
+        data.sg_result_reasonable = true;
+      #endif
+      return data;
+    }
+
+  #endif // TMC2660
+
+  #if ENABLED(STOP_ON_ERROR)
+    void report_driver_error(const TMC_driver_data &data) {
+      SERIAL_ECHOPGM(" driver error detected: 0x");
+      SERIAL_PRINTLN(data.drv_status, HEX);
+      if (data.is_ot) SERIAL_ECHOLNPGM("overtemperature");
+      if (data.is_s2g) SERIAL_ECHOLNPGM("coil short circuit");
+      TERN_(TMC_DEBUG, tmc_report_all(true, true, true, true));
+      kill(PSTR("Driver error"));
+    }
+  #endif
+
+  template<typename TMC>
+  void report_driver_otpw(TMC &st) {
+    char timestamp[14];
+    duration_t elapsed = print_job_timer.duration();
+    const bool has_days = (elapsed.value > 60*60*24L);
+    (void)elapsed.toDigital(timestamp, has_days);
+    SERIAL_EOL();
+    SERIAL_ECHO(timestamp);
+    SERIAL_ECHOPGM(": ");
+    st.printLabel();
+    SERIAL_ECHOLNPAIR(" driver overtemperature warning! (", st.getMilliamps(), "mA)");
+  }
+
+  template<typename TMC>
+  void report_polled_driver_data(TMC &st, const TMC_driver_data &data) {
+    const uint32_t pwm_scale = get_pwm_scale(st);
+    st.printLabel();
+    SERIAL_CHAR(':'); SERIAL_PRINT(pwm_scale, DEC);
+    #if ENABLED(TMC_DEBUG)
+      #if HAS_TMCX1X0 || HAS_TMC220x
+        SERIAL_CHAR('/'); SERIAL_PRINT(data.cs_actual, DEC);
+      #endif
+      #if HAS_STALLGUARD
+        SERIAL_CHAR('/');
+        if (data.sg_result_reasonable)
+          SERIAL_ECHO(data.sg_result);
+        else
+          SERIAL_CHAR('-');
+      #endif
+    #endif
+    SERIAL_CHAR('|');
+    if (st.error_count)       SERIAL_CHAR('E'); // Error
+    if (data.is_ot)           SERIAL_CHAR('O'); // Over-temperature
+    if (data.is_otpw)         SERIAL_CHAR('W'); // over-temperature pre-Warning
+    #if ENABLED(TMC_DEBUG)
+      if (data.is_stall)      SERIAL_CHAR('G'); // stallGuard
+      if (data.is_stealth)    SERIAL_CHAR('T'); // stealthChop
+      if (data.is_standstill) SERIAL_CHAR('I'); // standstIll
+    #endif
+    if (st.flag_otpw)         SERIAL_CHAR('F'); // otpw Flag
+    SERIAL_CHAR('|');
+    if (st.otpw_count > 0) SERIAL_PRINT(st.otpw_count, DEC);
+    SERIAL_CHAR('\t');
+  }
+
+  #if CURRENT_STEP_DOWN > 0
+
+    template<typename TMC>
+    void step_current_down(TMC &st) {
+      if (st.isEnabled()) {
+        const uint16_t I_rms = st.getMilliamps() - (CURRENT_STEP_DOWN);
+        if (I_rms > 50) {
+          st.rms_current(I_rms);
+          #if ENABLED(REPORT_CURRENT_CHANGE)
+            st.printLabel();
+            SERIAL_ECHOLNPAIR(" current decreased to ", I_rms);
+          #endif
+        }
+      }
+    }
+
+  #else
+
+    #define step_current_down(...)
+
+  #endif
+
+  template<typename TMC>
+  bool monitor_tmc_driver(TMC &st, const bool need_update_error_counters, const bool need_debug_reporting) {
+    TMC_driver_data data = get_driver_data(st);
+    if (data.drv_status == 0xFFFFFFFF || data.drv_status == 0x0) return false;
+
+    bool should_step_down = false;
+
+    if (need_update_error_counters) {
+      if (data.is_ot | data.is_s2g) st.error_count++;
+      else if (st.error_count > 0) st.error_count--;
+
+      #if ENABLED(STOP_ON_ERROR)
+        if (st.error_count >= 10) {
+          SERIAL_EOL();
+          st.printLabel();
+          report_driver_error(data);
+        }
+      #endif
+
+      // Report if a warning was triggered
+      if (data.is_otpw && st.otpw_count == 0)
+        report_driver_otpw(st);
+
+      #if CURRENT_STEP_DOWN > 0
+        // Decrease current if is_otpw is true and driver is enabled and there's been more than 4 warnings
+        if (data.is_otpw && st.otpw_count > 4 && st.isEnabled())
+          should_step_down = true;
+      #endif
+
+      if (data.is_otpw) {
+        st.otpw_count++;
+        st.flag_otpw = true;
+      }
+      else if (st.otpw_count > 0) st.otpw_count = 0;
+    }
+
+    #if ENABLED(TMC_DEBUG)
+      if (need_debug_reporting) report_polled_driver_data(st, data);
+    #endif
+
+    return should_step_down;
+  }
+
+  void monitor_tmc_drivers() {
+    const millis_t ms = millis();
+
+    // Poll TMC drivers at the configured interval
+    static millis_t next_poll = 0;
+    const bool need_update_error_counters = ELAPSED(ms, next_poll);
+    if (need_update_error_counters) next_poll = ms + MONITOR_DRIVER_STATUS_INTERVAL_MS;
+
+    // Also poll at intervals for debugging
+    #if ENABLED(TMC_DEBUG)
+      static millis_t next_debug_reporting = 0;
+      const bool need_debug_reporting = report_tmc_status_interval && ELAPSED(ms, next_debug_reporting);
+      if (need_debug_reporting) next_debug_reporting = ms + report_tmc_status_interval;
+    #else
+      constexpr bool need_debug_reporting = false;
+    #endif
+
+    if (need_update_error_counters || need_debug_reporting) {
+
+      #if AXIS_IS_TMC(X) || AXIS_IS_TMC(X2)
+      {
+        bool result = false;
+        #if AXIS_IS_TMC(X)
+          if (monitor_tmc_driver(stepperX, need_update_error_counters, need_debug_reporting)) result = true;
+        #endif
+        #if AXIS_IS_TMC(X2)
+          if (monitor_tmc_driver(stepperX2, need_update_error_counters, need_debug_reporting)) result = true;
+        #endif
+        if (result) {
+          #if AXIS_IS_TMC(X)
+            step_current_down(stepperX);
+          #endif
+          #if AXIS_IS_TMC(X2)
+            step_current_down(stepperX2);
+          #endif
+        }
+      }
+      #endif
+
+      #if AXIS_IS_TMC(Y) || AXIS_IS_TMC(Y2)
+      {
+        bool result = false;
+        #if AXIS_IS_TMC(Y)
+          if (monitor_tmc_driver(stepperY, need_update_error_counters, need_debug_reporting)) result = true;
+        #endif
+        #if AXIS_IS_TMC(Y2)
+          if (monitor_tmc_driver(stepperY2, need_update_error_counters, need_debug_reporting)) result = true;
+        #endif
+        if (result) {
+          #if AXIS_IS_TMC(Y)
+            step_current_down(stepperY);
+          #endif
+          #if AXIS_IS_TMC(Y2)
+            step_current_down(stepperY2);
+          #endif
+        }
+      }
+      #endif
+
+      #if AXIS_IS_TMC(Z) || AXIS_IS_TMC(Z2) || AXIS_IS_TMC(Z3) || AXIS_IS_TMC(Z4)
+      {
+        bool result = false;
+        #if AXIS_IS_TMC(Z)
+          if (monitor_tmc_driver(stepperZ, need_update_error_counters, need_debug_reporting)) result = true;
+        #endif
+        #if AXIS_IS_TMC(Z2)
+          if (monitor_tmc_driver(stepperZ2, need_update_error_counters, need_debug_reporting)) result = true;
+        #endif
+        #if AXIS_IS_TMC(Z3)
+          if (monitor_tmc_driver(stepperZ3, need_update_error_counters, need_debug_reporting)) result = true;
+        #endif
+        #if AXIS_IS_TMC(Z4)
+          if (monitor_tmc_driver(stepperZ4, need_update_error_counters, need_debug_reporting)) result = true;
+        #endif
+        if (result) {
+          #if AXIS_IS_TMC(Z)
+            step_current_down(stepperZ);
+          #endif
+          #if AXIS_IS_TMC(Z2)
+            step_current_down(stepperZ2);
+          #endif
+          #if AXIS_IS_TMC(Z3)
+            step_current_down(stepperZ3);
+          #endif
+          #if AXIS_IS_TMC(Z4)
+            step_current_down(stepperZ4);
+          #endif
+        }
+      }
+      #endif
+
+      #if AXIS_IS_TMC(E0)
+        (void)monitor_tmc_driver(stepperE0, need_update_error_counters, need_debug_reporting);
+      #endif
+      #if AXIS_IS_TMC(E1)
+        (void)monitor_tmc_driver(stepperE1, need_update_error_counters, need_debug_reporting);
+      #endif
+      #if AXIS_IS_TMC(E2)
+        (void)monitor_tmc_driver(stepperE2, need_update_error_counters, need_debug_reporting);
+      #endif
+      #if AXIS_IS_TMC(E3)
+        (void)monitor_tmc_driver(stepperE3, need_update_error_counters, need_debug_reporting);
+      #endif
+      #if AXIS_IS_TMC(E4)
+        (void)monitor_tmc_driver(stepperE4, need_update_error_counters, need_debug_reporting);
+      #endif
+      #if AXIS_IS_TMC(E5)
+        (void)monitor_tmc_driver(stepperE5, need_update_error_counters, need_debug_reporting);
+      #endif
+      #if AXIS_IS_TMC(E6)
+        (void)monitor_tmc_driver(stepperE6, need_update_error_counters, need_debug_reporting);
+      #endif
+      #if AXIS_IS_TMC(E7)
+        (void)monitor_tmc_driver(stepperE7, need_update_error_counters, need_debug_reporting);
+      #endif
+
+      if (TERN0(TMC_DEBUG, need_debug_reporting)) SERIAL_EOL();
+    }
+  }
+
+#endif // MONITOR_DRIVER_STATUS
+
+#if ENABLED(TMC_DEBUG)
+
+  /**
+   * M122 [S<0|1>] [Pnnn] Enable periodic status reports
+   */
+  #if ENABLED(MONITOR_DRIVER_STATUS)
+    void tmc_set_report_interval(const uint16_t update_interval) {
+      if ((report_tmc_status_interval = update_interval))
+        SERIAL_ECHOLNPGM("axis:pwm_scale"
+          #if HAS_STEALTHCHOP
+            "/curr_scale"
+          #endif
+          #if HAS_STALLGUARD
+            "/mech_load"
+          #endif
+          "|flags|warncount"
+        );
+    }
+  #endif
+
+  enum TMC_debug_enum : char {
+    TMC_CODES,
+    TMC_UART_ADDR,
+    TMC_ENABLED,
+    TMC_CURRENT,
+    TMC_RMS_CURRENT,
+    TMC_MAX_CURRENT,
+    TMC_IRUN,
+    TMC_IHOLD,
+    TMC_GLOBAL_SCALER,
+    TMC_CS_ACTUAL,
+    TMC_PWM_SCALE,
+    TMC_PWM_SCALE_SUM,
+    TMC_PWM_SCALE_AUTO,
+    TMC_PWM_OFS_AUTO,
+    TMC_PWM_GRAD_AUTO,
+    TMC_VSENSE,
+    TMC_STEALTHCHOP,
+    TMC_MICROSTEPS,
+    TMC_TSTEP,
+    TMC_TPWMTHRS,
+    TMC_TPWMTHRS_MMS,
+    TMC_OTPW,
+    TMC_OTPW_TRIGGERED,
+    TMC_TOFF,
+    TMC_TBL,
+    TMC_HEND,
+    TMC_HSTRT,
+    TMC_SGT,
+    TMC_MSCNT,
+    TMC_INTERPOLATE
+  };
+  enum TMC_drv_status_enum : char {
+    TMC_DRV_CODES,
+    TMC_STST,
+    TMC_OLB,
+    TMC_OLA,
+    TMC_S2GB,
+    TMC_S2GA,
+    TMC_DRV_OTPW,
+    TMC_OT,
+    TMC_STALLGUARD,
+    TMC_DRV_CS_ACTUAL,
+    TMC_FSACTIVE,
+    TMC_SG_RESULT,
+    TMC_DRV_STATUS_HEX,
+    TMC_T157,
+    TMC_T150,
+    TMC_T143,
+    TMC_T120,
+    TMC_STEALTH,
+    TMC_S2VSB,
+    TMC_S2VSA
+  };
+  enum TMC_get_registers_enum : char {
+    TMC_AXIS_CODES,
+    TMC_GET_GCONF,
+    TMC_GET_IHOLD_IRUN,
+    TMC_GET_GSTAT,
+    TMC_GET_IOIN,
+    TMC_GET_TPOWERDOWN,
+    TMC_GET_TSTEP,
+    TMC_GET_TPWMTHRS,
+    TMC_GET_TCOOLTHRS,
+    TMC_GET_THIGH,
+    TMC_GET_CHOPCONF,
+    TMC_GET_COOLCONF,
+    TMC_GET_PWMCONF,
+    TMC_GET_PWM_SCALE,
+    TMC_GET_DRV_STATUS,
+    TMC_GET_DRVCONF,
+    TMC_GET_DRVCTRL,
+    TMC_GET_DRVSTATUS,
+    TMC_GET_SGCSCONF,
+    TMC_GET_SMARTEN
+  };
+
+  template<class TMC>
+  static void print_vsense(TMC &st) { serialprintPGM(st.vsense() ? PSTR("1=.18") : PSTR("0=.325")); }
+
+  #if HAS_DRIVER(TMC2130) || HAS_DRIVER(TMC5130)
+    static void _tmc_status(TMC2130Stepper &st, const TMC_debug_enum i) {
+      switch (i) {
+        case TMC_PWM_SCALE: SERIAL_PRINT(st.PWM_SCALE(), DEC); break;
+        case TMC_SGT: SERIAL_PRINT(st.sgt(), DEC); break;
+        case TMC_STEALTHCHOP: serialprint_truefalse(st.en_pwm_mode()); break;
+        case TMC_INTERPOLATE: serialprint_truefalse(st.intpol()); break;
+        default: break;
+      }
+    }
+  #endif
+  #if HAS_TMCX1X0
+    static void _tmc_parse_drv_status(TMC2130Stepper &st, const TMC_drv_status_enum i) {
+      switch (i) {
+        case TMC_STALLGUARD: if (st.stallguard()) SERIAL_CHAR('*'); break;
+        case TMC_SG_RESULT:  SERIAL_PRINT(st.sg_result(), DEC); break;
+        case TMC_FSACTIVE:   if (st.fsactive())   SERIAL_CHAR('*'); break;
+        case TMC_DRV_CS_ACTUAL: SERIAL_PRINT(st.cs_actual(), DEC); break;
+        default: break;
+      }
+    }
+  #endif
+
+  #if HAS_DRIVER(TMC2160) || HAS_DRIVER(TMC5160)
+    template<char AXIS_LETTER, char DRIVER_ID, AxisEnum AXIS_ID>
+    void print_vsense(TMCMarlin<TMC2160Stepper, AXIS_LETTER, DRIVER_ID, AXIS_ID> &) { }
+
+    template<char AXIS_LETTER, char DRIVER_ID, AxisEnum AXIS_ID>
+    void print_vsense(TMCMarlin<TMC5160Stepper, AXIS_LETTER, DRIVER_ID, AXIS_ID> &) { }
+
+    static void _tmc_status(TMC2160Stepper &st, const TMC_debug_enum i) {
+      switch (i) {
+        case TMC_PWM_SCALE: SERIAL_PRINT(st.PWM_SCALE(), DEC); break;
+        case TMC_SGT: SERIAL_PRINT(st.sgt(), DEC); break;
+        case TMC_STEALTHCHOP: serialprint_truefalse(st.en_pwm_mode()); break;
+        case TMC_GLOBAL_SCALER:
+          {
+            uint16_t value = st.GLOBAL_SCALER();
+            SERIAL_PRINT(value ?: 256, DEC);
+            SERIAL_ECHOPGM("/256");
+          }
+          break;
+        case TMC_INTERPOLATE: serialprint_truefalse(st.intpol()); break;
+        default: break;
+      }
+    }
+  #endif
+
+  #if HAS_TMC220x
+    static void _tmc_status(TMC2208Stepper &st, const TMC_debug_enum i) {
+      switch (i) {
+        case TMC_PWM_SCALE_SUM: SERIAL_PRINT(st.pwm_scale_sum(), DEC); break;
+        case TMC_PWM_SCALE_AUTO: SERIAL_PRINT(st.pwm_scale_auto(), DEC); break;
+        case TMC_PWM_OFS_AUTO: SERIAL_PRINT(st.pwm_ofs_auto(), DEC); break;
+        case TMC_PWM_GRAD_AUTO: SERIAL_PRINT(st.pwm_grad_auto(), DEC); break;
+        case TMC_STEALTHCHOP: serialprint_truefalse(st.stealth()); break;
+        case TMC_INTERPOLATE: serialprint_truefalse(st.intpol()); break;
+        default: break;
+      }
+    }
+
+    #if HAS_DRIVER(TMC2209)
+      template<char AXIS_LETTER, char DRIVER_ID, AxisEnum AXIS_ID>
+      static void _tmc_status(TMCMarlin<TMC2209Stepper, AXIS_LETTER, DRIVER_ID, AXIS_ID> &st, const TMC_debug_enum i) {
+        switch (i) {
+          case TMC_SGT:       SERIAL_PRINT(st.SGTHRS(), DEC); break;
+          case TMC_UART_ADDR: SERIAL_PRINT(st.get_address(), DEC); break;
+          default:
+            TMC2208Stepper *parent = &st;
+            _tmc_status(*parent, i);
+            break;
+        }
+      }
+    #endif
+
+    static void _tmc_parse_drv_status(TMC2208Stepper &st, const TMC_drv_status_enum i) {
+      switch (i) {
+        case TMC_T157: if (st.t157()) SERIAL_CHAR('*'); break;
+        case TMC_T150: if (st.t150()) SERIAL_CHAR('*'); break;
+        case TMC_T143: if (st.t143()) SERIAL_CHAR('*'); break;
+        case TMC_T120: if (st.t120()) SERIAL_CHAR('*'); break;
+        case TMC_S2VSA: if (st.s2vsa()) SERIAL_CHAR('*'); break;
+        case TMC_S2VSB: if (st.s2vsb()) SERIAL_CHAR('*'); break;
+        case TMC_DRV_CS_ACTUAL: SERIAL_PRINT(st.cs_actual(), DEC); break;
+        default: break;
+      }
+    }
+
+    #if HAS_DRIVER(TMC2209)
+      static void _tmc_parse_drv_status(TMC2209Stepper &st, const TMC_drv_status_enum i) {
+        switch (i) {
+          case TMC_SG_RESULT: SERIAL_PRINT(st.SG_RESULT(), DEC); break;
+          default:            _tmc_parse_drv_status(static_cast<TMC2208Stepper &>(st), i); break;
+        }
+      }
+    #endif
+  #endif
+
+  #if HAS_DRIVER(TMC2660)
+    static void _tmc_parse_drv_status(TMC2660Stepper, const TMC_drv_status_enum) { }
+    static void _tmc_status(TMC2660Stepper &st, const TMC_debug_enum i) {
+      switch (i) {
+        case TMC_INTERPOLATE: serialprint_truefalse(st.intpol()); break;
+        default: break;
+      }
+    }
+  #endif
+
+  template <typename TMC>
+  static void tmc_status(TMC &st, const TMC_debug_enum i) {
+    SERIAL_CHAR('\t');
+    switch (i) {
+      case TMC_CODES: st.printLabel(); break;
+      case TMC_ENABLED: serialprint_truefalse(st.isEnabled()); break;
+      case TMC_CURRENT: SERIAL_ECHO(st.getMilliamps()); break;
+      case TMC_RMS_CURRENT: SERIAL_ECHO(st.rms_current()); break;
+      case TMC_MAX_CURRENT: SERIAL_PRINT((float)st.rms_current() * 1.41, 0); break;
+      case TMC_IRUN:
+        SERIAL_PRINT(st.irun(), DEC);
+        SERIAL_ECHOPGM("/31");
+        break;
+      case TMC_IHOLD:
+        SERIAL_PRINT(st.ihold(), DEC);
+        SERIAL_ECHOPGM("/31");
+        break;
+      case TMC_CS_ACTUAL:
+        SERIAL_PRINT(st.cs_actual(), DEC);
+        SERIAL_ECHOPGM("/31");
+        break;
+      case TMC_VSENSE: print_vsense(st); break;
+      case TMC_MICROSTEPS: SERIAL_ECHO(st.microsteps()); break;
+      case TMC_TSTEP: {
+        const uint32_t tstep_value = st.TSTEP();
+        if (tstep_value != 0xFFFFF) SERIAL_ECHO(tstep_value); else SERIAL_ECHOPGM("max");
+      } break;
+      #if ENABLED(HYBRID_THRESHOLD)
+        case TMC_TPWMTHRS: SERIAL_ECHO(uint32_t(st.TPWMTHRS())); break;
+        case TMC_TPWMTHRS_MMS: {
+          const uint32_t tpwmthrs_val = st.get_pwm_thrs();
+          if (tpwmthrs_val) SERIAL_ECHO(tpwmthrs_val); else SERIAL_CHAR('-');
+        } break;
+      #endif
+      case TMC_OTPW: serialprint_truefalse(st.otpw()); break;
+      #if ENABLED(MONITOR_DRIVER_STATUS)
+        case TMC_OTPW_TRIGGERED: serialprint_truefalse(st.getOTPW()); break;
+      #endif
+      case TMC_TOFF: SERIAL_PRINT(st.toff(), DEC); break;
+      case TMC_TBL: SERIAL_PRINT(st.blank_time(), DEC); break;
+      case TMC_HEND: SERIAL_PRINT(st.hysteresis_end(), DEC); break;
+      case TMC_HSTRT: SERIAL_PRINT(st.hysteresis_start(), DEC); break;
+      case TMC_MSCNT: SERIAL_PRINT(st.get_microstep_counter(), DEC); break;
+      default: _tmc_status(st, i); break;
+    }
+  }
+
+  #if HAS_DRIVER(TMC2660)
+    template<char AXIS_LETTER, char DRIVER_ID, AxisEnum AXIS_ID>
+    void tmc_status(TMCMarlin<TMC2660Stepper, AXIS_LETTER, DRIVER_ID, AXIS_ID> &st, const TMC_debug_enum i) {
+      SERIAL_CHAR('\t');
+      switch (i) {
+        case TMC_CODES: st.printLabel(); break;
+        case TMC_ENABLED: serialprint_truefalse(st.isEnabled()); break;
+        case TMC_CURRENT: SERIAL_ECHO(st.getMilliamps()); break;
+        case TMC_RMS_CURRENT: SERIAL_ECHO(st.rms_current()); break;
+        case TMC_MAX_CURRENT: SERIAL_PRINT((float)st.rms_current() * 1.41, 0); break;
+        case TMC_IRUN:
+          SERIAL_PRINT(st.cs(), DEC);
+          SERIAL_ECHOPGM("/31");
+          break;
+        case TMC_VSENSE: serialprintPGM(st.vsense() ? PSTR("1=.165") : PSTR("0=.310")); break;
+        case TMC_MICROSTEPS: SERIAL_ECHO(st.microsteps()); break;
+        //case TMC_OTPW: serialprint_truefalse(st.otpw()); break;
+        //case TMC_OTPW_TRIGGERED: serialprint_truefalse(st.getOTPW()); break;
+        case TMC_SGT: SERIAL_PRINT(st.sgt(), DEC); break;
+        case TMC_TOFF: SERIAL_PRINT(st.toff(), DEC); break;
+        case TMC_TBL: SERIAL_PRINT(st.blank_time(), DEC); break;
+        case TMC_HEND: SERIAL_PRINT(st.hysteresis_end(), DEC); break;
+        case TMC_HSTRT: SERIAL_PRINT(st.hysteresis_start(), DEC); break;
+        default: break;
+      }
+    }
+  #endif
+
+  template <typename TMC>
+  static void tmc_parse_drv_status(TMC &st, const TMC_drv_status_enum i) {
+    SERIAL_CHAR('\t');
+    switch (i) {
+      case TMC_DRV_CODES:     st.printLabel();  break;
+      case TMC_STST:          if (!st.stst())   SERIAL_CHAR('*'); break;
+      case TMC_OLB:           if (st.olb())     SERIAL_CHAR('*'); break;
+      case TMC_OLA:           if (st.ola())     SERIAL_CHAR('*'); break;
+      case TMC_S2GB:          if (st.s2gb())    SERIAL_CHAR('*'); break;
+      case TMC_S2GA:          if (st.s2ga())    SERIAL_CHAR('*'); break;
+      case TMC_DRV_OTPW:      if (st.otpw())    SERIAL_CHAR('*'); break;
+      case TMC_OT:            if (st.ot())      SERIAL_CHAR('*'); break;
+      case TMC_DRV_STATUS_HEX: {
+        const uint32_t drv_status = st.DRV_STATUS();
+        SERIAL_CHAR('\t');
+        st.printLabel();
+        SERIAL_CHAR('\t');
+        print_hex_long(drv_status, ':');
+        if (drv_status == 0xFFFFFFFF || drv_status == 0) SERIAL_ECHOPGM("\t Bad response!");
+        SERIAL_EOL();
+        break;
+      }
+      default: _tmc_parse_drv_status(st, i); break;
+    }
+  }
+
+  static void tmc_debug_loop(const TMC_debug_enum i, const bool print_x, const bool print_y, const bool print_z, const bool print_e) {
+    if (print_x) {
+      #if AXIS_IS_TMC(X)
+        tmc_status(stepperX, i);
+      #endif
+      #if AXIS_IS_TMC(X2)
+        tmc_status(stepperX2, i);
+      #endif
+    }
+
+    if (print_y) {
+      #if AXIS_IS_TMC(Y)
+        tmc_status(stepperY, i);
+      #endif
+      #if AXIS_IS_TMC(Y2)
+        tmc_status(stepperY2, i);
+      #endif
+    }
+
+    if (print_z) {
+      #if AXIS_IS_TMC(Z)
+        tmc_status(stepperZ, i);
+      #endif
+      #if AXIS_IS_TMC(Z2)
+        tmc_status(stepperZ2, i);
+      #endif
+      #if AXIS_IS_TMC(Z3)
+        tmc_status(stepperZ3, i);
+      #endif
+      #if AXIS_IS_TMC(Z4)
+        tmc_status(stepperZ4, i);
+      #endif
+    }
+
+    if (print_e) {
+      #if AXIS_IS_TMC(E0)
+        tmc_status(stepperE0, i);
+      #endif
+      #if AXIS_IS_TMC(E1)
+        tmc_status(stepperE1, i);
+      #endif
+      #if AXIS_IS_TMC(E2)
+        tmc_status(stepperE2, i);
+      #endif
+      #if AXIS_IS_TMC(E3)
+        tmc_status(stepperE3, i);
+      #endif
+      #if AXIS_IS_TMC(E4)
+        tmc_status(stepperE4, i);
+      #endif
+      #if AXIS_IS_TMC(E5)
+        tmc_status(stepperE5, i);
+      #endif
+      #if AXIS_IS_TMC(E6)
+        tmc_status(stepperE6, i);
+      #endif
+      #if AXIS_IS_TMC(E7)
+        tmc_status(stepperE7, i);
+      #endif
+    }
+
+    SERIAL_EOL();
+  }
+
+  static void drv_status_loop(const TMC_drv_status_enum i, const bool print_x, const bool print_y, const bool print_z, const bool print_e) {
+    if (print_x) {
+      #if AXIS_IS_TMC(X)
+        tmc_parse_drv_status(stepperX, i);
+      #endif
+      #if AXIS_IS_TMC(X2)
+        tmc_parse_drv_status(stepperX2, i);
+      #endif
+    }
+
+    if (print_y) {
+      #if AXIS_IS_TMC(Y)
+        tmc_parse_drv_status(stepperY, i);
+      #endif
+      #if AXIS_IS_TMC(Y2)
+        tmc_parse_drv_status(stepperY2, i);
+      #endif
+    }
+
+    if (print_z) {
+      #if AXIS_IS_TMC(Z)
+        tmc_parse_drv_status(stepperZ, i);
+      #endif
+      #if AXIS_IS_TMC(Z2)
+        tmc_parse_drv_status(stepperZ2, i);
+      #endif
+      #if AXIS_IS_TMC(Z3)
+        tmc_parse_drv_status(stepperZ3, i);
+      #endif
+      #if AXIS_IS_TMC(Z4)
+        tmc_parse_drv_status(stepperZ4, i);
+      #endif
+    }
+
+    if (print_e) {
+      #if AXIS_IS_TMC(E0)
+        tmc_parse_drv_status(stepperE0, i);
+      #endif
+      #if AXIS_IS_TMC(E1)
+        tmc_parse_drv_status(stepperE1, i);
+      #endif
+      #if AXIS_IS_TMC(E2)
+        tmc_parse_drv_status(stepperE2, i);
+      #endif
+      #if AXIS_IS_TMC(E3)
+        tmc_parse_drv_status(stepperE3, i);
+      #endif
+      #if AXIS_IS_TMC(E4)
+        tmc_parse_drv_status(stepperE4, i);
+      #endif
+      #if AXIS_IS_TMC(E5)
+        tmc_parse_drv_status(stepperE5, i);
+      #endif
+      #if AXIS_IS_TMC(E6)
+        tmc_parse_drv_status(stepperE6, i);
+      #endif
+      #if AXIS_IS_TMC(E7)
+        tmc_parse_drv_status(stepperE7, i);
+      #endif
+    }
+
+    SERIAL_EOL();
+  }
+
+  /**
+   * M122 report functions
+   */
+
+  void tmc_report_all(bool print_x, const bool print_y, const bool print_z, const bool print_e) {
+    #define TMC_REPORT(LABEL, ITEM) do{ SERIAL_ECHOPGM(LABEL);  tmc_debug_loop(ITEM, print_x, print_y, print_z, print_e); }while(0)
+    #define DRV_REPORT(LABEL, ITEM) do{ SERIAL_ECHOPGM(LABEL); drv_status_loop(ITEM, print_x, print_y, print_z, print_e); }while(0)
+    TMC_REPORT("\t",                 TMC_CODES);
+    #if HAS_DRIVER(TMC2209)
+      TMC_REPORT("Address\t",        TMC_UART_ADDR);
+    #endif
+    TMC_REPORT("Enabled\t",          TMC_ENABLED);
+    TMC_REPORT("Set current",        TMC_CURRENT);
+    TMC_REPORT("RMS current",        TMC_RMS_CURRENT);
+    TMC_REPORT("MAX current",        TMC_MAX_CURRENT);
+    TMC_REPORT("Run current",        TMC_IRUN);
+    TMC_REPORT("Hold current",       TMC_IHOLD);
+    #if HAS_DRIVER(TMC2160) || HAS_DRIVER(TMC5160)
+      TMC_REPORT("Global scaler",    TMC_GLOBAL_SCALER);
+    #endif
+    TMC_REPORT("CS actual",          TMC_CS_ACTUAL);
+    TMC_REPORT("PWM scale",          TMC_PWM_SCALE);
+    #if HAS_DRIVER(TMC2130) || HAS_DRIVER(TMC2224) || HAS_DRIVER(TMC2660) || HAS_TMC220x
+      TMC_REPORT("vsense\t",         TMC_VSENSE);
+    #endif
+    TMC_REPORT("stealthChop",        TMC_STEALTHCHOP);
+    TMC_REPORT("msteps\t",           TMC_MICROSTEPS);
+    TMC_REPORT("interp\t",           TMC_INTERPOLATE);
+    TMC_REPORT("tstep\t",            TMC_TSTEP);
+    TMC_REPORT("PWM thresh.",        TMC_TPWMTHRS);
+    TMC_REPORT("[mm/s]\t",           TMC_TPWMTHRS_MMS);
+    TMC_REPORT("OT prewarn",         TMC_OTPW);
+    #if ENABLED(MONITOR_DRIVER_STATUS)
+      TMC_REPORT("triggered\n OTP\t", TMC_OTPW_TRIGGERED);
+    #endif
+
+    #if HAS_TMC220x
+      TMC_REPORT("pwm scale sum",     TMC_PWM_SCALE_SUM);
+      TMC_REPORT("pwm scale auto",    TMC_PWM_SCALE_AUTO);
+      TMC_REPORT("pwm offset auto",   TMC_PWM_OFS_AUTO);
+      TMC_REPORT("pwm grad auto",     TMC_PWM_GRAD_AUTO);
+    #endif
+
+    TMC_REPORT("off time",           TMC_TOFF);
+    TMC_REPORT("blank time",         TMC_TBL);
+    TMC_REPORT("hysteresis\n -end\t", TMC_HEND);
+    TMC_REPORT(" -start\t",          TMC_HSTRT);
+    TMC_REPORT("Stallguard thrs",    TMC_SGT);
+    TMC_REPORT("uStep count",        TMC_MSCNT);
+    DRV_REPORT("DRVSTATUS",          TMC_DRV_CODES);
+    #if HAS_TMCX1X0 || HAS_TMC220x
+      DRV_REPORT("sg_result",        TMC_SG_RESULT);
+    #endif
+    #if HAS_TMCX1X0
+      DRV_REPORT("stallguard",       TMC_STALLGUARD);
+      DRV_REPORT("fsactive",         TMC_FSACTIVE);
+    #endif
+    DRV_REPORT("stst\t",             TMC_STST);
+    DRV_REPORT("olb\t",              TMC_OLB);
+    DRV_REPORT("ola\t",              TMC_OLA);
+    DRV_REPORT("s2gb\t",             TMC_S2GB);
+    DRV_REPORT("s2ga\t",             TMC_S2GA);
+    DRV_REPORT("otpw\t",             TMC_DRV_OTPW);
+    DRV_REPORT("ot\t",               TMC_OT);
+    #if HAS_TMC220x
+      DRV_REPORT("157C\t",           TMC_T157);
+      DRV_REPORT("150C\t",           TMC_T150);
+      DRV_REPORT("143C\t",           TMC_T143);
+      DRV_REPORT("120C\t",           TMC_T120);
+      DRV_REPORT("s2vsa\t",          TMC_S2VSA);
+      DRV_REPORT("s2vsb\t",          TMC_S2VSB);
+    #endif
+    DRV_REPORT("Driver registers:\n",TMC_DRV_STATUS_HEX);
+    SERIAL_EOL();
+  }
+
+  #define PRINT_TMC_REGISTER(REG_CASE) case TMC_GET_##REG_CASE: print_hex_long(st.REG_CASE(), ':'); break
+
+  #if HAS_TMCX1X0
+    static void tmc_get_ic_registers(TMC2130Stepper &st, const TMC_get_registers_enum i) {
+      switch (i) {
+        PRINT_TMC_REGISTER(TCOOLTHRS);
+        PRINT_TMC_REGISTER(THIGH);
+        PRINT_TMC_REGISTER(COOLCONF);
+        default: SERIAL_CHAR('\t'); break;
+      }
+    }
+  #endif
+  #if HAS_TMC220x
+    static void tmc_get_ic_registers(TMC2208Stepper, const TMC_get_registers_enum) { SERIAL_CHAR('\t'); }
+  #endif
+
+  #if HAS_TRINAMIC_CONFIG
+    template<class TMC>
+    static void tmc_get_registers(TMC &st, const TMC_get_registers_enum i) {
+      switch (i) {
+        case TMC_AXIS_CODES: SERIAL_CHAR('\t'); st.printLabel(); break;
+        PRINT_TMC_REGISTER(GCONF);
+        PRINT_TMC_REGISTER(IHOLD_IRUN);
+        PRINT_TMC_REGISTER(GSTAT);
+        PRINT_TMC_REGISTER(IOIN);
+        PRINT_TMC_REGISTER(TPOWERDOWN);
+        PRINT_TMC_REGISTER(TSTEP);
+        PRINT_TMC_REGISTER(TPWMTHRS);
+        PRINT_TMC_REGISTER(CHOPCONF);
+        PRINT_TMC_REGISTER(PWMCONF);
+        PRINT_TMC_REGISTER(PWM_SCALE);
+        PRINT_TMC_REGISTER(DRV_STATUS);
+        default: tmc_get_ic_registers(st, i); break;
+      }
+      SERIAL_CHAR('\t');
+    }
+  #endif
+  #if HAS_DRIVER(TMC2660)
+    template <char AXIS_LETTER, char DRIVER_ID, AxisEnum AXIS_ID>
+    static void tmc_get_registers(TMCMarlin<TMC2660Stepper, AXIS_LETTER, DRIVER_ID, AXIS_ID> &st, const TMC_get_registers_enum i) {
+      switch (i) {
+        case TMC_AXIS_CODES: SERIAL_CHAR('\t'); st.printLabel(); break;
+        PRINT_TMC_REGISTER(DRVCONF);
+        PRINT_TMC_REGISTER(DRVCTRL);
+        PRINT_TMC_REGISTER(CHOPCONF);
+        PRINT_TMC_REGISTER(DRVSTATUS);
+        PRINT_TMC_REGISTER(SGCSCONF);
+        PRINT_TMC_REGISTER(SMARTEN);
+        default: SERIAL_CHAR('\t'); break;
+      }
+      SERIAL_CHAR('\t');
+    }
+  #endif
+
+  static void tmc_get_registers(TMC_get_registers_enum i, const bool print_x, const bool print_y, const bool print_z, const bool print_e) {
+    if (print_x) {
+      #if AXIS_IS_TMC(X)
+        tmc_get_registers(stepperX, i);
+      #endif
+      #if AXIS_IS_TMC(X2)
+        tmc_get_registers(stepperX2, i);
+      #endif
+    }
+
+    if (print_y) {
+      #if AXIS_IS_TMC(Y)
+        tmc_get_registers(stepperY, i);
+      #endif
+      #if AXIS_IS_TMC(Y2)
+        tmc_get_registers(stepperY2, i);
+      #endif
+    }
+
+    if (print_z) {
+      #if AXIS_IS_TMC(Z)
+        tmc_get_registers(stepperZ, i);
+      #endif
+      #if AXIS_IS_TMC(Z2)
+        tmc_get_registers(stepperZ2, i);
+      #endif
+      #if AXIS_IS_TMC(Z3)
+        tmc_get_registers(stepperZ3, i);
+      #endif
+      #if AXIS_IS_TMC(Z4)
+        tmc_get_registers(stepperZ4, i);
+      #endif
+    }
+
+    if (print_e) {
+      #if AXIS_IS_TMC(E0)
+        tmc_get_registers(stepperE0, i);
+      #endif
+      #if AXIS_IS_TMC(E1)
+        tmc_get_registers(stepperE1, i);
+      #endif
+      #if AXIS_IS_TMC(E2)
+        tmc_get_registers(stepperE2, i);
+      #endif
+      #if AXIS_IS_TMC(E3)
+        tmc_get_registers(stepperE3, i);
+      #endif
+      #if AXIS_IS_TMC(E4)
+        tmc_get_registers(stepperE4, i);
+      #endif
+      #if AXIS_IS_TMC(E5)
+        tmc_get_registers(stepperE5, i);
+      #endif
+      #if AXIS_IS_TMC(E6)
+        tmc_get_registers(stepperE6, i);
+      #endif
+      #if AXIS_IS_TMC(E7)
+        tmc_get_registers(stepperE7, i);
+      #endif
+    }
+
+    SERIAL_EOL();
+  }
+
+  void tmc_get_registers(bool print_x, bool print_y, bool print_z, bool print_e) {
+    #define _TMC_GET_REG(LABEL, ITEM) do{ SERIAL_ECHOPGM(LABEL); tmc_get_registers(ITEM, print_x, print_y, print_z, print_e); }while(0)
+    #define TMC_GET_REG(NAME, TABS) _TMC_GET_REG(STRINGIFY(NAME) TABS, TMC_GET_##NAME)
+    _TMC_GET_REG("\t", TMC_AXIS_CODES);
+    TMC_GET_REG(GCONF, "\t\t");
+    TMC_GET_REG(IHOLD_IRUN, "\t");
+    TMC_GET_REG(GSTAT, "\t\t");
+    TMC_GET_REG(IOIN, "\t\t");
+    TMC_GET_REG(TPOWERDOWN, "\t");
+    TMC_GET_REG(TSTEP, "\t\t");
+    TMC_GET_REG(TPWMTHRS, "\t");
+    TMC_GET_REG(TCOOLTHRS, "\t");
+    TMC_GET_REG(THIGH, "\t\t");
+    TMC_GET_REG(CHOPCONF, "\t");
+    TMC_GET_REG(COOLCONF, "\t");
+    TMC_GET_REG(PWMCONF, "\t");
+    TMC_GET_REG(PWM_SCALE, "\t");
+    TMC_GET_REG(DRV_STATUS, "\t");
+  }
+
+#endif // TMC_DEBUG
+
+#if USE_SENSORLESS
+
+  bool tmc_enable_stallguard(TMC2130Stepper &st) {
+    const bool stealthchop_was_enabled = st.en_pwm_mode();
+
+    st.TCOOLTHRS(0xFFFFF);
+    st.en_pwm_mode(false);
+    st.diag1_stall(true);
+
+    return stealthchop_was_enabled;
+  }
+  void tmc_disable_stallguard(TMC2130Stepper &st, const bool restore_stealth) {
+    st.TCOOLTHRS(0);
+    st.en_pwm_mode(restore_stealth);
+    st.diag1_stall(false);
+  }
+
+  bool tmc_enable_stallguard(TMC2209Stepper &st) {
+    const bool stealthchop_was_enabled = !st.en_spreadCycle();
+
+    st.TCOOLTHRS(0xFFFFF);
+    st.en_spreadCycle(false);
+    return stealthchop_was_enabled;
+  }
+  void tmc_disable_stallguard(TMC2209Stepper &st, const bool restore_stealth) {
+    st.en_spreadCycle(!restore_stealth);
+    st.TCOOLTHRS(0);
+  }
+
+  bool tmc_enable_stallguard(TMC2660Stepper) {
+    // TODO
+    return false;
+  }
+  void tmc_disable_stallguard(TMC2660Stepper, const bool) {};
+
+#endif // USE_SENSORLESS
+
+#if HAS_TMC_SPI
+  #define SET_CS_PIN(st) OUT_WRITE(st##_CS_PIN, HIGH)
+  void tmc_init_cs_pins() {
+    #if AXIS_HAS_SPI(X)
+      SET_CS_PIN(X);
+    #endif
+    #if AXIS_HAS_SPI(Y)
+      SET_CS_PIN(Y);
+    #endif
+    #if AXIS_HAS_SPI(Z)
+      SET_CS_PIN(Z);
+    #endif
+    #if AXIS_HAS_SPI(X2)
+      SET_CS_PIN(X2);
+    #endif
+    #if AXIS_HAS_SPI(Y2)
+      SET_CS_PIN(Y2);
+    #endif
+    #if AXIS_HAS_SPI(Z2)
+      SET_CS_PIN(Z2);
+    #endif
+    #if AXIS_HAS_SPI(Z3)
+      SET_CS_PIN(Z3);
+    #endif
+    #if AXIS_HAS_SPI(Z4)
+      SET_CS_PIN(Z4);
+    #endif
+    #if AXIS_HAS_SPI(E0)
+      SET_CS_PIN(E0);
+    #endif
+    #if AXIS_HAS_SPI(E1)
+      SET_CS_PIN(E1);
+    #endif
+    #if AXIS_HAS_SPI(E2)
+      SET_CS_PIN(E2);
+    #endif
+    #if AXIS_HAS_SPI(E3)
+      SET_CS_PIN(E3);
+    #endif
+    #if AXIS_HAS_SPI(E4)
+      SET_CS_PIN(E4);
+    #endif
+    #if AXIS_HAS_SPI(E5)
+      SET_CS_PIN(E5);
+    #endif
+    #if AXIS_HAS_SPI(E6)
+      SET_CS_PIN(E6);
+    #endif
+    #if AXIS_HAS_SPI(E7)
+      SET_CS_PIN(E7);
+    #endif
+  }
+#endif // HAS_TMC_SPI
+
+template<typename TMC>
+static bool test_connection(TMC &st) {
+  SERIAL_ECHOPGM("Testing ");
+  st.printLabel();
+  SERIAL_ECHOPGM(" connection... ");
+  const uint8_t test_result = st.test_connection();
+
+  if (test_result > 0) SERIAL_ECHOPGM("Error: All ");
+
+  const char *stat;
+  switch (test_result) {
+    default:
+    case 0: stat = PSTR("OK"); break;
+    case 1: stat = PSTR("HIGH"); break;
+    case 2: stat = PSTR("LOW"); break;
+  }
+  serialprintPGM(stat);
+  SERIAL_EOL();
+
+  return test_result;
+}
+
+void test_tmc_connection(const bool test_x, const bool test_y, const bool test_z, const bool test_e) {
+  uint8_t axis_connection = 0;
+
+  if (test_x) {
+    #if AXIS_IS_TMC(X)
+      axis_connection += test_connection(stepperX);
+    #endif
+    #if AXIS_IS_TMC(X2)
+      axis_connection += test_connection(stepperX2);
+    #endif
+  }
+
+  if (test_y) {
+    #if AXIS_IS_TMC(Y)
+      axis_connection += test_connection(stepperY);
+    #endif
+    #if AXIS_IS_TMC(Y2)
+      axis_connection += test_connection(stepperY2);
+    #endif
+  }
+
+  if (test_z) {
+    #if AXIS_IS_TMC(Z)
+      axis_connection += test_connection(stepperZ);
+    #endif
+    #if AXIS_IS_TMC(Z2)
+      axis_connection += test_connection(stepperZ2);
+    #endif
+    #if AXIS_IS_TMC(Z3)
+      axis_connection += test_connection(stepperZ3);
+    #endif
+    #if AXIS_IS_TMC(Z4)
+      axis_connection += test_connection(stepperZ4);
+    #endif
+  }
+
+  if (test_e) {
+    #if AXIS_IS_TMC(E0)
+      axis_connection += test_connection(stepperE0);
+    #endif
+    #if AXIS_IS_TMC(E1)
+      axis_connection += test_connection(stepperE1);
+    #endif
+    #if AXIS_IS_TMC(E2)
+      axis_connection += test_connection(stepperE2);
+    #endif
+    #if AXIS_IS_TMC(E3)
+      axis_connection += test_connection(stepperE3);
+    #endif
+    #if AXIS_IS_TMC(E4)
+      axis_connection += test_connection(stepperE4);
+    #endif
+    #if AXIS_IS_TMC(E5)
+      axis_connection += test_connection(stepperE5);
+    #endif
+    #if AXIS_IS_TMC(E6)
+      axis_connection += test_connection(stepperE6);
+    #endif
+    #if AXIS_IS_TMC(E7)
+      axis_connection += test_connection(stepperE7);
+    #endif
+  }
+
+  if (axis_connection) LCD_MESSAGEPGM(MSG_ERROR_TMC);
+}
+
+#endif // HAS_TRINAMIC_CONFIG
diff --git a/Marlin/src/feature/tmc_util.h b/Marlin/src/feature/tmc_util.h
new file mode 100644
index 0000000..b21b89f
--- /dev/null
+++ b/Marlin/src/feature/tmc_util.h
@@ -0,0 +1,401 @@
+/**
+ * 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"
+#include "../lcd/marlinui.h"
+
+#if HAS_TRINAMIC_CONFIG
+
+#include <TMCStepper.h>
+#include "../module/planner.h"
+
+#define CHOPPER_DEFAULT_12V  { 3, -1, 1 }
+#define CHOPPER_DEFAULT_19V  { 4,  1, 1 }
+#define CHOPPER_DEFAULT_24V  { 4,  2, 1 }
+#define CHOPPER_DEFAULT_36V  { 5,  2, 4 }
+#define CHOPPER_PRUSAMK3_24V { 3, -2, 6 }
+#define CHOPPER_MARLIN_119   { 5,  2, 3 }
+#define CHOPPER_09STEP_24V   { 3, -1, 5 }
+
+#if ENABLED(MONITOR_DRIVER_STATUS) && !defined(MONITOR_DRIVER_STATUS_INTERVAL_MS)
+  #define MONITOR_DRIVER_STATUS_INTERVAL_MS 500U
+#endif
+
+constexpr uint16_t _tmc_thrs(const uint16_t msteps, const uint32_t thrs, const uint32_t spmm) {
+  return 12650000UL * msteps / (256 * thrs * spmm);
+}
+
+template<char AXIS_LETTER, char DRIVER_ID>
+class TMCStorage {
+  protected:
+    // Only a child class has access to constructor => Don't create on its own! "Poor man's abstract class"
+    TMCStorage() {}
+
+  public:
+    uint16_t val_mA = 0;
+
+    #if ENABLED(MONITOR_DRIVER_STATUS)
+      uint8_t otpw_count = 0,
+              error_count = 0;
+      bool flag_otpw = false;
+      inline bool getOTPW() { return flag_otpw; }
+      inline void clear_otpw() { flag_otpw = 0; }
+    #endif
+
+    inline uint16_t getMilliamps() { return val_mA; }
+
+    inline void printLabel() {
+      SERIAL_CHAR(AXIS_LETTER);
+      if (DRIVER_ID > '0') SERIAL_CHAR(DRIVER_ID);
+    }
+
+    struct {
+      TERN_(HAS_STEALTHCHOP, bool stealthChop_enabled = false);
+      TERN_(HYBRID_THRESHOLD, uint8_t hybrid_thrs = 0);
+      TERN_(USE_SENSORLESS, int16_t homing_thrs = 0);
+    } stored;
+};
+
+template<class TMC, char AXIS_LETTER, char DRIVER_ID, AxisEnum AXIS_ID>
+class TMCMarlin : public TMC, public TMCStorage<AXIS_LETTER, DRIVER_ID> {
+  public:
+    TMCMarlin(const uint16_t cs_pin, const float RS) :
+      TMC(cs_pin, RS)
+      {}
+    TMCMarlin(const uint16_t cs_pin, const float RS, const uint8_t axis_chain_index) :
+      TMC(cs_pin, RS, axis_chain_index)
+      {}
+    TMCMarlin(const uint16_t CS, const float RS, const uint16_t pinMOSI, const uint16_t pinMISO, const uint16_t pinSCK) :
+      TMC(CS, RS, pinMOSI, pinMISO, pinSCK)
+      {}
+    TMCMarlin(const uint16_t CS, const float RS, const uint16_t pinMOSI, const uint16_t pinMISO, const uint16_t pinSCK, const uint8_t axis_chain_index) :
+      TMC(CS, RS, pinMOSI, pinMISO, pinSCK,  axis_chain_index)
+      {}
+    inline uint16_t rms_current() { return TMC::rms_current(); }
+    inline void rms_current(uint16_t mA) {
+      this->val_mA = mA;
+      TMC::rms_current(mA);
+    }
+    inline void rms_current(const uint16_t mA, const float mult) {
+      this->val_mA = mA;
+      TMC::rms_current(mA, mult);
+    }
+    inline uint16_t get_microstep_counter() { return TMC::MSCNT(); }
+
+    #if HAS_STEALTHCHOP
+      inline bool get_stealthChop()                { return this->en_pwm_mode(); }
+      inline bool get_stored_stealthChop()         { return this->stored.stealthChop_enabled; }
+      inline void refresh_stepping_mode()          { this->en_pwm_mode(this->stored.stealthChop_enabled); }
+      inline void set_stealthChop(const bool stch) { this->stored.stealthChop_enabled = stch; refresh_stepping_mode(); }
+      inline bool toggle_stepping_mode()           { set_stealthChop(!this->stored.stealthChop_enabled); return get_stealthChop(); }
+    #endif
+
+    #if ENABLED(HYBRID_THRESHOLD)
+      uint32_t get_pwm_thrs() {
+        return _tmc_thrs(this->microsteps(), this->TPWMTHRS(), planner.settings.axis_steps_per_mm[AXIS_ID]);
+      }
+      void set_pwm_thrs(const uint32_t thrs) {
+        TMC::TPWMTHRS(_tmc_thrs(this->microsteps(), thrs, planner.settings.axis_steps_per_mm[AXIS_ID]));
+        TERN_(HAS_LCD_MENU, this->stored.hybrid_thrs = thrs);
+      }
+    #endif
+
+    #if USE_SENSORLESS
+      inline int16_t homing_threshold() { return TMC::sgt(); }
+      void homing_threshold(int16_t sgt_val) {
+        sgt_val = (int16_t)constrain(sgt_val, sgt_min, sgt_max);
+        TMC::sgt(sgt_val);
+        TERN_(HAS_LCD_MENU, this->stored.homing_thrs = sgt_val);
+      }
+      #if ENABLED(SPI_ENDSTOPS)
+        bool test_stall_status();
+      #endif
+    #endif
+
+    #if HAS_LCD_MENU
+      inline void refresh_stepper_current() { rms_current(this->val_mA); }
+
+      #if ENABLED(HYBRID_THRESHOLD)
+        inline void refresh_hybrid_thrs() { set_pwm_thrs(this->stored.hybrid_thrs); }
+      #endif
+      #if USE_SENSORLESS
+        inline void refresh_homing_thrs() { homing_threshold(this->stored.homing_thrs); }
+      #endif
+    #endif
+
+    static constexpr int8_t sgt_min = -64,
+                            sgt_max =  63;
+};
+
+template<char AXIS_LETTER, char DRIVER_ID, AxisEnum AXIS_ID>
+class TMCMarlin<TMC2208Stepper, AXIS_LETTER, DRIVER_ID, AXIS_ID> : public TMC2208Stepper, public TMCStorage<AXIS_LETTER, DRIVER_ID> {
+  public:
+    TMCMarlin(Stream * SerialPort, const float RS, const uint8_t) :
+      TMC2208Stepper(SerialPort, RS)
+      {}
+    TMCMarlin(Stream * SerialPort, const float RS, uint8_t addr, const uint16_t mul_pin1, const uint16_t mul_pin2) :
+      TMC2208Stepper(SerialPort, RS, addr, mul_pin1, mul_pin2)
+     {}
+    TMCMarlin(const uint16_t RX, const uint16_t TX, const float RS, const uint8_t) :
+      TMC2208Stepper(RX, TX, RS)
+      {}
+
+    uint16_t rms_current() { return TMC2208Stepper::rms_current(); }
+    inline void rms_current(const uint16_t mA) {
+      this->val_mA = mA;
+      TMC2208Stepper::rms_current(mA);
+    }
+    inline void rms_current(const uint16_t mA, const float mult) {
+      this->val_mA = mA;
+      TMC2208Stepper::rms_current(mA, mult);
+    }
+    inline uint16_t get_microstep_counter() { return TMC2208Stepper::MSCNT(); }
+
+    #if HAS_STEALTHCHOP
+      inline bool get_stealthChop()                { return !this->en_spreadCycle(); }
+      inline bool get_stored_stealthChop()         { return this->stored.stealthChop_enabled; }
+      inline void refresh_stepping_mode()          { this->en_spreadCycle(!this->stored.stealthChop_enabled); }
+      inline void set_stealthChop(const bool stch) { this->stored.stealthChop_enabled = stch; refresh_stepping_mode(); }
+      inline bool toggle_stepping_mode()           { set_stealthChop(!this->stored.stealthChop_enabled); return get_stealthChop(); }
+    #endif
+
+    #if ENABLED(HYBRID_THRESHOLD)
+      uint32_t get_pwm_thrs() {
+        return _tmc_thrs(this->microsteps(), this->TPWMTHRS(), planner.settings.axis_steps_per_mm[AXIS_ID]);
+      }
+      void set_pwm_thrs(const uint32_t thrs) {
+        TMC2208Stepper::TPWMTHRS(_tmc_thrs(this->microsteps(), thrs, planner.settings.axis_steps_per_mm[AXIS_ID]));
+        TERN_(HAS_LCD_MENU, this->stored.hybrid_thrs = thrs);
+      }
+    #endif
+
+    #if HAS_LCD_MENU
+      inline void refresh_stepper_current() { rms_current(this->val_mA); }
+
+      #if ENABLED(HYBRID_THRESHOLD)
+        inline void refresh_hybrid_thrs() { set_pwm_thrs(this->stored.hybrid_thrs); }
+      #endif
+    #endif
+};
+
+template<char AXIS_LETTER, char DRIVER_ID, AxisEnum AXIS_ID>
+class TMCMarlin<TMC2209Stepper, AXIS_LETTER, DRIVER_ID, AXIS_ID> : public TMC2209Stepper, public TMCStorage<AXIS_LETTER, DRIVER_ID> {
+  public:
+    TMCMarlin(Stream * SerialPort, const float RS, const uint8_t addr) :
+      TMC2209Stepper(SerialPort, RS, addr)
+      {}
+    TMCMarlin(const uint16_t RX, const uint16_t TX, const float RS, const uint8_t addr) :
+      TMC2209Stepper(RX, TX, RS, addr)
+      {}
+    uint8_t get_address() { return slave_address; }
+    uint16_t rms_current() { return TMC2209Stepper::rms_current(); }
+    inline void rms_current(const uint16_t mA) {
+      this->val_mA = mA;
+      TMC2209Stepper::rms_current(mA);
+    }
+    inline void rms_current(const uint16_t mA, const float mult) {
+      this->val_mA = mA;
+      TMC2209Stepper::rms_current(mA, mult);
+    }
+    inline uint16_t get_microstep_counter() { return TMC2209Stepper::MSCNT(); }
+
+    #if HAS_STEALTHCHOP
+      inline bool get_stealthChop()                { return !this->en_spreadCycle(); }
+      inline bool get_stored_stealthChop()         { return this->stored.stealthChop_enabled; }
+      inline void refresh_stepping_mode()          { this->en_spreadCycle(!this->stored.stealthChop_enabled); }
+      inline void set_stealthChop(const bool stch) { this->stored.stealthChop_enabled = stch; refresh_stepping_mode(); }
+      inline bool toggle_stepping_mode()           { set_stealthChop(!this->stored.stealthChop_enabled); return get_stealthChop(); }
+    #endif
+
+    #if ENABLED(HYBRID_THRESHOLD)
+      uint32_t get_pwm_thrs() {
+        return _tmc_thrs(this->microsteps(), this->TPWMTHRS(), planner.settings.axis_steps_per_mm[AXIS_ID]);
+      }
+      void set_pwm_thrs(const uint32_t thrs) {
+        TMC2209Stepper::TPWMTHRS(_tmc_thrs(this->microsteps(), thrs, planner.settings.axis_steps_per_mm[AXIS_ID]));
+        TERN_(HAS_LCD_MENU, this->stored.hybrid_thrs = thrs);
+      }
+    #endif
+    #if USE_SENSORLESS
+      inline int16_t homing_threshold() { return TMC2209Stepper::SGTHRS(); }
+      void homing_threshold(int16_t sgt_val) {
+        sgt_val = (int16_t)constrain(sgt_val, sgt_min, sgt_max);
+        TMC2209Stepper::SGTHRS(sgt_val);
+        TERN_(HAS_LCD_MENU, this->stored.homing_thrs = sgt_val);
+      }
+    #endif
+
+    #if HAS_LCD_MENU
+      inline void refresh_stepper_current() { rms_current(this->val_mA); }
+
+      #if ENABLED(HYBRID_THRESHOLD)
+        inline void refresh_hybrid_thrs() { set_pwm_thrs(this->stored.hybrid_thrs); }
+      #endif
+      #if USE_SENSORLESS
+        inline void refresh_homing_thrs() { homing_threshold(this->stored.homing_thrs); }
+      #endif
+    #endif
+
+    static constexpr uint8_t sgt_min = 0,
+                             sgt_max = 255;
+};
+
+template<char AXIS_LETTER, char DRIVER_ID, AxisEnum AXIS_ID>
+class TMCMarlin<TMC2660Stepper, AXIS_LETTER, DRIVER_ID, AXIS_ID> : public TMC2660Stepper, public TMCStorage<AXIS_LETTER, DRIVER_ID> {
+  public:
+    TMCMarlin(const uint16_t cs_pin, const float RS, const uint8_t) :
+      TMC2660Stepper(cs_pin, RS)
+      {}
+    TMCMarlin(const uint16_t CS, const float RS, const uint16_t pinMOSI, const uint16_t pinMISO, const uint16_t pinSCK, const uint8_t) :
+      TMC2660Stepper(CS, RS, pinMOSI, pinMISO, pinSCK)
+      {}
+    inline uint16_t rms_current() { return TMC2660Stepper::rms_current(); }
+    inline void rms_current(const uint16_t mA) {
+      this->val_mA = mA;
+      TMC2660Stepper::rms_current(mA);
+    }
+    inline uint16_t get_microstep_counter() { return TMC2660Stepper::mstep(); }
+
+    #if USE_SENSORLESS
+      inline int16_t homing_threshold() { return TMC2660Stepper::sgt(); }
+      void homing_threshold(int16_t sgt_val) {
+        sgt_val = (int16_t)constrain(sgt_val, sgt_min, sgt_max);
+        TMC2660Stepper::sgt(sgt_val);
+        TERN_(HAS_LCD_MENU, this->stored.homing_thrs = sgt_val);
+      }
+    #endif
+
+    #if HAS_LCD_MENU
+      inline void refresh_stepper_current() { rms_current(this->val_mA); }
+
+      #if USE_SENSORLESS
+        inline void refresh_homing_thrs() { homing_threshold(this->stored.homing_thrs); }
+      #endif
+    #endif
+
+    static constexpr int8_t sgt_min = -64,
+                            sgt_max =  63;
+};
+
+template<typename TMC>
+void tmc_print_current(TMC &st) {
+  st.printLabel();
+  SERIAL_ECHOLNPAIR(" driver current: ", st.getMilliamps());
+}
+
+#if ENABLED(MONITOR_DRIVER_STATUS)
+  template<typename TMC>
+  void tmc_report_otpw(TMC &st) {
+    st.printLabel();
+    SERIAL_ECHOPGM(" temperature prewarn triggered: ");
+    serialprint_truefalse(st.getOTPW());
+    SERIAL_EOL();
+  }
+  template<typename TMC>
+  void tmc_clear_otpw(TMC &st) {
+    st.clear_otpw();
+    st.printLabel();
+    SERIAL_ECHOLNPGM(" prewarn flag cleared");
+  }
+#endif
+#if ENABLED(HYBRID_THRESHOLD)
+  template<typename TMC>
+  void tmc_print_pwmthrs(TMC &st) {
+    st.printLabel();
+    SERIAL_ECHOLNPAIR(" stealthChop max speed: ", st.get_pwm_thrs());
+  }
+#endif
+#if USE_SENSORLESS
+  template<typename TMC>
+  void tmc_print_sgt(TMC &st) {
+    st.printLabel();
+    SERIAL_ECHOPGM(" homing sensitivity: ");
+    SERIAL_PRINTLN(st.homing_threshold(), DEC);
+  }
+#endif
+
+void monitor_tmc_drivers();
+void test_tmc_connection(const bool test_x, const bool test_y, const bool test_z, const bool test_e);
+
+#if ENABLED(TMC_DEBUG)
+  #if ENABLED(MONITOR_DRIVER_STATUS)
+    void tmc_set_report_interval(const uint16_t update_interval);
+  #endif
+  void tmc_report_all(const bool print_x, const bool print_y, const bool print_z, const bool print_e);
+  void tmc_get_registers(const bool print_x, const bool print_y, const bool print_z, const bool print_e);
+#endif
+
+/**
+ * TMC2130-specific sensorless homing using stallGuard2.
+ * stallGuard2 only works when in spreadCycle mode.
+ * spreadCycle and stealthChop are mutually-exclusive.
+ *
+ * Defined here because of limitations with templates and headers.
+ */
+#if USE_SENSORLESS
+
+  // Track enabled status of stealthChop and only re-enable where applicable
+  struct sensorless_t { bool x, y, z, x2, y2, z2, z3, z4; };
+
+  #if ENABLED(IMPROVE_HOMING_RELIABILITY)
+    extern millis_t sg_guard_period;
+    constexpr uint16_t default_sg_guard_duration = 400;
+
+    struct slow_homing_t {
+      xy_ulong_t acceleration;
+      TERN_(HAS_CLASSIC_JERK, xy_float_t jerk_xy);
+    };
+  #endif
+
+  bool tmc_enable_stallguard(TMC2130Stepper &st);
+  void tmc_disable_stallguard(TMC2130Stepper &st, const bool restore_stealth);
+
+  bool tmc_enable_stallguard(TMC2209Stepper &st);
+  void tmc_disable_stallguard(TMC2209Stepper &st, const bool restore_stealth);
+
+  bool tmc_enable_stallguard(TMC2660Stepper);
+  void tmc_disable_stallguard(TMC2660Stepper, const bool);
+
+  #if ENABLED(SPI_ENDSTOPS)
+
+    template<class TMC, char AXIS_LETTER, char DRIVER_ID, AxisEnum AXIS_ID>
+    bool TMCMarlin<TMC, AXIS_LETTER, DRIVER_ID, AXIS_ID>::test_stall_status() {
+      this->switchCSpin(LOW);
+
+      // read stallGuard flag from TMC library, will handle HW and SW SPI
+      TMC2130_n::DRV_STATUS_t drv_status{0};
+      drv_status.sr = this->DRV_STATUS();
+
+      this->switchCSpin(HIGH);
+
+      return drv_status.stallGuard;
+    }
+  #endif // SPI_ENDSTOPS
+
+#endif // USE_SENSORLESS
+
+#if HAS_TMC_SPI
+  void tmc_init_cs_pins();
+#endif
+
+#endif // HAS_TRINAMIC_CONFIG
diff --git a/Marlin/src/feature/tramming.cpp b/Marlin/src/feature/tramming.cpp
new file mode 100644
index 0000000..d03f0cf
--- /dev/null
+++ b/Marlin/src/feature/tramming.cpp
@@ -0,0 +1,69 @@
+/**
+ * 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"
+
+#if ENABLED(ASSISTED_TRAMMING)
+
+#include "tramming.h"
+
+#define DEBUG_OUT ENABLED(DEBUG_LEVELING_FEATURE)
+#include "../core/debug_out.h"
+
+PGMSTR(point_name_1, TRAMMING_POINT_NAME_1);
+PGMSTR(point_name_2, TRAMMING_POINT_NAME_2);
+PGMSTR(point_name_3, TRAMMING_POINT_NAME_3);
+#ifdef TRAMMING_POINT_NAME_4
+  PGMSTR(point_name_4, TRAMMING_POINT_NAME_4);
+  #ifdef TRAMMING_POINT_NAME_5
+    PGMSTR(point_name_5, TRAMMING_POINT_NAME_5);
+    #ifdef TRAMMING_POINT_NAME_6
+      PGMSTR(point_name_6, TRAMMING_POINT_NAME_6);
+    #endif
+  #endif
+#endif
+
+PGM_P const tramming_point_name[] PROGMEM = {
+  point_name_1, point_name_2, point_name_3
+  #ifdef TRAMMING_POINT_NAME_4
+    , point_name_4
+    #ifdef TRAMMING_POINT_NAME_5
+      , point_name_5
+      #ifdef TRAMMING_POINT_NAME_6
+        , point_name_6
+      #endif
+    #endif
+  #endif
+};
+
+#ifdef ASSISTED_TRAMMING_WAIT_POSITION
+
+  // Move to the defined wait position
+  void move_to_tramming_wait_pos() {
+    constexpr xyz_pos_t wait_pos = ASSISTED_TRAMMING_WAIT_POSITION;
+    if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Moving away");
+    do_blocking_move_to(wait_pos, XY_PROBE_FEEDRATE_MM_S);
+  }
+
+#endif
+
+#endif // ASSISTED_TRAMMING
diff --git a/Marlin/src/feature/tramming.h b/Marlin/src/feature/tramming.h
new file mode 100644
index 0000000..eb27fe8
--- /dev/null
+++ b/Marlin/src/feature/tramming.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
+
+#include "../inc/MarlinConfig.h"
+#include "../module/probe.h"
+
+#if !WITHIN(TRAMMING_SCREW_THREAD, 30, 51) || TRAMMING_SCREW_THREAD % 10 > 1
+  #error "TRAMMING_SCREW_THREAD must be equal to 30, 31, 40, 41, 50, or 51."
+#endif
+
+constexpr xy_pos_t screws_tilt_adjust_pos[] = TRAMMING_POINT_XY;
+
+#define G35_PROBE_COUNT COUNT(screws_tilt_adjust_pos)
+static_assert(WITHIN(G35_PROBE_COUNT, 3, 6), "TRAMMING_POINT_XY requires between 3 and 6 XY positions.");
+
+#define VALIDATE_TRAMMING_POINT(N) static_assert(N >= G35_PROBE_COUNT || Probe::build_time::can_reach(screws_tilt_adjust_pos[N]), \
+  "TRAMMING_POINT_XY point " STRINGIFY(N) " is not reachable with the default NOZZLE_TO_PROBE offset and PROBING_MARGIN.")
+VALIDATE_TRAMMING_POINT(0); VALIDATE_TRAMMING_POINT(1); VALIDATE_TRAMMING_POINT(2); VALIDATE_TRAMMING_POINT(3); VALIDATE_TRAMMING_POINT(4); VALIDATE_TRAMMING_POINT(5);
+
+extern const char point_name_1[], point_name_2[], point_name_3[]
+  #ifdef TRAMMING_POINT_NAME_4
+    , point_name_4[]
+    #ifdef TRAMMING_POINT_NAME_5
+      , point_name_5[]
+      #ifdef TRAMMING_POINT_NAME_6
+        , point_name_6[]
+      #endif
+    #endif
+  #endif
+;
+
+#define _NR_TRAM_NAMES 2
+#ifdef TRAMMING_POINT_NAME_3
+  #undef _NR_TRAM_NAMES
+  #define _NR_TRAM_NAMES 3
+  #ifdef TRAMMING_POINT_NAME_4
+    #undef _NR_TRAM_NAMES
+    #define _NR_TRAM_NAMES 4
+    #ifdef TRAMMING_POINT_NAME_5
+      #undef _NR_TRAM_NAMES
+      #define _NR_TRAM_NAMES 5
+      #ifdef TRAMMING_POINT_NAME_6
+        #undef _NR_TRAM_NAMES
+        #define _NR_TRAM_NAMES 6
+      #endif
+    #endif
+  #endif
+#endif
+static_assert(_NR_TRAM_NAMES >= G35_PROBE_COUNT, "Define enough TRAMMING_POINT_NAME_s for all TRAMMING_POINT_XY entries.");
+#undef _NR_TRAM_NAMES
+
+extern PGM_P const tramming_point_name[];
+
+#ifdef ASSISTED_TRAMMING_WAIT_POSITION
+  void move_to_tramming_wait_pos();
+#else
+  inline void move_to_tramming_wait_pos() {}
+#endif
diff --git a/Marlin/src/feature/twibus.cpp b/Marlin/src/feature/twibus.cpp
new file mode 100644
index 0000000..855a318
--- /dev/null
+++ b/Marlin/src/feature/twibus.cpp
@@ -0,0 +1,190 @@
+/**
+ * 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 ENABLED(EXPERIMENTAL_I2CBUS)
+
+#include "twibus.h"
+
+#include <Wire.h>
+
+TWIBus i2c;
+
+TWIBus::TWIBus() {
+  #if I2C_SLAVE_ADDRESS == 0
+    Wire.begin();                  // No address joins the BUS as the master
+  #else
+    Wire.begin(I2C_SLAVE_ADDRESS); // Join the bus as a slave
+  #endif
+  reset();
+}
+
+void TWIBus::reset() {
+  buffer_s = 0;
+  buffer[0] = 0x00;
+}
+
+void TWIBus::address(const uint8_t adr) {
+  if (!WITHIN(adr, 8, 127)) {
+    SERIAL_ECHO_MSG("Bad I2C address (8-127)");
+  }
+
+  addr = adr;
+
+  debug(PSTR("address"), adr);
+}
+
+void TWIBus::addbyte(const char c) {
+  if (buffer_s >= COUNT(buffer)) return;
+  buffer[buffer_s++] = c;
+  debug(PSTR("addbyte"), c);
+}
+
+void TWIBus::addbytes(char src[], uint8_t bytes) {
+  debug(PSTR("addbytes"), bytes);
+  while (bytes--) addbyte(*src++);
+}
+
+void TWIBus::addstring(char str[]) {
+  debug(PSTR("addstring"), str);
+  while (char c = *str++) addbyte(c);
+}
+
+void TWIBus::send() {
+  debug(PSTR("send"), addr);
+
+  Wire.beginTransmission(I2C_ADDRESS(addr));
+  Wire.write(buffer, buffer_s);
+  Wire.endTransmission();
+
+  reset();
+}
+
+// static
+void TWIBus::echoprefix(uint8_t bytes, const char pref[], uint8_t adr) {
+  SERIAL_ECHO_START();
+  serialprintPGM(pref);
+  SERIAL_ECHOPAIR(": from:", adr, " bytes:", bytes, " data:");
+}
+
+// static
+void TWIBus::echodata(uint8_t bytes, const char pref[], uint8_t adr) {
+  echoprefix(bytes, pref, adr);
+  while (bytes-- && Wire.available()) SERIAL_CHAR(Wire.read());
+  SERIAL_EOL();
+}
+
+void TWIBus::echobuffer(const char pref[], uint8_t adr) {
+  echoprefix(buffer_s, pref, adr);
+  LOOP_L_N(i, buffer_s) SERIAL_CHAR(buffer[i]);
+  SERIAL_EOL();
+}
+
+bool TWIBus::request(const uint8_t bytes) {
+  if (!addr) return false;
+
+  debug(PSTR("request"), bytes);
+
+  // requestFrom() is a blocking function
+  if (Wire.requestFrom(I2C_ADDRESS(addr), bytes) == 0) {
+    debug("request fail", I2C_ADDRESS(addr));
+    return false;
+  }
+
+  return true;
+}
+
+void TWIBus::relay(const uint8_t bytes) {
+  debug(PSTR("relay"), bytes);
+
+  if (request(bytes))
+    echodata(bytes, PSTR("i2c-reply"), addr);
+}
+
+uint8_t TWIBus::capture(char *dst, const uint8_t bytes) {
+  reset();
+  uint8_t count = 0;
+  while (count < bytes && Wire.available())
+    dst[count++] = Wire.read();
+
+  debug(PSTR("capture"), count);
+
+  return count;
+}
+
+// static
+void TWIBus::flush() {
+  while (Wire.available()) Wire.read();
+}
+
+#if I2C_SLAVE_ADDRESS > 0
+
+  void TWIBus::receive(uint8_t bytes) {
+    debug(PSTR("receive"), bytes);
+    echodata(bytes, PSTR("i2c-receive"), 0);
+  }
+
+  void TWIBus::reply(char str[]/*=nullptr*/) {
+    debug(PSTR("reply"), str);
+
+    if (str) {
+      reset();
+      addstring(str);
+    }
+
+    Wire.write(buffer, buffer_s);
+
+    reset();
+  }
+
+  void i2c_on_receive(int bytes) { // just echo all bytes received to serial
+    i2c.receive(bytes);
+  }
+
+  void i2c_on_request() {          // just send dummy data for now
+    i2c.reply("Hello World!\n");
+  }
+
+#endif
+
+#if ENABLED(DEBUG_TWIBUS)
+
+  // static
+  void TWIBus::prefix(const char func[]) {
+    SERIAL_ECHOPGM("TWIBus::");
+    serialprintPGM(func);
+    SERIAL_ECHOPGM(": ");
+  }
+  void TWIBus::debug(const char func[], uint32_t adr) {
+    if (DEBUGGING(INFO)) { prefix(func); SERIAL_ECHOLN(adr); }
+  }
+  void TWIBus::debug(const char func[], char c) {
+    if (DEBUGGING(INFO)) { prefix(func); SERIAL_ECHOLN(c); }
+  }
+  void TWIBus::debug(const char func[], char str[]) {
+    if (DEBUGGING(INFO)) { prefix(func); SERIAL_ECHOLN(str); }
+  }
+
+#endif
+
+#endif // EXPERIMENTAL_I2CBUS
diff --git a/Marlin/src/feature/twibus.h b/Marlin/src/feature/twibus.h
new file mode 100644
index 0000000..5939153
--- /dev/null
+++ b/Marlin/src/feature/twibus.h
@@ -0,0 +1,253 @@
+/**
+ * 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 "../core/macros.h"
+
+#include <Wire.h>
+
+// Print debug messages with M111 S2 (Uses 236 bytes of PROGMEM)
+//#define DEBUG_TWIBUS
+
+typedef void (*twiReceiveFunc_t)(int bytes);
+typedef void (*twiRequestFunc_t)();
+
+/**
+ * For a light i2c protocol that runs on two boards running Marlin see:
+ * See https://github.com/MarlinFirmware/Marlin/issues/4776#issuecomment-246262879
+ */
+#if I2C_SLAVE_ADDRESS > 0
+
+  void i2c_on_receive(int bytes); // Demo i2c onReceive handler
+  void i2c_on_request();          // Demo i2c onRequest handler
+
+#endif
+
+#define TWIBUS_BUFFER_SIZE 32
+
+/**
+ * TWIBUS class
+ *
+ * This class implements a wrapper around the two wire (I2C) bus, allowing
+ * Marlin to send and request data from any slave device on the bus.
+ *
+ * The two main consumers of this class are M260 and M261. M260 provides a way
+ * to send an I2C packet to a device (no repeated starts) by caching up to 32
+ * bytes in a buffer and then sending the buffer.
+ * M261 requests data from a device. The received data is relayed to serial out
+ * for the host to interpret.
+ *
+ *  For more information see
+ *    - https://marlinfw.org/docs/gcode/M260.html
+ *    - https://marlinfw.org/docs/gcode/M261.html
+ */
+class TWIBus {
+  private:
+    /**
+     * @brief Number of bytes on buffer
+     * @description Number of bytes in the buffer waiting to be flushed to the bus
+     */
+    uint8_t buffer_s = 0;
+
+    /**
+     * @brief Internal buffer
+     * @details A fixed buffer. TWI commands can be no longer than this.
+     */
+    uint8_t buffer[TWIBUS_BUFFER_SIZE];
+
+
+  public:
+    /**
+     * @brief Target device address
+     * @description The target device address. Persists until changed.
+     */
+    uint8_t addr = 0;
+
+    /**
+     * @brief Class constructor
+     * @details Initialize the TWI bus and clear the buffer
+     */
+    TWIBus();
+
+    /**
+     * @brief Reset the buffer
+     * @details Set the buffer to a known-empty state
+     */
+    void reset();
+
+    /**
+     * @brief Send the buffer data to the bus
+     * @details Flush the buffer to the target address
+     */
+    void send();
+
+    /**
+     * @brief Add one byte to the buffer
+     * @details Add a byte to the end of the buffer.
+     *          Silently fails if the buffer is full.
+     *
+     * @param c a data byte
+     */
+    void addbyte(const char c);
+
+    /**
+     * @brief Add some bytes to the buffer
+     * @details Add bytes to the end of the buffer.
+     *          Concatenates at the buffer size.
+     *
+     * @param src source data address
+     * @param bytes the number of bytes to add
+     */
+    void addbytes(char src[], uint8_t bytes);
+
+    /**
+     * @brief Add a null-terminated string to the buffer
+     * @details Add bytes to the end of the buffer up to a nul.
+     *          Concatenates at the buffer size.
+     *
+     * @param str source string address
+     */
+    void addstring(char str[]);
+
+    /**
+     * @brief Set the target slave address
+     * @details The target slave address for sending the full packet
+     *
+     * @param adr 7-bit integer address
+     */
+    void address(const uint8_t adr);
+
+    /**
+     * @brief Prefix for echo to serial
+     * @details Echo a label, length, address, and "data:"
+     *
+     * @param bytes the number of bytes to request
+     */
+    static void echoprefix(uint8_t bytes, const char prefix[], uint8_t adr);
+
+    /**
+     * @brief Echo data on the bus to serial
+     * @details Echo some number of bytes from the bus
+     *          to serial in a parser-friendly format.
+     *
+     * @param bytes the number of bytes to request
+     */
+    static void echodata(uint8_t bytes, const char prefix[], uint8_t adr);
+
+    /**
+     * @brief Echo data in the buffer to serial
+     * @details Echo the entire buffer to serial
+     *          to serial in a parser-friendly format.
+     *
+     * @param bytes the number of bytes to request
+     */
+    void echobuffer(const char prefix[], uint8_t adr);
+
+    /**
+     * @brief Request data from the slave device and wait.
+     * @details Request a number of bytes from a slave device.
+     *          Wait for the data to arrive, and return true
+     *          on success.
+     *
+     * @param bytes the number of bytes to request
+     * @return status of the request: true=success, false=fail
+     */
+    bool request(const uint8_t bytes);
+
+    /**
+     * @brief Capture data from the bus into the buffer.
+     * @details Capture data after a request has succeeded.
+     *
+     * @param bytes the number of bytes to request
+     * @return the number of bytes captured to the buffer
+     */
+    uint8_t capture(char *dst, const uint8_t bytes);
+
+    /**
+     * @brief Flush the i2c bus.
+     * @details Get all bytes on the bus and throw them away.
+     */
+    static void flush();
+
+    /**
+     * @brief Request data from the slave device, echo to serial.
+     * @details Request a number of bytes from a slave device and output
+     *          the returned data to serial in a parser-friendly format.
+     *
+     * @param bytes the number of bytes to request
+     */
+    void relay(const uint8_t bytes);
+
+    #if I2C_SLAVE_ADDRESS > 0
+
+      /**
+       * @brief Register a slave receive handler
+       * @details Set a handler to receive data addressed to us
+       *
+       * @param handler A function to handle receiving bytes
+       */
+      inline void onReceive(const twiReceiveFunc_t handler) { Wire.onReceive(handler); }
+
+      /**
+       * @brief Register a slave request handler
+       * @details Set a handler to send data requested from us
+       *
+       * @param handler A function to handle receiving bytes
+       */
+      inline void onRequest(const twiRequestFunc_t handler) { Wire.onRequest(handler); }
+
+      /**
+       * @brief Default handler to receive
+       * @details Receive bytes sent to our slave address
+       *          and simply echo them to serial.
+       */
+      void receive(uint8_t bytes);
+
+      /**
+       * @brief Send a reply to the bus
+       * @details Send the buffer and clear it.
+       *          If a string is passed, write it into the buffer first.
+       */
+      void reply(char str[]=nullptr);
+      inline void reply(const char str[]) { reply((char*)str); }
+
+    #endif
+
+    #if ENABLED(DEBUG_TWIBUS)
+      /**
+       * @brief Prints a debug message
+       * @details Prints a simple debug message "TWIBus::function: value"
+       */
+      static void prefix(const char func[]);
+      static void debug(const char func[], uint32_t adr);
+      static void debug(const char func[], char c);
+      static void debug(const char func[], char adr[]);
+      static inline void debug(const char func[], uint8_t v) { debug(func, (uint32_t)v); }
+    #else
+      static inline void debug(const char[], uint32_t) {}
+      static inline void debug(const char[], char) {}
+      static inline void debug(const char[], char[]) {}
+      static inline void debug(const char[], uint8_t) {}
+    #endif
+};
+
+extern TWIBus i2c;
diff --git a/Marlin/src/feature/z_stepper_align.cpp b/Marlin/src/feature/z_stepper_align.cpp
new file mode 100644
index 0000000..1b4eb44
--- /dev/null
+++ b/Marlin/src/feature/z_stepper_align.cpp
@@ -0,0 +1,121 @@
+/**
+ * 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/>.
+ *
+ */
+
+/**
+ * feature/z_stepper_align.cpp
+ */
+
+#include "../inc/MarlinConfigPre.h"
+
+#if ENABLED(Z_STEPPER_AUTO_ALIGN)
+
+#include "z_stepper_align.h"
+#include "../module/probe.h"
+
+ZStepperAlign z_stepper_align;
+
+xy_pos_t ZStepperAlign::xy[NUM_Z_STEPPER_DRIVERS];
+
+#if ENABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS)
+  xy_pos_t ZStepperAlign::stepper_xy[NUM_Z_STEPPER_DRIVERS];
+#endif
+
+void ZStepperAlign::reset_to_default() {
+  #ifdef Z_STEPPER_ALIGN_XY
+
+    constexpr xy_pos_t xy_init[] = Z_STEPPER_ALIGN_XY;
+    static_assert(COUNT(xy_init) == NUM_Z_STEPPER_DRIVERS,
+      "Z_STEPPER_ALIGN_XY requires "
+      #if NUM_Z_STEPPER_DRIVERS == 4
+        "four {X,Y} entries (Z, Z2, Z3, and Z4)."
+      #elif NUM_Z_STEPPER_DRIVERS == 3
+        "three {X,Y} entries (Z, Z2, and Z3)."
+      #else
+        "two {X,Y} entries (Z and Z2)."
+      #endif
+    );
+
+    #define VALIDATE_ALIGN_POINT(N) static_assert(N >= NUM_Z_STEPPER_DRIVERS || Probe::build_time::can_reach(xy_init[N]), \
+      "Z_STEPPER_ALIGN_XY point " STRINGIFY(N) " is not reachable with the default NOZZLE_TO_PROBE offset and PROBING_MARGIN.")
+    VALIDATE_ALIGN_POINT(0); VALIDATE_ALIGN_POINT(1); VALIDATE_ALIGN_POINT(2); VALIDATE_ALIGN_POINT(3);
+
+  #else // !Z_STEPPER_ALIGN_XY
+
+    const xy_pos_t xy_init[] = {
+      #if NUM_Z_STEPPER_DRIVERS >= 3  // First probe point...
+        #if !Z_STEPPERS_ORIENTATION
+          { probe.min_x(), probe.min_y() }, // SW
+        #elif Z_STEPPERS_ORIENTATION == 1
+          { probe.min_x(), probe.max_y() }, // NW
+        #elif Z_STEPPERS_ORIENTATION == 2
+          { probe.max_x(), probe.max_y() }, // NE
+        #elif Z_STEPPERS_ORIENTATION == 3
+          { probe.max_x(), probe.min_y() }, // SE
+        #else
+          #error "Z_STEPPERS_ORIENTATION must be from 0 to 3 (first point SW, NW, NE, SE)."
+        #endif
+        #if NUM_Z_STEPPER_DRIVERS == 4    // 3 more points...
+          #if !Z_STEPPERS_ORIENTATION
+            { probe.min_x(), probe.max_y() }, { probe.max_x(), probe.max_y() }, { probe.max_x(), probe.min_y() }  // SW
+          #elif Z_STEPPERS_ORIENTATION == 1
+            { probe.max_x(), probe.max_y() }, { probe.max_x(), probe.min_y() }, { probe.min_x(), probe.min_y() }  // NW
+          #elif Z_STEPPERS_ORIENTATION == 2
+            { probe.max_x(), probe.min_y() }, { probe.min_x(), probe.min_y() }, { probe.min_x(), probe.max_y() }  // NE
+          #elif Z_STEPPERS_ORIENTATION == 3
+            { probe.min_x(), probe.min_y() }, { probe.min_x(), probe.max_y() }, { probe.max_x(), probe.max_y() }  // SE
+          #endif
+        #elif !Z_STEPPERS_ORIENTATION     // or 2 more points...
+          { probe.max_x(), probe.min_y() }, { X_CENTER, probe.max_y() } // SW
+        #elif Z_STEPPERS_ORIENTATION == 1
+          { probe.min_x(), probe.min_y() }, { probe.max_x(), Y_CENTER } // NW
+        #elif Z_STEPPERS_ORIENTATION == 2
+          { probe.min_x(), probe.max_y() }, { X_CENTER, probe.min_y() } // NE
+        #elif Z_STEPPERS_ORIENTATION == 3
+          { probe.max_x(), probe.max_y() }, { probe.min_x(), Y_CENTER } // SE
+        #endif
+      #elif Z_STEPPERS_ORIENTATION
+        { X_CENTER, probe.min_y() }, { X_CENTER, probe.max_y() }
+      #else
+        { probe.min_x(), Y_CENTER }, { probe.max_x(), Y_CENTER }
+      #endif
+    };
+
+  #endif // !Z_STEPPER_ALIGN_XY
+
+  COPY(xy, xy_init);
+
+  #if ENABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS)
+    constexpr xy_pos_t stepper_xy_init[] = Z_STEPPER_ALIGN_STEPPER_XY;
+    static_assert(
+      COUNT(stepper_xy_init) == NUM_Z_STEPPER_DRIVERS,
+      "Z_STEPPER_ALIGN_STEPPER_XY requires "
+      #if NUM_Z_STEPPER_DRIVERS == 4
+        "four {X,Y} entries (Z, Z2, Z3, and Z4)."
+      #elif NUM_Z_STEPPER_DRIVERS == 3
+        "three {X,Y} entries (Z, Z2, and Z3)."
+      #endif
+    );
+    COPY(stepper_xy, stepper_xy_init);
+  #endif
+}
+
+#endif // Z_STEPPER_AUTO_ALIGN
diff --git a/Marlin/src/feature/z_stepper_align.h b/Marlin/src/feature/z_stepper_align.h
new file mode 100644
index 0000000..e1b235b
--- /dev/null
+++ b/Marlin/src/feature/z_stepper_align.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
+
+/**
+ * feature/z_stepper_align.h
+ */
+
+#include "../inc/MarlinConfig.h"
+
+class ZStepperAlign {
+  public:
+    static xy_pos_t xy[NUM_Z_STEPPER_DRIVERS];
+
+    #if ENABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS)
+      static xy_pos_t stepper_xy[NUM_Z_STEPPER_DRIVERS];
+    #endif
+
+  static void reset_to_default();
+};
+
+extern ZStepperAlign z_stepper_align;