Initial commit. Unusable Marlin 2.0.5.3 core without any custimization.

Marlin/src/feature/babystep.cpp

@@ -0,0 +1,119 @@
+/**
+ * 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 <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../inc/MarlinConfig.h"
+
+#if ENABLED(BABYSTEPPING)
+
+#include "babystep.h"
+#include "../MarlinCore.h"
+#include "../module/planner.h"
+#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) && !TEST(axis_known_position, axis)) return;
+
+  accum += distance; // Count up babysteps for the UI
+  #if ENABLED(BABYSTEP_DISPLAY_TOTAL)
+    axis_total[BS_TOTAL_IND(axis)] += distance;
+  #endif
+
+  #if ENABLED(BABYSTEP_ALWAYS_AVAILABLE)
+    #define BSA_ENABLE(AXIS) do{ switch (AXIS) { case X_AXIS: ENABLE_AXIS_X(); break; case Y_AXIS: ENABLE_AXIS_Y(); break; case Z_AXIS: ENABLE_AXIS_Z(); break; default: break; } }while(0)
+  #else
+    #define BSA_ENABLE(AXIS) NOOP
+  #endif
+
+  #if IS_CORE
+    #if ENABLED(BABYSTEP_XY)
+      switch (axis) {
+        case CORE_AXIS_1: // X on CoreXY and CoreXZ, Y on CoreYZ
+          BSA_ENABLE(CORE_AXIS_1);
+          BSA_ENABLE(CORE_AXIS_2);
+          steps[CORE_AXIS_1] += distance * 2;
+          steps[CORE_AXIS_2] += distance * 2;
+          break;
+        case CORE_AXIS_2: // Y on CoreXY, Z on CoreXZ and CoreYZ
+          BSA_ENABLE(CORE_AXIS_1);
+          BSA_ENABLE(CORE_AXIS_2);
+          steps[CORE_AXIS_1] += CORESIGN(distance * 2);
+          steps[CORE_AXIS_2] -= CORESIGN(distance * 2);
+          break;
+        case NORMAL_AXIS: // Z on CoreXY, Y on CoreXZ, X on CoreYZ
+        default:
+          BSA_ENABLE(NORMAL_AXIS);
+          steps[NORMAL_AXIS] += distance;
+          break;
+      }
+    #elif CORE_IS_XZ || CORE_IS_YZ
+      // Only Z stepping needs to be handled here
+      BSA_ENABLE(CORE_AXIS_1);
+      BSA_ENABLE(CORE_AXIS_2);
+      steps[CORE_AXIS_1] += CORESIGN(distance * 2);
+      steps[CORE_AXIS_2] -= CORESIGN(distance * 2);
+    #else
+      BSA_ENABLE(Z_AXIS);
+      steps[Z_AXIS] += distance;
+    #endif
+  #else
+    #if ENABLED(BABYSTEP_XY)
+      BSA_ENABLE(axis);
+    #else
+      BSA_ENABLE(Z_AXIS);
+    #endif
+    steps[BS_AXIS_IND(axis)] += distance;
+  #endif
+  #if ENABLED(BABYSTEP_ALWAYS_AVAILABLE)
+    gcode.reset_stepper_timeout();
+  #endif
+
+  #if ENABLED(INTEGRATED_BABYSTEPPING)
+    if (has_steps()) stepper.initiateBabystepping();
+  #endif
+}
+
+#endif // BABYSTEPPING

Marlin/src/feature/babystep.h

@@ -0,0 +1,85 @@
+/**
+ * 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 <http://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 (true
+        #if ENABLED(BABYSTEP_XY)
+          && axis == Z_AXIS
+        #endif
+      ) 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;

Marlin/src/feature/backlash.cpp

@@ -0,0 +1,146 @@
+/**
+ * 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 <http://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)
+  #if HAS_CUSTOM_PROBE_PIN
+    #define TEST_PROBE_PIN (READ(Z_MIN_PROBE_PIN) != Z_MIN_PROBE_ENDSTOP_INVERTING)
+  #else
+    #define TEST_PROBE_PIN (READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING)
+  #endif
+
+  // 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) && TEST_PROBE_PIN)
+      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

Marlin/src/feature/backlash.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 <http://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) {
+    // Return the measurement averaged over all readings
+    return (
+      #if ENABLED(MEASURE_BACKLASH_WHEN_PROBING)
+        measured_count[a] > 0 ? measured_mm[a] / measured_count[a] :
+      #endif
+      0
+    );
+    #if DISABLED(MEASURE_BACKLASH_WHEN_PROBING)
+      UNUSED(a);
+    #endif
+  }
+
+  static inline bool has_measurement(const AxisEnum a) {
+    return (false
+      #if ENABLED(MEASURE_BACKLASH_WHEN_PROBING)
+        || (measured_count[a] > 0)
+      #endif
+    );
+    #if DISABLED(MEASURE_BACKLASH_WHEN_PROBING)
+      UNUSED(a);
+    #endif
+  }
+
+  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;

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 <http://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

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 <http://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+extern uint8_t baricuda_valve_pressure,
+               baricuda_e_to_p_pressure;

Marlin/src/feature/bedlevel/abl/abl.cpp

@@ -0,0 +1,417 @@
+/**
+ * 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 <http://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;
+  #if ENABLED(EXTENSIBLE_UI)
+    ExtUI::onMeshUpdate(x, y, z_values[x][y]);
+  #endif
+
+  // 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 || 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();
+  #if ENABLED(ABL_BILINEAR_SUBDIVISION)
+    bed_level_virt_interpolate();
+  #endif
+}
+
+#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

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 <http://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

Marlin/src/feature/bedlevel/bedlevel.cpp

@@ -0,0 +1,255 @@
+/**
+ * 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 <http://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/ultralcd.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
+    #if ENABLED(MESH_BED_LEVELING)
+      mbl.has_mesh()
+    #elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
+      !!bilinear_grid_spacing.x
+    #elif ENABLED(AUTO_BED_LEVELING_UBL)
+      ubl.mesh_is_valid()
+    #else // 3POINT, LINEAR
+      true
+    #endif
+  ;
+}
+
+/**
+ * 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*/) {
+
+  #if ENABLED(AUTO_BED_LEVELING_BILINEAR)
+    const bool can_change = (!enable || leveling_is_valid());
+  #else
+    constexpr bool can_change = true;
+  #endif
+
+  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;
+        #if ENABLED(EXTENSIBLE_UI)
+          ExtUI::onMeshUpdate(x, y, 0);
+        #endif
+      }
+    #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;
+
+    #if ENABLED(LCD_BED_LEVELING)
+      ui.wait_for_move = false;
+    #endif
+  }
+
+#endif
+
+#endif // HAS_LEVELING

Marlin/src/feature/bedlevel/bedlevel.h

@@ -0,0 +1,98 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+#include "../../inc/MarlinConfigPre.h"
+
+#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

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 <http://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) {
+        line_to_destination(scaled_fr_mm_s);
+        current_position = destination;
+        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.
+        line_to_destination(scaled_fr_mm_s);
+        current_position = destination;
+        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

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 <http://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;

Marlin/src/feature/bedlevel/ubl/ubl.cpp

@@ -0,0 +1,243 @@
+/**
+ * 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 <http://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/configuration_store.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 I99");
+    LOOP_L_N(x, GRID_MAX_POINTS_X)
+      for (uint8_t y = 0;  y < GRID_MAX_POINTS_Y; 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)
+  );
+
+  #if HAS_LCD_MENU
+    bool unified_bed_leveling::lcd_map_control = false;
+  #endif
+
+  volatile int 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;
+      #if ENABLED(EXTENSIBLE_UI)
+        ExtUI::onMeshUpdate(x, y, value);
+      #endif
+    }
+  }
+
+  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);
+    for (int8_t i = 0; i < GRID_MAX_POINTS_X; i++) {
+      if (i < 10) SERIAL_CHAR(' ');
+      SERIAL_ECHO(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();
+      }
+      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

Marlin/src/feature/bedlevel/ubl/ubl.h

@@ -0,0 +1,314 @@
+/**
+ * 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 <http://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))
+
+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 ENABLED(NEWPANEL)
+      static void move_z_with_encoder(const float &multiplier);
+      static float measure_point_with_encoder();
+      static float measure_business_card_thickness(float in_height);
+      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;
+    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;
+    #endif
+
+    static volatile int encoder_diff; // Volatile because it's changed 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(const float &x) {
+      const int8_t cx = (x - (MESH_MIN_X)) * RECIPROCAL(MESH_X_DIST);
+      return constrain(cx, 0, (GRID_MAX_POINTS_X) - 1);   // -1 is appropriate if we want all movement to the X_MAX
+    }                                                     // position. But with this defined this way, it is possible
+                                                          // to extrapolate off of this point even further out. Probably
+                                                          // that is OK because something else should be keeping that from
+                                                          // happening and should not be worried about at this level.
+    static int8_t cell_index_y(const float &y) {
+      const int8_t cy = (y - (MESH_MIN_Y)) * RECIPROCAL(MESH_Y_DIST);
+      return constrain(cy, 0, (GRID_MAX_POINTS_Y) - 1);   // -1 is appropriate if we want all movement to the Y_MAX
+    }                                                     // position. But with this defined this way, it is possible
+                                                          // to extrapolate off of this point even further out. Probably
+                                                          // that is OK because something else should be keeping that from
+                                                          // happening and should not be worried about at this level.
+
+    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);
+    }
+
+    /**
+     * 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 (
+          #ifdef UBL_Z_RAISE_WHEN_OFF_MESH
+            UBL_Z_RAISE_WHEN_OFF_MESH
+          #else
+            NAN
+          #endif
+        );
+      }
+
+      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 (
+          #ifdef UBL_Z_RAISE_WHEN_OFF_MESH
+            UBL_Z_RAISE_WHEN_OFF_MESH
+          #else
+            NAN
+          #endif
+        );
+      }
+
+      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"

Marlin/src/feature/bedlevel/ubl/ubl_motion.cpp

@@ -0,0 +1,483 @@
+/**
+ * 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 <http://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) {
+
+      // For a move off the bed, use a constant Z raise
+      if (!WITHIN(iend.x, 0, GRID_MAX_POINTS_X - 1) || !WITHIN(iend.y, 0, GRID_MAX_POINTS_Y - 1)) {
+
+        // Note: There is no Z Correction in this case. We are off the grid and don't know what
+        // a reasonable correction would be.  If the user has specified a UBL_Z_RAISE_WHEN_OFF_MESH
+        // value, that will be used instead of a calculated (Bi-Linear interpolation) correction.
+
+        #ifdef UBL_Z_RAISE_WHEN_OFF_MESH
+          end.z += UBL_Z_RAISE_WHEN_OFF_MESH;
+        #endif
+        planner.buffer_segment(end, scaled_fr_mm_s, extruder);
+        current_position = destination;
+        return;
+      }
+
+      FINAL_MOVE:
+
+      // 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);
+
+      float z1, z2;
+      if (iend.x >= GRID_MAX_POINTS_X - 1)
+        z1 = z2 = 0.0;
+      else {
+        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 yratio = (end.y - mesh_index_to_ypos(iend.y)) * RECIPROCAL(MESH_Y_DIST),
+                  z0 = iend.y < GRID_MAX_POINTS_Y - 1 ? (z1 + (z2 - z1) * yratio) * planner.fade_scaling_factor_for_z(end.z) : 0.0;
+
+      // 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

Marlin/src/feature/binary_protocol.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 <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../inc/MarlinConfigPre.h"
+
+#if ENABLED(BINARY_FILE_TRANSFER)
+
+#include "../sd/cardreader.h"
+#include "binary_protocol.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 // BINARY_FILE_TRANSFER

Marlin/src/feature/binary_protocol.h

@@ -0,0 +1,480 @@
+/**
+ * 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 <http://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) {
+  switch (index) {
+    case 0: return MYSERIAL0.available();
+    #if NUM_SERIAL > 1
+      case 1: return MYSERIAL1.available();
+    #endif
+  }
+  return false;
+}
+
+inline int bs_read_serial(const uint8_t index) {
+  switch (index) {
+    case 0: return MYSERIAL0.read();
+    #if NUM_SERIAL > 1
+      case 1: return MYSERIAL1.read();
+    #endif
+  }
+  return -1;
+}
+
+#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;
+    #if ENABLED(BINARY_STREAM_COMPRESSION)
+      heatshrink_decoder_reset(&hsd);
+    #endif
+    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();
+    }
+    #if ENABLED(BINARY_STREAM_COMPRESSION)
+      heatshrink_decoder_finish(&hsd);
+    #endif
+    transfer_active = false;
+    return true;
+  }
+
+  static void transfer_abort() {
+    if (!dummy_transfer) {
+      card.closefile();
+      card.removeFile(card.filename);
+      card.release();
+      #if ENABLED(BINARY_STREAM_COMPRESSION)
+        heatshrink_decoder_finish(&hsd);
+      #endif
+    }
+    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(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];

Marlin/src/feature/bltouch.cpp

@@ -0,0 +1,208 @@
+/**
+ * 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 <http://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"
+
+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 (
+    #if ENABLED(Z_MIN_PROBE_USES_Z_MIN_ENDSTOP_PIN)
+      READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING
+    #else
+      READ(Z_MIN_PROBE_PIN) != Z_MIN_PROBE_ENDSTOP_INVERTING
+    #endif
+  );
+}
+
+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
+  #if ENABLED(BLTOUCH_FORCE_SW_MODE)
+   _set_SW_mode();
+  #endif
+
+  // 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

Marlin/src/feature/bltouch.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 <http://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+#include "../inc/MarlinConfigPre.h"
+
+// BLTouch commands are sent as servo angles
+typedef unsigned char BLTCommand;
+
+#define BLTOUCH_DEPLOY          10
+#define BLTOUCH_SW_MODE         60
+#define BLTOUCH_STOW            90
+#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); }
+
+  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;

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 <http://www.gnu.org/licenses/>.
+ *
+ */
+#include "../inc/MarlinConfig.h"
+
+#if ENABLED(CANCEL_OBJECTS)
+
+#include "cancel_object.h"
+#include "../gcode/gcode.h"
+#include "../lcd/ultralcd.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 + 1));
+    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

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 <http://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;

Marlin/src/feature/caselight.cpp

@@ -0,0 +1,93 @@
+/**
+ * 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 <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../inc/MarlinConfig.h"
+
+#if HAS_CASE_LIGHT
+
+uint8_t case_light_brightness = CASE_LIGHT_DEFAULT_BRIGHTNESS;
+bool case_light_on = CASE_LIGHT_DEFAULT_ON;
+
+#if ENABLED(CASE_LIGHT_USE_NEOPIXEL)
+  #include "leds/leds.h"
+  LEDColor case_light_color =
+    #ifdef CASE_LIGHT_NEOPIXEL_COLOR
+      CASE_LIGHT_NEOPIXEL_COLOR
+    #else
+      { 255, 255, 255, 255 }
+    #endif
+  ;
+#endif
+
+/**
+ * The following are 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.
+ */
+uint8_t case_light_brightness_sav;   // saves brighness info so can restore when "M355 S1" received
+bool case_light_arg_flag;  // flag to notify if S or P argument type
+
+#ifndef INVERT_CASE_LIGHT
+  #define INVERT_CASE_LIGHT false
+#endif
+
+void update_case_light() {
+
+  if (!(case_light_arg_flag && !case_light_on))
+    case_light_brightness_sav = case_light_brightness;  // save brightness except if this is an S0 argument
+  if (case_light_arg_flag && case_light_on)
+    case_light_brightness = case_light_brightness_sav;  // restore last brightens if this is an S1 argument
+
+  #if ENABLED(CASE_LIGHT_USE_NEOPIXEL) || DISABLED(CASE_LIGHT_NO_BRIGHTNESS)
+    const uint8_t i = case_light_on ? case_light_brightness : 0, n10ct = INVERT_CASE_LIGHT ? 255 - i : i;
+  #endif
+
+  #if ENABLED(CASE_LIGHT_USE_NEOPIXEL)
+
+    leds.set_color(
+      MakeLEDColor(case_light_color.r, case_light_color.g, case_light_color.b, case_light_color.w, n10ct),
+      false
+    );
+
+  #else // !CASE_LIGHT_USE_NEOPIXEL
+
+    #if DISABLED(CASE_LIGHT_NO_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 = case_light_on ? !INVERT_CASE_LIGHT : INVERT_CASE_LIGHT;
+        WRITE(CASE_LIGHT_PIN, s ? HIGH : LOW);
+      }
+
+  #endif // !CASE_LIGHT_USE_NEOPIXEL
+}
+
+#endif // HAS_CASE_LIGHT

Marlin/src/feature/caselight.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 <http://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+extern uint8_t case_light_brightness;
+extern bool case_light_on;
+extern uint8_t case_light_brightness_sav;   // saves brighness info when case_light_on is false
+extern bool case_light_arg_flag;  // flag to notify if S or P argument type
+
+void update_case_light();

Marlin/src/feature/closedloop.cpp

@@ -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 <http://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"
+
+void init_closedloop() {
+  OUT_WRITE(CLOSED_LOOP_ENABLE_PIN, LOW);
+  SET_INPUT_PULLUP(CLOSED_LOOP_MOVE_COMPLETE_PIN);
+}
+
+void set_closedloop(const byte val) {
+  OUT_WRITE(CLOSED_LOOP_ENABLE_PIN, val);
+}
+
+#endif // EXTERNAL_CLOSED_LOOP_CONTROLLER

Marlin/src/feature/closedloop.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 <http://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+void init_closedloop();
+void set_closedloop(const byte val);

Marlin/src/feature/controllerfan.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 <http://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}
+#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 = MOTOR_IS_ON(Z,Z)
+      #if HAS_Z2_ENABLE
+        || MOTOR_IS_ON(Z2,Z)
+      #endif
+      #if HAS_Z3_ENABLE
+        || MOTOR_IS_ON(Z3,Z)
+      #endif
+      #if HAS_Z4_ENABLE
+        || MOTOR_IS_ON(Z4,Z)
+      #endif
+      || (DISABLED(CONTROLLER_FAN_USE_Z_ONLY) && (
+          MOTOR_IS_ON(X,X) || MOTOR_IS_ON(Y,Y)
+          #if HAS_X2_ENABLE
+            || MOTOR_IS_ON(X2,X)
+          #endif
+          #if HAS_Y2_ENABLE
+            || MOTOR_IS_ON(Y2,Y)
+          #endif
+          #if E_STEPPERS
+            REPEAT(E_STEPPERS, _OR_ENABLED_E)
+          #endif
+        )
+      )
+    ;
+
+    // If any of the drivers or the heated bed are enabled...
+    if (motor_on
+      #if HAS_HEATED_BED
+        || thermalManager.temp_bed.soft_pwm_amount > 0
+      #endif
+    ) 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 + settings.duration * 1000UL)
+      ? 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

Marlin/src/feature/controllerfan.h

@@ -0,0 +1,76 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://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 constexpr &settings = controllerFan_defaults;
+    #endif
+    static inline bool state() { return speed > 0; }
+    static inline void init() { reset(); }
+    static inline void reset() {
+      #if ENABLED(CONTROLLER_FAN_EDITABLE)
+        settings = controllerFan_defaults;
+      #endif
+    }
+    static void setup();
+    static void update();
+};
+
+extern ControllerFan controllerFan;
+
+#endif

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 EXTRUDERS > 1
+    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 EXTRUDERS > 1
+    //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 EXTRUDERS > 1
+    WRITE(DAC1_SYNC, HIGH);
+  #endif
+  WRITE(SPI_EEPROM1_CS, HIGH);
+  WRITE(SPI_EEPROM2_CS, HIGH);
+  WRITE(SPI_FLASH_CS, HIGH);
+  WRITE(SS_PIN, HIGH);
+}
+
+#endif // MB(ALLIGATOR)

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 <http://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();
+};

Marlin/src/feature/dac/dac_mcp4728.cpp

@@ -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 <http://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:
+ * http://ww1.microchip.com/downloads/en/DeviceDoc/22187a.pdf
+ *
+ * For discussion and feedback, please go to:
+ * http://arduino.cc/forum/index.php/topic,51842.0.html
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(DAC_STEPPER_CURRENT)
+
+#include "dac_mcp4728.h"
+
+xyze_uint_t mcp4728_values;
+
+/**
+ * Begin I2C, get current values (input register and eeprom) of mcp4728
+ */
+void mcp4728_init() {
+  Wire.begin();
+  Wire.requestFrom(I2C_ADDRESS(DAC_DEV_ADDRESS), 24);
+  while (Wire.available()) {
+    char deviceID = Wire.read(),
+         hiByte = Wire.read(),
+         loByte = Wire.read();
+
+    if (!(deviceID & 0x08))
+      mcp4728_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) {
+  mcp4728_values[channel] = value;
+  return mcp4728_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(mcp4728_values[i]));
+    Wire.write(lowByte(mcp4728_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 mcp4728_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 * mcp4728_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 * mcp4728_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_uint8_t &pct) {
+  mcp4728_values *= 0.01 * pct * (DAC_STEPPER_MAX);
+  mcp4728_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(mcp4728_values[i]));
+    Wire.write(lowByte(mcp4728_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 // DAC_STEPPER_CURRENT

Marlin/src/feature/dac/dac_mcp4728.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 <http://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)
+
+void mcp4728_init();
+uint8_t mcp4728_analogWrite(const uint8_t channel, const uint16_t value);
+uint8_t mcp4728_eepromWrite();
+uint8_t mcp4728_setVref_all(const uint8_t value);
+uint8_t mcp4728_setGain_all(const uint8_t value);
+uint16_t mcp4728_getValue(const uint8_t channel);
+uint8_t mcp4728_fastWrite();
+uint8_t mcp4728_simpleCommand(const byte simpleCommand);
+uint8_t mcp4728_getDrvPct(const uint8_t channel);
+void mcp4728_setDrvPct(xyze_uint8_t &pct);

Marlin/src/feature/dac/stepper_dac.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 <http://www.gnu.org/licenses/>.
+ *
+ */
+
+/**
+ * stepper_dac.cpp - To set stepper current via DAC
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(DAC_STEPPER_CURRENT)
+
+#include "stepper_dac.h"
+
+bool dac_present = false;
+constexpr xyze_uint8_t dac_order = DAC_STEPPER_ORDER;
+xyze_uint8_t dac_channel_pct = DAC_MOTOR_CURRENT_DEFAULT;
+
+int dac_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 dac_current_percent(uint8_t channel, float val) {
+  if (!dac_present) return;
+
+  NOMORE(val, 100);
+
+  mcp4728_analogWrite(dac_order[channel], val * 0.01 * (DAC_STEPPER_MAX));
+  mcp4728_simpleCommand(UPDATE);
+}
+
+void dac_current_raw(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);
+}
+
+static float dac_perc(int8_t n) { return 100.0 * mcp4728_getValue(dac_order[n]) * RECIPROCAL(DAC_STEPPER_MAX); }
+static float dac_amps(int8_t n) { return mcp4728_getDrvPct(dac_order[n]) * (DAC_STEPPER_MAX) * 0.125 * RECIPROCAL(DAC_STEPPER_SENSE); }
+
+uint8_t dac_current_get_percent(const AxisEnum axis) { return mcp4728_getDrvPct(dac_order[axis]); }
+void dac_current_set_percents(xyze_uint8_t &pct) {
+  LOOP_XYZE(i) dac_channel_pct[i] = pct[dac_order[i]];
+  mcp4728_setDrvPct(dac_channel_pct);
+}
+
+void dac_print_values() {
+  if (!dac_present) return;
+
+  SERIAL_ECHO_MSG("Stepper current values in % (Amps):");
+  SERIAL_ECHO_START();
+  SERIAL_ECHOLNPAIR_P(
+    SP_X_LBL, dac_perc(X_AXIS), PSTR(" ("), dac_amps(X_AXIS), PSTR(")")
+    SP_Y_LBL, dac_perc(Y_AXIS), PSTR(" ("), dac_amps(Y_AXIS), PSTR(")")
+    SP_Z_LBL, dac_perc(Z_AXIS), PSTR(" ("), dac_amps(Z_AXIS), PSTR(")")
+    SP_E_LBL, dac_perc(E_AXIS), PSTR(" ("), dac_amps(E_AXIS), PSTR(")")
+  );
+}
+
+void dac_commit_eeprom() {
+  if (!dac_present) return;
+  mcp4728_eepromWrite();
+}
+
+#endif // DAC_STEPPER_CURRENT

Marlin/src/feature/dac/stepper_dac.h

@@ -0,0 +1,36 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+/**
+ * stepper_dac.h   - To set stepper current via DAC
+ */
+
+#include "dac_mcp4728.h"
+
+int dac_init();
+void dac_current_percent(uint8_t channel, float val);
+void dac_current_raw(uint8_t channel, uint16_t val);
+void dac_print_values();
+void dac_commit_eeprom();
+uint8_t dac_current_get_percent(AxisEnum axis);
+void dac_current_set_percents(xyze_uint8_t &pct);

Marlin/src/feature/digipot/digipot.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 <http://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+void digipot_i2c_set_current(const uint8_t channel, const float current);
+void digipot_i2c_init();

Marlin/src/feature/digipot/digipot_mcp4018.cpp

@@ -0,0 +1,103 @@
+/**
+ * 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 <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if BOTH(DIGIPOT_I2C, DIGIPOT_MCP4018)
+
+#include "Stream.h"
+#include "utility/twi.h"
+#include <SlowSoftI2CMaster.h>  //https://github.com/stawel/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_A4988_MAX_VALUE         127
+
+#define DIGIPOT_A4988_Itripmax(Vref)    ((Vref)/(8.0*DIGIPOT_A4988_Rsx))
+
+#define DIGIPOT_A4988_FACTOR            ((DIGIPOT_A4988_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 = ceil(float(DIGIPOT_A4988_FACTOR) * current);
+  return byte(constrain(value, 0, DIGIPOT_A4988_MAX_VALUE));
+}
+
+const uint8_t sda_pins[DIGIPOT_I2C_NUM_CHANNELS] = {
+  DIGIPOTS_I2C_SDA_X
+  #if DIGIPOT_I2C_NUM_CHANNELS > 1
+    , DIGIPOTS_I2C_SDA_Y
+    #if DIGIPOT_I2C_NUM_CHANNELS > 2
+      , DIGIPOTS_I2C_SDA_Z
+      #if DIGIPOT_I2C_NUM_CHANNELS > 3
+        , DIGIPOTS_I2C_SDA_E0
+        #if DIGIPOT_I2C_NUM_CHANNELS > 4
+          , DIGIPOTS_I2C_SDA_E1
+        #endif
+      #endif
+    #endif
+  #endif
+};
+
+static SlowSoftI2CMaster pots[DIGIPOT_I2C_NUM_CHANNELS] = {
+  SlowSoftI2CMaster { sda_pins[X_AXIS], DIGIPOTS_I2C_SCL }
+  #if DIGIPOT_I2C_NUM_CHANNELS > 1
+    , SlowSoftI2CMaster { sda_pins[Y_AXIS], DIGIPOTS_I2C_SCL }
+    #if DIGIPOT_I2C_NUM_CHANNELS > 2
+      , SlowSoftI2CMaster { sda_pins[Z_AXIS], DIGIPOTS_I2C_SCL }
+      #if DIGIPOT_I2C_NUM_CHANNELS > 3
+        , SlowSoftI2CMaster { sda_pins[E_AXIS], DIGIPOTS_I2C_SCL }
+        #if DIGIPOT_I2C_NUM_CHANNELS > 4
+          , SlowSoftI2CMaster { sda_pins[E_AXIS + 1], DIGIPOTS_I2C_SCL }
+        #endif
+      #endif
+    #endif
+  #endif
+};
+
+static void 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 digipot_i2c_set_current(const uint8_t channel, const float current) {
+  i2c_send(channel, current_to_wiper(_MIN(_MAX(current, 0), float(DIGIPOT_A4988_MAX_CURRENT))));
+}
+
+void digipot_i2c_init() {
+  static const float digipot_motor_current[] PROGMEM = DIGIPOT_I2C_MOTOR_CURRENTS;
+
+  LOOP_L_N(i, DIGIPOT_I2C_NUM_CHANNELS)
+    pots[i].i2c_init();
+
+  // setup initial currents as defined in Configuration_adv.h
+  LOOP_L_N(i, COUNT(digipot_motor_current))
+    digipot_i2c_set_current(i, pgm_read_float(&digipot_motor_current[i]));
+}
+
+#endif // DIGIPOT_I2C && DIGIPOT_MCP4018

Marlin/src/feature/digipot/digipot_mcp4451.cpp

@@ -0,0 +1,90 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(DIGIPOT_I2C) && DISABLED(DIGIPOT_MCP4018)
+
+#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.96
+  #define DIGIPOT_I2C_MAX_CURRENT 1.736
+#elif MB(AZTEEG_X5_MINI, AZTEEG_X5_MINI_WIFI)
+  #define DIGIPOT_I2C_FACTOR 113.5
+  #define DIGIPOT_I2C_MAX_CURRENT 2.0
+#else
+  #define DIGIPOT_I2C_FACTOR 106.7
+  #define DIGIPOT_I2C_MAX_CURRENT 2.5
+#endif
+
+static byte current_to_wiper(const float current) {
+  return byte(CEIL(float((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 digipot_i2c_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 digipot_i2c_init() {
+  #if MB(MKS_SBASE)
+    configure_i2c(16); // Setting clock_option to 16 ensure the I2C bus is initialized at 400kHz
+  #else
+    Wire.begin();
+  #endif
+  // setup initial currents as defined in Configuration_adv.h
+  static const float digipot_motor_current[] PROGMEM = DIGIPOT_I2C_MOTOR_CURRENTS;
+  LOOP_L_N(i, COUNT(digipot_motor_current))
+    digipot_i2c_set_current(i, pgm_read_float(&digipot_motor_current[i]));
+}
+
+#endif // DIGIPOT_I2C

Marlin/src/feature/e_parser.cpp

@@ -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 <http://www.gnu.org/licenses/>.
+ *
+ */
+
+/**
+ * emergency_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::enabled;
+
+#if ENABLED(HOST_PROMPT_SUPPORT)
+  uint8_t EmergencyParser::M876_reason; // = 0
+#endif
+
+// Global instance
+EmergencyParser emergency_parser;
+
+#endif // EMERGENCY_PARSER

Marlin/src/feature/e_parser.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 <http://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+/**
+ * emergency_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;
+void quickstop_stepper();
+
+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;
+
+  #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) {
+    #define ISEOL(C) ((C) == '\n' || (C) == '\r')
+    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': case '1': case '2':
+          case '3': case '4': case '5':
+          case '6': case '7': case '8':
+          case '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': case '1': case '2':
+          case '3': case '4': case '5':
+          case '6': case '7': case '8':
+          case '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_stepper(); 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;

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 <http://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 ", FIXFLOAT(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 ", FIXFLOAT(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;

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 <http://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

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 <http://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();

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 <http://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

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 <http://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;

Marlin/src/feature/fwretract.cpp

@@ -0,0 +1,216 @@
+/**
+ * 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 <http://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 EXTRUDERS > 1
+  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() {
+  #if ENABLED(FWRETRACT_AUTORETRACT)
+    autoretract_enabled = false;
+  #endif
+  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;
+    #if EXTRUDERS > 1
+      retracted_swap[i] = false;
+    #endif
+    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 EXTRUDERS > 1
+    , 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 EXTRUDERS > 1
+    // 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 EXTRUDERS > 1
+        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 = (
+                (swapping ? settings.swap_retract_length : settings.retract_length)
+                #if ENABLED(RETRACT_SYNC_MIXING)
+                  * (MIXING_STEPPERS)
+                #endif
+              );
+
+  // 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 to destination
+      settings.retract_feedrate_mm_s
+      #if ENABLED(RETRACT_SYNC_MIXING)
+        * (MIXING_STEPPERS)
+      #endif
+    );
+
+    // 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;
+
+    const feedRate_t fr_mm_s = (
+      (swapping ? settings.swap_retract_recover_feedrate_mm_s : settings.retract_recover_feedrate_mm_s)
+      #if ENABLED(RETRACT_SYNC_MIXING)
+        * (MIXING_STEPPERS)
+      #endif
+    );
+    prepare_internal_move_to_destination(fr_mm_s);        // Recover E, set_current_to_destination
+  }
+
+  #if ENABLED(RETRACT_SYNC_MIXING)
+    mixer.T(old_mixing_tool);                             // Restore original mixing tool
+  #endif
+
+  retracted[active_extruder] = retracting;                // Active extruder now retracted / recovered
+
+  // If swap retract/recover update the retracted_swap flag too
+  #if EXTRUDERS > 1
+    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 EXTRUDERS > 1
+        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

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 <http://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 EXTRUDERS > 1
+    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 EXTRUDERS > 1
+      , bool swapping = false
+    #endif
+  );
+};
+
+extern FWRetract fwretract;
+
+#endif // FWRETRACT

Marlin/src/feature/host_actions.cpp

@@ -0,0 +1,187 @@
+/**
+ * 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 <http://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(const char * const pstr, const bool eol) {
+  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
+
+#if ENABLED(HOST_PROMPT_SUPPORT)
+
+  const char CONTINUE_STR[] PROGMEM = "Continue",
+             DISMISS_STR[] PROGMEM = "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) {
+    host_action(PSTR("notification "), false);
+    serialprintPGM(message);
+    SERIAL_EOL();
+  }
+
+  void host_action_prompt(const char * const ptype, const bool eol=true) {
+    host_action(PSTR("prompt_"), false);
+    serialprintPGM(ptype);
+    if (eol) SERIAL_EOL();
+  }
+
+  void host_action_prompt_plus(const char * const ptype, const char * const pstr, const char extra_char='\0') {
+    host_action_prompt(ptype, false);
+    SERIAL_CHAR(' ');
+    serialprintPGM(pstr);
+    if (extra_char != '\0') SERIAL_CHAR(extra_char);
+    SERIAL_EOL();
+  }
+  void host_action_prompt_begin(const PromptReason reason, const char * 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(const char * 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_do(const PromptReason reason, const char * const pstr, const char * const btn1/*=nullptr*/, const char * const btn2/*=nullptr*/) {
+    host_action_prompt_begin(reason, pstr);
+    if (btn1) host_action_prompt_button(btn1);
+    if (btn2) host_action_prompt_button(btn2);
+    host_action_prompt_show();
+  }
+
+  void filament_load_host_prompt() {
+    const bool disable_to_continue = (false
+      #if HAS_FILAMENT_SENSOR
+        || runout.filament_ran_out
+      #endif
+    );
+    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 const char m876_prefix[] PROGMEM = "M876 Handle Re";
+      serialprintPGM(m876_prefix); SERIAL_ECHOLNPAIR("ason: ", host_prompt_reason);
+      serialprintPGM(m876_prefix); SERIAL_ECHOLNPAIR("sponse: ", response);
+    #endif
+    const char *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 HAS_LCD_MENU && ENABLED(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 HAS_LCD_MENU && ENABLED(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:
+        #if HAS_RESUME_CONTINUE
+          wait_for_user = false;
+        #endif
+        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

Marlin/src/feature/host_actions.h

@@ -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 <http://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+#include "../inc/MarlinConfigPre.h"
+
+void host_action(const char * 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
+
+#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_prompt_begin(const PromptReason reason, const char * const pstr, const char extra_char='\0');
+  void host_action_prompt_button(const char * const pstr);
+  void host_action_prompt_end();
+  void host_action_prompt_show();
+  void host_prompt_do(const PromptReason reason, const char * const pstr, const char * const btn1=nullptr, const char * const btn2=nullptr);
+  inline void host_prompt_open(const PromptReason reason, const char * const pstr, const char * const btn1=nullptr, const char * const btn2=nullptr) {
+    if (host_prompt_reason == PROMPT_NOT_DEFINED) host_prompt_do(reason, pstr, btn1, btn2);
+  }
+
+  void filament_load_host_prompt();
+
+#endif

Marlin/src/feature/joystick.cpp

@@ -0,0 +1,186 @@
+/**
+ * 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 <http://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"
+#include "../module/temperature.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;
+
+    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.
+
+    #if ENABLED(EXTENSIBLE_UI)
+      ExtUI::_joystick_update(norm_jog);
+    #endif
+
+    // 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

Marlin/src/feature/joystick.h

@@ -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 <http://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+/**
+ * joystick.h - joystick input / jogging
+ */
+
+#include "../inc/MarlinConfigPre.h"
+#include "../core/types.h"
+#include "../core/macros.h"
+#include "../module/temperature.h"
+
+//#define JOYSTICK_DEBUG
+
+class Joystick {
+  friend class Temperature;
+  private:
+    #if HAS_JOY_ADC_X
+      static temp_info_t x;
+    #endif
+    #if HAS_JOY_ADC_Y
+      static temp_info_t y;
+    #endif
+    #if HAS_JOY_ADC_Z
+      static temp_info_t z;
+    #endif
+  public:
+    #if ENABLED(JOYSTICK_DEBUG)
+      static void report();
+    #endif
+    static void calculate(xyz_float_t &norm_jog);
+    static void inject_jog_moves();
+};
+
+extern Joystick joystick;

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 <http://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

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 <http://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);

Marlin/src/feature/leds/leds.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 <http://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 <SailfishRGB_LED.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
+  #if ENABLED(NEOPIXEL_LED)
+    neo.init();
+  #endif
+  #if ENABLED(PCA9533)
+    RGBinit();
+  #endif
+  #if ENABLED(LED_USER_PRESET_STARTUP)
+    set_default();
+  #endif
+}
+
+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
+
+  #if ENABLED(PCA9632)
+    // Update I2C LED driver
+    pca9632_set_led_color(incol);
+  #endif
+
+  #if ENABLED(PCA9533)
+    RGBsetColor(incol.r, incol.g, incol.b, true);
+  #endif
+
+  #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
+
+#endif // HAS_COLOR_LEDS

Marlin/src/feature/leds/leds.h

@@ -0,0 +1,218 @@
+/**
+ * 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 <http://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
+#define HAS_WHITE_LED EITHER(RGBW_LED, NEOPIXEL_LED)
+
+/**
+ * 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];
+    #if HAS_WHITE_LED
+      w = rgbw[3];
+    #endif
+    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
+      #if HAS_WHITE_LED
+        + w
+      #endif
+    ;
+  }
+} 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
+  );
+
+  inline void set_color(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
+    #if ENABLED(NEOPIXEL_LED)
+      , 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;

Marlin/src/feature/leds/neopixel.cpp

@@ -0,0 +1,117 @@
+/**
+ * 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 <http://www.gnu.org/licenses/>.
+ *
+ */
+
+/**
+ * Marlin RGB LED general support
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(NEOPIXEL_LED)
+
+#include "neopixel.h"
+
+#if ENABLED(NEOPIXEL_STARTUP_TEST)
+  #include "../../core/utility.h"
+#endif
+
+Marlin_NeoPixel neo;
+
+Adafruit_NeoPixel Marlin_NeoPixel::adaneo1(NEOPIXEL_PIXELS, NEOPIXEL_PIN, NEOPIXEL_TYPE + NEO_KHZ800)
+  #if MULTIPLE_NEOPIXEL_TYPES
+    , 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) {
+  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() {
+  SET_OUTPUT(NEOPIXEL_PIN);
+  set_brightness(NEOPIXEL_BRIGHTNESS); // 0 - 255 range
+  begin();
+  show();  // initialize to all off
+
+  #if ENABLED(NEOPIXEL_STARTUP_TEST)
+    safe_delay(1000);
+    set_color_startup(adaneo1.Color(255, 0, 0, 0));  // red
+    safe_delay(1000);
+    set_color_startup(adaneo1.Color(0, 255, 0, 0));  // green
+    safe_delay(1000);
+    set_color_startup(adaneo1.Color(0, 0, 255, 0));  // blue
+    safe_delay(1000);
+  #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
+
+#endif // NEOPIXEL_LED

Marlin/src/feature/leds/neopixel.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 <http://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+/**
+ * Neopixel support
+ */
+
+// ------------------------
+// Includes
+// ------------------------
+
+#include "../../inc/MarlinConfig.h"
+
+#include <Adafruit_NeoPixel.h>
+#include <stdint.h>
+
+// ------------------------
+// Defines
+// ------------------------
+
+#define MULTIPLE_NEOPIXEL_TYPES (defined(NEOPIXEL2_TYPE) && (NEOPIXEL2_TYPE != NEOPIXEL_TYPE))
+
+#define NEOPIXEL_IS_RGB  (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_RGBW !NEOPIXEL_IS_RGB
+
+#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 MULTIPLE_NEOPIXEL_TYPES
+      , adaneo2
+    #endif
+  ;
+
+public:
+  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();
+    #if MULTIPLE_NEOPIXEL_TYPES
+      adaneo2.begin();
+    #endif
+  }
+
+  static inline void set_pixel_color(const uint16_t n, const uint32_t c) {
+    adaneo1.setPixelColor(n, c);
+    #if MULTIPLE_NEOPIXEL_TYPES
+      adaneo2.setPixelColor(n, c);
+    #endif
+  }
+
+  static inline void set_brightness(const uint8_t b) {
+    adaneo1.setBrightness(b);
+    #if MULTIPLE_NEOPIXEL_TYPES
+      adaneo2.setBrightness(b);
+    #endif
+  }
+
+  static inline void show() {
+    adaneo1.show();
+    #if PIN_EXISTS(NEOPIXEL2)
+      #if MULTIPLE_NEOPIXEL_TYPES
+        adaneo2.show();
+      #else
+        adaneo1.setPin(NEOPIXEL2_PIN);
+        adaneo1.show();
+        adaneo1.setPin(NEOPIXEL_PIN);
+      #endif
+    #endif
+  }
+
+  #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() { 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;

Marlin/src/feature/leds/pca9632.cpp

@@ -0,0 +1,150 @@
+/**
+ * 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 <http://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
+#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 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)
+  #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 DISABLED(PCA9632_NO_AUTO_INC)
+    uint8_t data[4], len = 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;
+  #else
+    uint8_t data[6], len = 6;
+    data[0] = regadd + (PCA9632_RED >> 1);
+    data[1] = vr;
+    data[2] = regadd + (PCA9632_GRN >> 1);
+    data[3] = vg;
+    data[4] = regadd + (PCA9632_BLU >> 1);
+    data[5] = vb;
+  #endif
+  Wire.beginTransmission(I2C_ADDRESS(addr));
+  Wire.write(data, len);
+  Wire.endTransmission();
+}
+
+#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);
+
+  PCA9632_WriteAllRegisters(PCA9632_ADDRESS,PCA9632_PWM0, color.r, color.g, color.b);
+  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

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 <http://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

Marlin/src/feature/leds/printer_event_leds.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 <http://www.gnu.org/licenses/>.
+ *
+ */
+
+/**
+ * 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) {
+    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

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 <http://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+/**
+ * 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;

Marlin/src/feature/leds/tempstat.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 <http://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 = 0.0;
+    #if HAS_HEATED_BED
+      max_temp = _MAX(thermalManager.degTargetBed(), thermalManager.degBed());
+    #endif
+    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

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 <http://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+/**
+ * Marlin general RGB LED support
+ */
+
+void handle_status_leds();

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 <http://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

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 <http://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;

Marlin/src/feature/mixing.cpp

@@ -0,0 +1,192 @@
+/**
+ * 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 <http://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 DUAL_MIXING_EXTRUDER || ENABLED(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
+
+  #if ENABLED(GRADIENT_MIX)
+    refresh_gradient();
+  #endif
+}
+
+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, MIXING_VIRTUAL_TOOLS && t < 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 ENABLED(RETRACT_SYNC_MIXING)
+    // 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 DUAL_MIXING_EXTRUDER || ENABLED(GRADIENT_MIX)
+    update_mix_from_vtool();
+  #endif
+
+  #if ENABLED(GRADIENT_MIX)
+    update_gradient_for_planner_z();
+  #endif
+}
+
+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() {
+    update_gradient_for_z(planner.get_axis_position_mm(Z_AXIS));
+  }
+
+#endif // GRADIENT_MIX
+
+#endif // MIXING_EXTRUDER

Marlin/src/feature/mixing.h

@@ -0,0 +1,277 @@
+/**
+ * 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 <http://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+#include "../inc/MarlinConfig.h"
+
+//#define MIXER_NORMALIZER_DEBUG
+
+#ifndef __AVR__ // || DUAL_MIXING_EXTRUDER
+  // 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 ENABLED(RETRACT_SYNC_MIXING)
+    MIXER_AUTORETRACT_TOOL,
+  #endif
+  NR_MIXING_VIRTUAL_TOOLS
+};
+
+#if ENABLED(RETRACT_SYNC_MIXING)
+  #define MAX_VTOOLS 254
+#else
+  #define MAX_VTOOLS 255
+#endif
+static_assert(NR_MIXING_VIRTUAL_TOOLS <= MAX_VTOOLS, "MIXING_VIRTUAL_TOOLS must be <= " STRINGIFY(MAX_VTOOLS) "!");
+
+#define MIXER_STEPPER_LOOP(VAR) \
+  for (uint_fast8_t VAR = 0; VAR < MIXING_STEPPERS; VAR++)
+
+#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)
+
+#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];
+    #if ENABLED(GRADIENT_VTOOL)
+      int8_t vtool_index;                 // Use this virtual tool number as index
+    #endif
+  } 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;
+    #if ENABLED(GRADIENT_VTOOL)
+      refresh_gradient();
+    #endif
+    #if DUAL_MIXING_EXTRUDER
+      update_mix_from_vtool();
+    #endif
+  }
+
+  // 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 DUAL_MIXING_EXTRUDER || ENABLED(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 // DUAL_MIXING_EXTRUDER || GRADIENT_MIX
+
+  #if DUAL_MIXING_EXTRUDER
+
+    // Update the virtual tool from an edited mix
+    static inline void update_vtool_from_mix() {
+      copy_mix_to_color(color[selected_vtool]);
+      #if ENABLED(GRADIENT_MIX)
+        refresh_gradient();
+      #endif
+      // MIXER_STEPPER_LOOP(i) collector[i] = mix[i];
+      // normalize();
+    }
+
+  #endif // DUAL_MIXING_EXTRUDER
+
+  #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;

Marlin/src/feature/mmu2/mmu2.cpp

@@ -0,0 +1,801 @@
+/**
+ * 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 <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(PRUSA_MMU2)
+
+#include "mmu2.h"
+#include "../../lcd/menu/menu_mmu2.h"
+
+MMU2 mmu2;
+
+#include "../../gcode/gcode.h"
+#include "../../lcd/ultralcd.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 60000ul // 5min timeout for mmu commands (except P0)
+#define MMU_P0_TIMEOUT 3000ul   // Timeout for P0 command: 3seconds
+
+#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
+
+#if ENABLED(MMU2_MODE_12V)
+  #define MMU_REQUIRED_FW_BUILDNR 132
+#else
+  #define MMU_REQUIRED_FW_BUILDNR 126
+#endif
+
+#define MMU2_NO_TOOL 99
+#define MMU_BAUD    115200
+
+#define mmuSerial   MMU2_SERIAL
+
+bool MMU2::enabled, MMU2::ready, MMU2::mmu_print_saved;
+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::last_request, MMU2::next_P0_request;
+char MMU2::rx_buffer[MMU_RX_SIZE], MMU2::tx_buffer[MMU_TX_SIZE];
+
+#if HAS_LCD_MENU && ENABLED(MMU2_MENUS)
+
+  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 };
+  static constexpr E_Step load_to_nozzle_sequence[] PROGMEM = { MMU2_LOAD_TO_NOZZLE_SEQUENCE };
+
+#endif // MMU2_MENUS
+
+MMU2::MMU2() {
+  rx_buffer[0] = '\0';
+}
+
+void MMU2::init() {
+
+  set_runout_valid(false);
+
+  #if PIN_EXISTS(MMU2_RST)
+    // TODO use macros for this
+    WRITE(MMU2_RST_PIN, HIGH);
+    SET_OUTPUT(MMU2_RST_PIN);
+  #endif
+
+  mmuSerial.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
+    tx_str_P(PSTR("X0\n")); // Send soft reset
+  #endif
+}
+
+uint8_t MMU2::get_current_tool() {
+  return extruder == MMU2_NO_TOOL ? -1 : extruder;
+}
+
+void MMU2::mmu_loop() {
+
+  switch (state) {
+
+    case 0: break;
+
+    case -1:
+      if (rx_start()) {
+        DEBUG_ECHOLNPGM("MMU => 'start'");
+        DEBUG_ECHOLNPGM("MMU <= 'S1'");
+
+        // send "read version" request
+        tx_str_P(PSTR("S1\n"));
+
+        state = -2;
+      }
+      else if (millis() > 3000000) {
+        SERIAL_ECHOLNPGM("MMU not responding - DISABLED");
+        state = 0;
+      }
+      break;
+
+    case -2:
+      if (rx_ok()) {
+        sscanf(rx_buffer, "%uok\n", &version);
+
+        DEBUG_ECHOLNPAIR("MMU => ", version, "\nMMU <= 'S2'");
+
+        tx_str_P(PSTR("S2\n")); // read build number
+        state = -3;
+      }
+      break;
+
+    case -3:
+      if (rx_ok()) {
+        sscanf(rx_buffer, "%uok\n", &buildnr);
+
+        DEBUG_ECHOLNPAIR("MMU => ", buildnr);
+
+        check_version();
+
+        #if ENABLED(MMU2_MODE_12V)
+          DEBUG_ECHOLNPGM("MMU <= 'M1'");
+
+          tx_str_P(PSTR("M1\n")); // switch to stealth mode
+          state = -5;
+
+        #else
+          DEBUG_ECHOLNPGM("MMU <= 'P0'");
+
+          tx_str_P(PSTR("P0\n")); // 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'");
+
+        tx_str_P(PSTR("P0\n")); // 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;
+      }
+      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);
+          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'");
+          tx_str_P(PSTR("C0\n"));
+          state = 3; // wait for response
+        }
+        else if (cmd == MMU_CMD_U0) {
+          // unload current
+          DEBUG_ECHOLNPGM("MMU <= 'U0'");
+
+          tx_str_P(PSTR("U0\n"));
+          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'");
+          tx_str_P(PSTR("R0\n"));
+          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(), next_P0_request)) {
+        // read FINDA
+        tx_str_P(PSTR("P0\n"));
+        state = 2; // wait for response
+      }
+      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);
+
+        state = 1;
+
+        if (cmd == 0) ready = true;
+
+        if (!finda && finda_runout_valid) filament_runout();
+      }
+      else if (ELAPSED(millis(), last_request + MMU_P0_TIMEOUT)) // Resend request after timeout (3s)
+        state = 1;
+
+      break;
+
+    case 3:   // response to mmu commands
+      if (rx_ok()) {
+        DEBUG_ECHOLNPGM("MMU => 'ok'");
+        ready = true;
+        state = 1;
+        last_cmd = MMU_CMD_NONE;
+      }
+      else if (ELAPSED(millis(), last_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;
+      }
+      break;
+  }
+}
+
+/**
+ * Check if MMU was started
+ */
+bool MMU2::rx_start() {
+  // check for start message
+  if (rx_str_P(PSTR("start\n"))) {
+    next_P0_request = millis() + 300;
+    return true;
+  }
+  return false;
+}
+
+/**
+ * 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 (mmuSerial.available()) {
+    rx_buffer[i++] = mmuSerial.read();
+    rx_buffer[i] = '\0';
+
+    if (i == sizeof(rx_buffer) - 1) {
+      DEBUG_ECHOLNPGM("rx buffer overrun");
+      break;
+    }
+  }
+
+  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) mmuSerial.write(pgm_read_byte(str++));
+  rx_buffer[0] = '\0';
+  last_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) mmuSerial.write(tx_buffer[i]);
+  rx_buffer[0] = '\0';
+  last_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) mmuSerial.write(tx_buffer[i]);
+  rx_buffer[0] = '\0';
+  last_request = millis();
+}
+
+/**
+ * Empty the rx buffer
+ */
+void MMU2::clear_rx_buffer() {
+  while (mmuSerial.available()) mmuSerial.read();
+  rx_buffer[0] = '\0';
+}
+
+/**
+ * Check if we received 'ok' from MMU
+ */
+bool MMU2::rx_ok() {
+  if (rx_str_P(PSTR("ok\n"))) {
+    next_P0_request = millis() + 300;
+    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_MMU2_WRONG_FIRMWARE));
+  }
+}
+
+/**
+ * Handle tool change
+ */
+void MMU2::tool_change(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;
+
+  #if ENABLED(MMU2_MENUS)
+
+    set_runout_valid(false);
+
+    switch (*special) {
+      case '?': {
+        uint8_t index = mmu2_choose_filament();
+        while (!thermalManager.wait_for_hotend(active_extruder, false)) safe_delay(100);
+        load_filament_to_nozzle(index);
+      } break;
+
+      case 'x': {
+        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;
+      } break;
+
+      case 'c': {
+        while (!thermalManager.wait_for_hotend(active_extruder, false)) safe_delay(100);
+        execute_extruder_sequence((const E_Step *)load_to_nozzle_sequence, COUNT(load_to_nozzle_sequence));
+      } break;
+    }
+
+    set_runout_valid(true);
+
+  #endif
+}
+
+/**
+ * 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 nexcessary
+ */
+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;
+
+  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(2, park_point /*= NOZZLE_PARK_POINT*/);
+
+        if (turn_off_nozzle) thermalManager.setTargetHotend(0, active_extruder);
+
+        LCD_MESSAGEPGM(MSG_MMU2_NOT_RESPONDING);
+        BUZZ(100, 659);
+        BUZZ(200, 698);
+        BUZZ(100, 659);
+        BUZZ(300, 440);
+        BUZZ(100, 659);
+      }
+    }
+    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(200, 404);
+        BUZZ(200, 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(200, 404);
+        BUZZ(200, 404);
+        LCD_MESSAGEPGM(MSG_MMU2_RESUMING);
+      }
+    }
+  }
+}
+
+void MMU2::set_filament_type(uint8_t index, 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_LCD_MENU && ENABLED(MMU2_MENUS)
+
+  // Load filament into MMU2
+  void MMU2::load_filament(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(uint8_t index) {
+
+    if (!enabled) return false;
+
+    if (thermalManager.tooColdToExtrude(active_extruder)) {
+      BUZZ(200, 404);
+      LCD_ALERTMESSAGEPGM(MSG_HOTEND_TOO_COLD);
+      return false;
+    }
+    else {
+      command(MMU_CMD_T0 + index);
+      manage_response(true, true);
+      command(MMU_CMD_C0);
+      mmu_loop();
+
+      extruder = index;
+      active_extruder = 0;
+
+      load_to_nozzle();
+
+      BUZZ(200, 404);
+      return true;
+    }
+  }
+
+  /**
+   *
+   * Load filament to nozzle of multimaterial printer
+   *
+   * This function is used only 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() {
+    if (!enabled) return;
+    execute_extruder_sequence((const E_Step *)load_to_nozzle_sequence, COUNT(load_to_nozzle_sequence));
+  }
+
+  bool MMU2::eject_filament(uint8_t index, 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(2500 / 60);
+    planner.synchronize();
+    command(MMU_CMD_E0 + index);
+    manage_response(false, false);
+
+    if (recover)  {
+      LCD_MESSAGEPGM(MSG_MMU2_EJECT_RECOVER);
+      BUZZ(200, 404);
+      #if ENABLED(HOST_PROMPT_SUPPORT)
+        host_prompt_do(PROMPT_USER_CONTINUE, PSTR("MMU2 Eject Recover"), CONTINUE_STR);
+      #endif
+      #if ENABLED(EXTENSIBLE_UI)
+        ExtUI::onUserConfirmRequired_P(PSTR("MMU2 Eject Recover"));
+      #endif
+      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;
+    }
+
+    filament_ramming();
+
+    command(MMU_CMD_U0);
+    manage_response(false, true);
+
+    BUZZ(200, 404);
+
+    // no active tool
+    extruder = MMU2_NO_TOOL;
+
+    set_runout_valid(false);
+
+    return true;
+  }
+
+  /**
+   * Unload sequence to optimize shape of the tip of the unloaded filament
+   */
+  void MMU2::filament_ramming() {
+    execute_extruder_sequence((const E_Step *)ramming_sequence, sizeof(ramming_sequence) / sizeof(E_Step));
+  }
+
+  void MMU2::execute_extruder_sequence(const E_Step * sequence, int steps) {
+
+    planner.synchronize();
+    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_LCD_MENU && MMU2_MENUS
+
+#endif // PRUSA_MMU2

Marlin/src/feature/mmu2/mmu2.h

@@ -0,0 +1,101 @@
+/**
+ * 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 <http://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(uint8_t index);
+  static void tool_change(const char* special);
+  static uint8_t get_current_tool();
+  static void set_filament_type(uint8_t index, uint8_t type);
+
+  #if HAS_LCD_MENU && ENABLED(MMU2_MENUS)
+    static bool unload();
+    static void load_filament(uint8_t);
+    static void load_all();
+    static bool load_filament_to_nozzle(uint8_t index);
+    static bool eject_filament(uint8_t index, bool recover);
+  #endif
+
+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, int argument);
+  static void tx_printf_P(const char* format, int argument1, 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);
+
+  #if HAS_LCD_MENU && ENABLED(MMU2_MENUS)
+    static void load_to_nozzle();
+    static void filament_ramming();
+    static void execute_extruder_sequence(const E_Step * sequence, int steps);
+  #endif
+
+  static void filament_runout();
+
+  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 last_request, next_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;

Marlin/src/feature/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 is filament 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'
+

Marlin/src/feature/pause.cpp

@@ -0,0 +1,687 @@
+/**
+ * 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 <http://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)
+
+#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 "../core/language.h"
+#include "../lcd/ultralcd.h"
+
+#if HAS_BUZZER
+  #include "../libs/buzzer.h"
+#endif
+
+#include "../libs/nozzle.h"
+#include "pause.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 filament_change_beep(const int8_t max_beep_count, const bool init=false) {
+
+    #if HAS_LCD_MENU
+      if (pause_mode == PAUSE_MODE_PAUSE_PRINT) return;
+    #endif
+
+    static millis_t next_buzz = 0;
+    static int8_t runout_beep = 0;
+
+    if (init) next_buzz = runout_beep = 0;
+
+    const millis_t ms = millis();
+    if (ELAPSED(ms, next_buzz)) {
+      if (max_beep_count < 0 || runout_beep < max_beep_count + 5) { // Only beep as long as we're supposed to
+        next_buzz = ms + ((max_beep_count < 0 || runout_beep < max_beep_count) ? 1000 : 500);
+        BUZZ(50, 880 - (runout_beep & 1) * 220);
+        runout_beep++;
+      }
+    }
+  }
+#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 PauseMode mode=PAUSE_MODE_SAME) {
+
+  #if ENABLED(PREVENT_COLD_EXTRUSION)
+    if (!DEBUGGING(DRYRUN) && thermalManager.targetTooColdToExtrude(active_extruder)) {
+      SERIAL_ECHO_MSG(STR_ERR_HOTEND_TOO_COLD);
+      return false;
+    }
+  #endif
+
+  #if HAS_LCD_MENU
+    lcd_pause_show_message(PAUSE_MESSAGE_HEATING, mode);
+  #else
+    UNUSED(mode);
+  #endif
+
+  return thermalManager.wait_for_hotend(active_extruder);
+}
+
+/**
+ * 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
+) {
+  #if !HAS_LCD_MENU
+    UNUSED(show_lcd);
+  #endif
+
+  if (!ensure_safe_temperature(mode)) {
+    #if HAS_LCD_MENU
+      if (show_lcd) lcd_pause_show_message(PAUSE_MESSAGE_STATUS, mode);
+    #endif
+    return false;
+  }
+
+  if (pause_for_user) {
+    #if HAS_LCD_MENU
+      if (show_lcd) lcd_pause_show_message(PAUSE_MESSAGE_INSERT, mode);
+    #endif
+    SERIAL_ECHO_MSG(_PMSG(STR_FILAMENT_CHANGE_INSERT));
+
+    #if HAS_BUZZER
+      filament_change_beep(max_beep_count, true);
+    #else
+      UNUSED(max_beep_count);
+    #endif
+
+    KEEPALIVE_STATE(PAUSED_FOR_USER);
+    #if ENABLED(HOST_PROMPT_SUPPORT)
+      const char tool = '0'
+        #if NUM_RUNOUT_SENSORS > 1
+          + active_extruder
+        #endif
+      ;
+      host_action_prompt_begin(PROMPT_USER_CONTINUE, PSTR("Load Filament T"), tool);
+      host_action_prompt_button(CONTINUE_STR);
+      host_action_prompt_show();
+    #endif
+    #if ENABLED(EXTENSIBLE_UI)
+      ExtUI::onUserConfirmRequired_P(PSTR("Load Filament"));
+    #endif
+    while (wait_for_user) {
+      #if HAS_BUZZER
+        filament_change_beep(max_beep_count);
+      #endif
+      idle_no_sleep();
+    }
+  }
+
+  #if HAS_LCD_MENU
+    if (show_lcd) lcd_pause_show_message(PAUSE_MESSAGE_LOAD, mode);
+  #endif
+
+  #if ENABLED(DUAL_X_CARRIAGE)
+    const int8_t saved_ext        = active_extruder;
+    const bool saved_ext_dup_mode = extruder_duplication_enabled;
+    active_extruder = DXC_ext;
+    extruder_duplication_enabled = false;
+  #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.
+    active_extruder = saved_ext;
+    extruder_duplication_enabled = saved_ext_dup_mode;
+    stepper.set_directions();
+  #endif
+
+  #if ENABLED(ADVANCED_PAUSE_CONTINUOUS_PURGE)
+
+    #if HAS_LCD_MENU
+      if (show_lcd) lcd_pause_show_message(PAUSE_MESSAGE_PURGE);
+    #endif
+
+    #if ENABLED(HOST_PROMPT_SUPPORT)
+      host_prompt_do(PROMPT_USER_CONTINUE, PSTR("Filament Purging..."), CONTINUE_STR);
+    #endif
+    #if ENABLED(EXTENSIBLE_UI)
+      ExtUI::onUserConfirmRequired_P(PSTR("Filament Purging..."));
+    #endif
+    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 HAS_LCD_MENU
+          if (show_lcd) lcd_pause_show_message(PAUSE_MESSAGE_PURGE);
+        #endif
+
+        // Extrude filament to get into hotend
+        unscaled_e_move(purge_length, ADVANCED_PAUSE_PURGE_FEEDRATE);
+      }
+
+      #if ENABLED(HOST_PROMPT_SUPPORT)
+        filament_load_host_prompt();  // Initiate another host prompt. (NOTE: host_response_handler may also do this!)
+      #endif
+
+      #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;
+          lcd_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 (false
+      #if HAS_LCD_MENU
+        || (show_lcd && pause_menu_response == PAUSE_RESPONSE_EXTRUDE_MORE)
+      #endif
+    );
+
+  #endif
+
+  return true;
+}
+
+/**
+ * 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
+) {
+  #if !HAS_LCD_MENU
+    UNUSED(show_lcd);
+  #endif
+
+  #if !BOTH(FILAMENT_UNLOAD_ALL_EXTRUDERS, MIXING_EXTRUDER)
+    constexpr float mix_multiplier = 1.0;
+  #endif
+
+  if (!ensure_safe_temperature(mode)) {
+    #if HAS_LCD_MENU
+      if (show_lcd) lcd_pause_show_message(PAUSE_MESSAGE_STATUS);
+    #endif
+    return false;
+  }
+
+  #if HAS_LCD_MENU
+    if (show_lcd) lcd_pause_show_message(PAUSE_MESSAGE_UNLOAD, mode);
+  #endif
+
+  // 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 E steppers for manual change
+  #if HAS_E_STEPPER_ENABLE
+    disable_e_stepper(active_extruder);
+    safe_delay(100);
+  #endif
+
+  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) {
+
+  #if !HAS_LCD_MENU
+    UNUSED(show_lcd);
+  #endif
+
+  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
+
+  #if ENABLED(HOST_PROMPT_SUPPORT)
+    host_prompt_open(PROMPT_INFO, PSTR("Pause"), DISMISS_STR);
+  #endif
+
+  if (!DEBUGGING(DRYRUN) && unload_length && thermalManager.targetTooColdToExtrude(active_extruder)) {
+    SERIAL_ECHO_MSG(STR_ERR_HOTEND_TOO_COLD);
+
+    #if HAS_LCD_MENU
+      if (show_lcd) { // Show status screen
+        lcd_pause_show_message(PAUSE_MESSAGE_STATUS);
+        LCD_MESSAGEPGM(MSG_M600_TOO_COLD);
+      }
+    #endif
+
+    return false; // unable to reach safe temperature
+  }
+
+  // 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) && FAN_COUNT > 0
+    thermalManager.set_fans_paused(true);
+  #endif
+
+  // Initial retract before move to filament change position
+  if (retract && thermalManager.hotEnoughToExtrude(active_extruder))
+    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_need_homing())
+    nozzle.park(2, park_point);
+
+  #if ENABLED(DUAL_X_CARRIAGE)
+    const int8_t saved_ext        = active_extruder;
+    const bool saved_ext_dup_mode = extruder_duplication_enabled;
+    active_extruder = DXC_ext;
+    extruder_duplication_enabled = false;
+  #endif
+
+  if (unload_length)   // Unload the filament
+    unload_filament(unload_length, show_lcd, PAUSE_MODE_CHANGE_FILAMENT);
+
+  #if ENABLED(DUAL_X_CARRIAGE)
+    active_extruder = saved_ext;
+    extruder_duplication_enabled = saved_ext_dup_mode;
+    stepper.set_directions();
+  #endif
+
+  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) {
+  #if HAS_LCD_MENU
+    lcd_pause_show_message(is_reload ? PAUSE_MESSAGE_INSERT : PAUSE_MESSAGE_WAITING);
+  #endif
+  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) {
+  bool nozzle_timed_out = false;
+
+  show_continue_prompt(is_reload);
+
+  #if HAS_BUZZER
+    filament_change_beep(max_beep_count, true);
+  #else
+    UNUSED(max_beep_count);
+  #endif
+
+  // Start the heater idle timers
+  const millis_t nozzle_timeout = (millis_t)(PAUSE_PARK_NOZZLE_TIMEOUT) * 1000UL;
+
+  HOTEND_LOOP() thermalManager.hotend_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;
+    active_extruder = DXC_ext;
+    extruder_duplication_enabled = false;
+  #endif
+
+  // Wait for filament insert by user and press button
+  KEEPALIVE_STATE(PAUSED_FOR_USER);
+  #if ENABLED(HOST_PROMPT_SUPPORT)
+    host_prompt_do(PROMPT_USER_CONTINUE, PSTR("Nozzle Parked"), CONTINUE_STR);
+  #endif
+  #if ENABLED(EXTENSIBLE_UI)
+    ExtUI::onUserConfirmRequired_P(PSTR("Nozzle Parked"));
+  #endif
+  wait_for_user = true;    // LCD click or M108 will clear this
+  while (wait_for_user) {
+    #if HAS_BUZZER
+      filament_change_beep(max_beep_count);
+    #endif
+
+    // If the nozzle has timed out...
+    if (!nozzle_timed_out)
+      HOTEND_LOOP() nozzle_timed_out |= thermalManager.hotend_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) {
+      #if HAS_LCD_MENU
+        lcd_pause_show_message(PAUSE_MESSAGE_HEAT);
+      #endif
+      SERIAL_ECHO_MSG(_PMSG(STR_FILAMENT_CHANGE_HEAT));
+
+      #if ENABLED(HOST_PROMPT_SUPPORT)
+        host_prompt_do(PROMPT_USER_CONTINUE, PSTR("HeaterTimeout"), PSTR("Reheat"));
+      #endif
+
+      #if ENABLED(EXTENSIBLE_UI)
+        ExtUI::onUserConfirmRequired_P(PSTR("HeaterTimeout"));
+      #endif
+
+      wait_for_user_response(0, true); // Wait for LCD click or M108
+
+      #if ENABLED(HOST_PROMPT_SUPPORT)
+        host_prompt_do(PROMPT_INFO, PSTR("Reheating"));
+      #endif
+      #if ENABLED(EXTENSIBLE_UI)
+        ExtUI::onStatusChanged(PSTR("Reheating..."));
+      #endif
+
+      // 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();
+
+      // Show the prompt to continue
+      show_continue_prompt(is_reload);
+
+      // Start the heater idle timers
+      const millis_t nozzle_timeout = (millis_t)(PAUSE_PARK_NOZZLE_TIMEOUT) * 1000UL;
+
+      HOTEND_LOOP() thermalManager.hotend_idle[e].start(nozzle_timeout);
+      #if ENABLED(HOST_PROMPT_SUPPORT)
+        host_prompt_do(PROMPT_USER_CONTINUE, PSTR("Reheat Done"), CONTINUE_STR);
+      #endif
+      #if ENABLED(EXTENSIBLE_UI)
+        ExtUI::onUserConfirmRequired_P(PSTR("Reheat finished."));
+      #endif
+      wait_for_user = true;
+      nozzle_timed_out = false;
+
+      #if HAS_BUZZER
+        filament_change_beep(max_beep_count, true);
+      #endif
+    }
+    idle_no_sleep();
+  }
+  #if ENABLED(DUAL_X_CARRIAGE)
+    active_extruder = saved_ext;
+    extruder_duplication_enabled = saved_ext_dup_mode;
+    stepper.set_directions();
+  #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"
+ * - 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.
+ * - Move the nozzle back to resume_position
+ * - 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*/ DXC_ARGS) {
+  /*
+  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.hotend_idle[e].timed_out;
+    thermalManager.reset_hotend_idle_timer(e);
+  }
+
+  if (nozzle_timed_out || thermalManager.hotEnoughToExtrude(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 HAS_LCD_MENU
+    lcd_pause_show_message(PAUSE_MESSAGE_RESUME);
+  #endif
+
+  // 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));
+
+  // 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));
+
+  #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));
+
+  #if HAS_LCD_MENU
+    lcd_pause_show_message(PAUSE_MESSAGE_STATUS);
+  #endif
+
+  #ifdef ACTION_ON_RESUMED
+    host_action_resumed();
+  #elif defined(ACTION_ON_RESUME)
+    host_action_resume();
+  #endif
+
+  --did_pause_print;
+
+  #if ENABLED(HOST_PROMPT_SUPPORT)
+    host_prompt_open(PROMPT_INFO, PSTR("Resuming"), DISMISS_STR);
+  #endif
+
+  #if ENABLED(SDSUPPORT)
+    if (did_pause_print) {
+      card.startFileprint();
+      --did_pause_print;
+    }
+  #endif
+
+  #if ENABLED(ADVANCED_PAUSE_FANS_PAUSE) && FAN_COUNT > 0
+    thermalManager.set_fans_paused(false);
+  #endif
+
+  #if HAS_FILAMENT_SENSOR
+    runout.reset();
+  #endif
+
+  // Resume the print job timer if it was running
+  if (print_job_timer.isPaused()) print_job_timer.start();
+
+  #if HAS_DISPLAY
+    ui.reset_status();
+    #if HAS_LCD_MENU
+      ui.return_to_status();
+    #endif
+  #endif
+}
+
+#endif // ADVANCED_PAUSE_FEATURE

Marlin/src/feature/pause.h

@@ -0,0 +1,101 @@
+/**
+ * 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 <http://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_PAUSING,
+  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
+#else
+  #define DXC_PARAMS
+  #define DXC_ARGS
+  #define DXC_PASS
+#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 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
+);
+
+#endif // ADVANCED_PAUSE_FEATURE

Marlin/src/feature/power.cpp

@@ -0,0 +1,122 @@
+/**
+ * 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 <http://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"
+
+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 ENABLED(AUTO_POWER_CHAMBER_FAN)
+    if (thermalManager.chamberfan_speed) return true;
+  #endif
+
+  // 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_HEATED_BED
+      || thermalManager.temp_bed.soft_pwm_amount > 0
+    #endif
+    #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) return true;
+
+  #if HAS_HEATED_BED
+    if (thermalManager.degTargetBed() > 0) return true;
+  #endif
+
+  #if HOTENDS && 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 + (POWER_TIMEOUT) * 1000UL))
+      power_off();
+  }
+}
+
+void Power::power_on() {
+  lastPowerOn = millis();
+  if (!powersupply_on) {
+    PSU_PIN_ON();
+
+    #if HAS_TRINAMIC_CONFIG
+      delay(PSU_POWERUP_DELAY); // Wait for power to settle
+      restore_stepper_drivers();
+    #endif
+  }
+}
+
+void Power::power_off() {
+  if (powersupply_on) PSU_PIN_OFF();
+}
+
+#endif // AUTO_POWER_CONTROL

Marlin/src/feature/power.h

@@ -0,0 +1,40 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://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();
+  private:
+    static millis_t lastPowerOn;
+    static bool is_power_needed();
+};
+
+extern Power powerManager;

Marlin/src/feature/powerloss.cpp

@@ -0,0 +1,566 @@
+/**
+ * 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 <http://www.gnu.org/licenses/>.
+ *
+ */
+
+/**
+ * power_loss_recovery.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];
+
+#include "../sd/cardreader.h"
+#include "../lcd/ultralcd.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_RETRACT_LEN
+  #define POWER_LOSS_RETRACT_LEN 0
+#endif
+#ifndef POWER_LOSS_ZRAISE
+  #define POWER_LOSS_ZRAISE 2
+#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 purge();
+    queue.inject_P(PSTR("M1000 S"));
+  }
+}
+
+/**
+ * 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*/) {
+
+  #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;
+    #if HAS_HOME_OFFSET
+      info.home_offset = home_offset;
+    #endif
+    #if HAS_POSITION_SHIFT
+      info.position_shift = position_shift;
+    #endif
+    info.feedrate = uint16_t(feedrate_mm_s * 60.0f);
+
+    #if EXTRUDERS > 1
+      info.active_extruder = active_extruder;
+    #endif
+
+    #if DISABLED(NO_VOLUMETRICS)
+      info.volumetric_enabled = parser.volumetric_enabled;
+      #if EXTRUDERS > 1
+        for (int8_t e = 0; e < EXTRUDERS; e++) info.filament_size[e] = planner.filament_size[e];
+      #else
+        if (parser.volumetric_enabled) info.filament_size = planner.filament_size[active_extruder];
+      #endif
+    #endif
+
+    #if EXTRUDERS
+      HOTEND_LOOP() info.target_temperature[e] = thermalManager.temp_hotend[e].target;
+    #endif
+
+    #if HAS_HEATED_BED
+      info.target_temperature_bed = thermalManager.temp_bed.target;
+    #endif
+
+    #if FAN_COUNT
+      COPY(info.fan_speed, thermalManager.fan_speed);
+    #endif
+
+    #if HAS_LEVELING
+      info.leveling = planner.leveling_active;
+      info.fade = (
+        #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
+          planner.z_fade_height
+        #else
+          0
+        #endif
+      );
+    #endif
+
+    #if ENABLED(GRADIENT_MIX)
+      memcpy(&info.gradient, &mixer.gradient, sizeof(info.gradient));
+    #endif
+
+    #if ENABLED(FWRETRACT)
+      COPY(info.retract, fwretract.current_retract);
+      info.retract_hop = fwretract.current_hop;
+    #endif
+
+    // Relative axis modes
+    info.axis_relative = gcode.axis_relative;
+
+    // Elapsed print job time
+    info.print_job_elapsed = print_job_timer.duration();
+
+    write();
+  }
+}
+
+#if PIN_EXISTS(POWER_LOSS)
+
+  void PrintJobRecovery::_outage() {
+    #if ENABLED(BACKUP_POWER_SUPPLY)
+      static bool lock = false;
+      if (lock) return; // No re-entrance from idle() during raise_z()
+      lock = true;
+    #endif
+    if (IS_SD_PRINTING()) save(true);
+    #if ENABLED(BACKUP_POWER_SUPPLY)
+      raise_z();
+    #endif
+
+    kill(GET_TEXT(MSG_OUTAGE_RECOVERY));
+  }
+
+  #if ENABLED(BACKUP_POWER_SUPPLY)
+
+    void PrintJobRecovery::raise_z() {
+      // Disable all heaters to reduce power loss
+      thermalManager.disable_all_heaters();
+      quickstop_stepper();
+      // Raise Z axis
+      gcode.process_subcommands_now_P(PSTR("G91\nG0 Z" STRINGIFY(POWER_LOSS_ZRAISE)));
+      planner.synchronize();
+    }
+
+  #endif
+
+#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() {
+
+  const uint32_t resume_sdpos = info.sdpos; // Get here before the stepper ISR overwrites it
+
+  #if HAS_LEVELING
+    // Make sure leveling is off before any G92 and G28
+    gcode.process_subcommands_now_P(PSTR("M420 S0 Z0"));
+  #endif
+
+  // Reset E, raise Z, home XY...
+  gcode.process_subcommands_now_P(PSTR("G92.9 E0"
+    #if Z_HOME_DIR > 0
+
+      // If Z homing goes to max, just reset E and home all
+      "\n"
+      "G28R0"
+      #if ENABLED(MARLIN_DEV_MODE)
+        "S"
+      #endif
+
+    #else // "G92.9 E0 ..."
+
+      // Set Z to 0, raise Z by RECOVERY_ZRAISE, and Home (XY only for Cartesian)
+      // with no raise. (Only do simulated homing in Marlin Dev Mode.)
+      #if ENABLED(BACKUP_POWER_SUPPLY)
+        "Z" STRINGIFY(POWER_LOSS_ZRAISE)    // Z-axis was already raised at outage
+      #else
+        "Z0\n"                              // Set Z=0
+        "G1Z" STRINGIFY(POWER_LOSS_ZRAISE)  // Raise Z
+      #endif
+      "\n"
+
+      "G28R0"
+      #if ENABLED(MARLIN_DEV_MODE)
+        "S"
+      #elif !IS_KINEMATIC
+        "XY"
+      #endif
+    #endif
+  ));
+
+  // Pretend that all axes are homed
+  axis_homed = axis_known_position = xyz_bits;
+
+  char cmd[MAX_CMD_SIZE+16], str_1[16], str_2[16];
+
+  // Select the previously active tool (with no_move)
+  #if EXTRUDERS > 1
+    sprintf_P(cmd, PSTR("T%i S"), info.active_extruder);
+    gcode.process_subcommands_now(cmd);
+  #endif
+
+  // Recover volumetric extrusion state
+  #if DISABLED(NO_VOLUMETRICS)
+    #if EXTRUDERS > 1
+      for (int8_t e = 0; e < EXTRUDERS; e++) {
+        dtostrf(info.filament_size[e], 1, 3, str_1);
+        sprintf_P(cmd, PSTR("M200 T%i D%s"), e, 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) {
+        dtostrf(info.filament_size, 1, 3, str_1);
+        sprintf_P(cmd, PSTR("M200 D%s"), str_1);
+        gcode.process_subcommands_now(cmd);
+      }
+    #endif
+  #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 HOTENDS
+    HOTEND_LOOP() {
+      const int16_t et = info.target_temperature[e];
+      if (et) {
+        #if HOTENDS > 1
+          sprintf_P(cmd, PSTR("T%i"), e);
+          gcode.process_subcommands_now(cmd);
+        #endif
+        sprintf_P(cmd, PSTR("M109 S%i"), et);
+        gcode.process_subcommands_now(cmd);
+      }
+    }
+  #endif
+
+  // Restore print cooling fan speeds
+  FANS_LOOP(i) {
+    uint8_t f = info.fan_speed[i];
+    if (f) {
+      sprintf_P(cmd, PSTR("M106 P%i S%i"), i, f);
+      gcode.process_subcommands_now(cmd);
+    }
+  }
+
+  // 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.leveling) {
+      sprintf_P(cmd, PSTR("M420 S%i Z%s"), int(info.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
+
+  // Extrude and retract to clean the nozzle
+  #if POWER_LOSS_PURGE_LEN
+    //sprintf_P(cmd, PSTR("G1 E%d F200"), POWER_LOSS_PURGE_LEN);
+    //gcode.process_subcommands_now(cmd);
+    gcode.process_subcommands_now_P(PSTR("G1 E" STRINGIFY(POWER_LOSS_PURGE_LEN) " F200"));
+  #endif
+
+  #if POWER_LOSS_RETRACT_LEN
+    sprintf_P(cmd, PSTR("G1 E%d F3000"), POWER_LOSS_PURGE_LEN - (POWER_LOSS_RETRACT_LEN));
+    gcode.process_subcommands_now(cmd);
+  #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
+    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);
+
+  // Un-retract
+  #if POWER_LOSS_PURGE_LEN
+    //sprintf_P(cmd, PSTR("G1 E%d F3000"), POWER_LOSS_PURGE_LEN);
+    //gcode.process_subcommands_now(cmd);
+    gcode.process_subcommands_now_P(PSTR("G1 E" STRINGIFY(POWER_LOSS_PURGE_LEN) " F3000"));
+  #endif
+
+  // 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);
+
+  // Relative axis modes
+  gcode.axis_relative = info.axis_relative;
+
+  #if HAS_HOME_OFFSET
+    home_offset = info.home_offset;
+  #endif
+  #if HAS_POSITION_SHIFT
+    position_shift = info.position_shift;
+  #endif
+  #if HAS_HOME_OFFSET || HAS_POSITION_SHIFT
+    LOOP_XYZ(i) update_workspace_offset((AxisEnum)i);
+  #endif
+
+  // Resume the SD file from the last position
+  char *fn = info.sd_filename;
+  extern const char M23_STR[];
+  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);
+}
+
+#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_ECHO(info.current_position[i]);
+        }
+        DEBUG_EOL();
+
+        #if HAS_HOME_OFFSET
+          DEBUG_ECHOPGM("home_offset: ");
+          LOOP_XYZ(i) {
+            if (i) DEBUG_CHAR(',');
+            DEBUG_ECHO(info.home_offset[i]);
+          }
+          DEBUG_EOL();
+        #endif
+
+        #if HAS_POSITION_SHIFT
+          DEBUG_ECHOPGM("position_shift: ");
+          LOOP_XYZ(i) {
+            if (i) DEBUG_CHAR(',');
+            DEBUG_ECHO(info.position_shift[i]);
+          }
+          DEBUG_EOL();
+        #endif
+
+        DEBUG_ECHOLNPAIR("feedrate: ", info.feedrate);
+
+        #if EXTRUDERS > 1
+          DEBUG_ECHOLNPAIR("active_extruder: ", int(info.active_extruder));
+        #endif
+
+        #if HOTENDS
+          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 FAN_COUNT
+          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.leveling), "\n fade: ", int(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);
+      }
+      else
+        DEBUG_ECHOLNPGM("INVALID DATA");
+    }
+    DEBUG_ECHOLNPGM("---");
+  }
+
+#endif // DEBUG_POWER_LOSS_RECOVERY
+
+#endif // POWER_LOSS_RECOVERY

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 <http://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+/**
+ * power_loss_recovery.h - Resume an SD print after power-loss
+ */
+
+#include "../sd/cardreader.h"
+#include "../inc/MarlinConfig.h"
+
+#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;
+
+  #if HAS_HOME_OFFSET
+    xyz_pos_t home_offset;
+  #endif
+  #if HAS_POSITION_SHIFT
+    xyz_pos_t position_shift;
+  #endif
+
+  uint16_t feedrate;
+
+  #if EXTRUDERS > 1
+    uint8_t active_extruder;
+  #endif
+
+  #if DISABLED(NO_VOLUMETRICS)
+    bool volumetric_enabled;
+    #if EXTRUDERS > 1
+      float filament_size[EXTRUDERS];
+    #else
+      float filament_size;
+    #endif
+  #endif
+
+  #if HOTENDS
+    int16_t target_temperature[HOTENDS];
+  #endif
+
+  #if HAS_HEATED_BED
+    int16_t target_temperature_bed;
+  #endif
+
+  #if FAN_COUNT
+    uint8_t fan_speed[FAN_COUNT];
+  #endif
+
+  #if HAS_LEVELING
+    bool leveling;
+    float fade;
+  #endif
+
+  #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];
+    #if ENABLED(GRADIENT_MIX)
+      gradient_t gradient;
+    #endif
+  #endif
+
+  // Relative axis modes
+  uint8_t axis_relative;
+
+  // SD Filename and position
+  char sd_filename[MAXPATHNAMELENGTH];
+  volatile uint32_t sdpos;
+
+  // Job elapsed time
+  millis_t print_job_elapsed;
+
+  uint8_t 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
+
+    static void init();
+    static void prepare();
+
+    static inline void setup() {
+      #if PIN_EXISTS(POWER_LOSS)
+        #if ENABLED(POWER_LOSS_PULL)
+          #if POWER_LOSS_STATE == LOW
+            SET_INPUT_PULLUP(POWER_LOSS_PIN);
+          #else
+            SET_INPUT_PULLDOWN(POWER_LOSS_PIN);
+          #endif
+        #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(); card.autostart_index = 0; }
+
+    static void load();
+    static void save(const bool force=ENABLED(SAVE_EACH_CMD_MODE));
+
+  #if PIN_EXISTS(POWER_LOSS)
+    static inline void outage() {
+      if (enabled && READ(POWER_LOSS_PIN) == POWER_LOSS_STATE)
+        _outage();
+    }
+  #endif
+
+  static inline bool valid() { return info.valid_head && info.valid_head == info.valid_foot; }
+
+  #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 raise_z();
+  #endif
+
+  #if PIN_EXISTS(POWER_LOSS)
+    static void _outage();
+  #endif
+};
+
+extern PrintJobRecovery recovery;

Marlin/src/feature/probe_temp_comp.cpp

@@ -0,0 +1,223 @@
+/**
+ * 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 <http://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[ProbeTempComp::cali_info_init[TSI_PROBE].measurements],  // = {0}
+        ProbeTempComp::z_offsets_bed[ProbeTempComp::cali_info_init[TSI_BED].measurements];      // = {0}
+
+#if ENABLED(USE_TEMP_EXT_COMPENSATION)
+  int16_t ProbeTempComp::z_offsets_ext[ProbeTempComp::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] = {
+  ProbeTempComp::cali_info_init[TSI_PROBE], ProbeTempComp::cali_info_init[TSI_BED]
+  #if ENABLED(USE_TEMP_EXT_COMPENSATION)
+    , ProbeTempComp::cali_info_init[TSI_EXT]
+  #endif
+};
+
+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,
+                end_temp = cali_info[tsi].end_temp,
+                res_temp = cali_info[tsi].temp_res;
+  const int16_t * const data = sensor_z_offsets[tsi];
+
+  if (temp <= start_temp) return 0.0f;
+  if (temp >= end_temp) return static_cast<float>(data[measurements - 1]) / 1000.0f;
+
+  // Linear interpolation
+  int16_t val1 = 0, val2 = data[0];
+  uint8_t idx = 0;
+  float meas_temp = start_temp + res_temp;
+  while (meas_temp < temp) {
+    if (++idx >= measurements) return static_cast<float>(val2) / 1000.0f;
+    meas_temp += res_temp;
+    val1 = val2;
+    val2 = data[idx];
+  }
+  const float factor = (meas_temp - temp) / static_cast<float>(res_temp);
+  return (static_cast<float>(val2) - static_cast<float>(val2 - val1) * factor) / 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

Marlin/src/feature/probe_temp_comp.h

@@ -0,0 +1,116 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://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.
+ */
+class ProbeTempComp {
+  public:
+
+    static constexpr temp_calib_t cali_info_init[TSI_COUNT] = {
+        {  10,  5,  30,  30 + 10 *  5 },       // Probe
+        {  10,  5,  60,  60 + 10 *  5 },       // Bed
+      #if ENABLED(USE_TEMP_EXT_COMPENSATION)
+        {  20,  5, 180, 180 +  5 * 20 }        // Extruder
+      #endif
+    };
+    static const temp_calib_t cali_info[TSI_COUNT];
+
+    // Where to park nozzle to wait for probe cooldown
+    static constexpr float park_point_x = PTC_PARK_POS_X,
+                           park_point_y = PTC_PARK_POS_Y,
+                           park_point_z = PTC_PARK_POS_Z,
+                           // XY coordinates of nozzle for probing the bed
+                           measure_point_x     = PTC_PROBE_POS_X,   // Coordinates to probe
+                           measure_point_y     = PTC_PROBE_POS_Y;
+                           //measure_point_x     = 12.0f,           // Coordinates to probe on MK52 magnetic heatbed
+                           //measure_point_y     =  7.3f;
+
+    static constexpr int  max_bed_temp         = PTC_MAX_BED_TEMP,  // Max temperature to avoid heating errors
+                          probe_calib_bed_temp = max_bed_temp,      // Bed temperature while calibrating probe
+                          bed_calib_probe_temp = 30;                // 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);
+      #if ENABLED(USE_TEMP_EXT_COMPENSATION)
+        clear_offsets(TSI_EXT);
+      #endif
+    }
+    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;

Marlin/src/feature/runout.cpp

@@ -0,0 +1,132 @@
+/**
+ * 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 <http://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
+
+/**
+ * Called by FilamentSensorSwitch::run when filament is detected.
+ * Called by FilamentSensorEncoder::block_completed when motion is detected.
+ */
+void FilamentSensorBase::filament_present(const uint8_t extruder) {
+  runout.filament_present(extruder); // calls response.filament_present(extruder)
+}
+
+#if ENABLED(FILAMENT_MOTION_SENSOR)
+  uint8_t FilamentSensorEncoder::motion_detected;
+#endif
+
+#ifdef FILAMENT_RUNOUT_DISTANCE_MM
+  float RunoutResponseDelayed::runout_distance_mm = FILAMENT_RUNOUT_DISTANCE_MM;
+  volatile float RunoutResponseDelayed::runout_mm_countdown[EXTRUDERS];
+#else
+  int8_t RunoutResponseDebounced::runout_count; // = 0
+#endif
+
+//
+// Filament Runout event handler
+//
+#include "../MarlinCore.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 ENABLED(ADVANCED_PAUSE_FEATURE)
+    if (did_pause_print) return;  // Action already in progress. Purge triggered repeated runout.
+  #endif
+
+  #if ENABLED(EXTENSIBLE_UI)
+    ExtUI::onFilamentRunout(ExtUI::getActiveTool());
+  #endif
+
+  #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")
+        #if ENABLED(ADVANCED_PAUSE_FEATURE)
+          || strstr(FILAMENT_RUNOUT_SCRIPT, "M25")
+        #endif
+      )
+    ) {
+      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

Marlin/src/feature/runout.h

@@ -0,0 +1,351 @@
+/**
+ * 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 <http://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 "../inc/MarlinConfig.h"
+
+#if ENABLED(EXTENSIBLE_UI)
+  #include "../lcd/extui/ui_api.h"
+#endif
+
+#if ENABLED(ADVANCED_PAUSE_FEATURE)
+  #include "pause.h"
+#endif
+
+//#define FILAMENT_RUNOUT_SENSOR_DEBUG
+#ifndef FILAMENT_RUNOUT_THRESHOLD
+  #define FILAMENT_RUNOUT_THRESHOLD 5
+#endif
+
+void event_filament_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);
+    }
+
+    #ifdef FILAMENT_RUNOUT_DISTANCE_MM
+      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()
+        #if ENABLED(ADVANCED_PAUSE_FEATURE)
+          || did_pause_print
+        #endif
+      )) {
+        #ifdef FILAMENT_RUNOUT_DISTANCE_MM
+          cli(); // Prevent RunoutResponseDelayed::block_completed from accumulating here
+        #endif
+        response.run();
+        sensor.run();
+        const bool ran_out = response.has_run_out();
+        #ifdef FILAMENT_RUNOUT_DISTANCE_MM
+          sei();
+        #endif
+        if (ran_out) {
+          filament_ran_out = true;
+          event_filament_runout();
+          planner.synchronize();
+        }
+      }
+    }
+};
+
+/*************************** FILAMENT PRESENCE SENSORS ***************************/
+
+class FilamentSensorBase {
+  protected:
+    static void filament_present(const uint8_t extruder);
+
+  public:
+    static inline void setup() {
+      #if ENABLED(FIL_RUNOUT_PULLUP)
+        #define INIT_RUNOUT_PIN(P) SET_INPUT_PULLUP(P)
+      #elif ENABLED(FIL_RUNOUT_PULLDOWN)
+        #define INIT_RUNOUT_PIN(P) SET_INPUT_PULLDOWN(P)
+      #else
+        #define INIT_RUNOUT_PIN(P) SET_INPUT(P)
+      #endif
+
+      #define _INIT_RUNOUT(N) INIT_RUNOUT_PIN(FIL_RUNOUT##N##_PIN);
+      REPEAT_S(1, INCREMENT(NUM_RUNOUT_SENSORS), _INIT_RUNOUT)
+      #undef _INIT_RUNOUT
+    }
+
+    // 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()
+        #if DISABLED(FIL_RUNOUT_INVERTING)
+          ^ uint8_t(_BV(NUM_RUNOUT_SENSORS) - 1)
+        #endif
+      );
+    }
+
+  #undef INIT_RUNOUT_PIN
+};
+
+#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);
+        #endif
+
+        if (true
+          #if NUM_RUNOUT_SENSORS > 1
+            #if ENABLED(DUAL_X_CARRIAGE)
+              && (dual_x_carriage_mode == DXC_DUPLICATION_MODE || dual_x_carriage_mode == DXC_MIRRORED_MODE)
+            #elif ENABLED(MULTI_NOZZLE_DUPLICATION)
+              && extruder_duplication_enabled
+            #else
+              && false
+            #endif
+          #endif
+        ) return runout_states;               // Any extruder
+
+        #if NUM_RUNOUT_SENSORS > 1
+          return TEST(runout_states, extruder); // Specific extruder
+        #endif
+      }
+
+    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 *********************************/
+
+#ifdef FILAMENT_RUNOUT_DISTANCE_MM
+
+  // 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
+          #if ENABLED(ADVANCED_PAUSE_FEATURE)
+            || did_pause_print // Allow pause purge move to re-trigger runout state
+          #endif
+        ) {
+          // 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 // !FILAMENT_RUNOUT_DISTANCE_MM
+
+  // 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 // !FILAMENT_RUNOUT_DISTANCE_MM
+
+/********************************* TEMPLATE SPECIALIZATION *********************************/
+
+typedef TFilamentMonitor<
+  #ifdef FILAMENT_RUNOUT_DISTANCE_MM
+    RunoutResponseDelayed,
+    #if ENABLED(FILAMENT_MOTION_SENSOR)
+      FilamentSensorEncoder
+    #else
+      FilamentSensorSwitch
+    #endif
+  #else
+    RunoutResponseDebounced, FilamentSensorSwitch
+  #endif
+> FilamentMonitor;
+
+extern FilamentMonitor runout;

Marlin/src/feature/snmm.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 <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../inc/MarlinConfig.h"
+
+#if ENABLED(MK2_MULTIPLEXER)
+
+#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 // MK2_MULTIPLEXER

Marlin/src/feature/snmm.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 <http://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+void select_multiplexed_stepper(const uint8_t e);

Marlin/src/feature/solenoid.cpp

@@ -0,0 +1,98 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://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
+
+#if ENABLED(MANUAL_SOLENOID_CONTROL)
+  #define HAS_SOLENOID(N) HAS_SOLENOID_##N
+#else
+  #define HAS_SOLENOID(N) (HAS_SOLENOID_##N && EXTRUDERS > N)
+#endif
+
+// Used primarily with MANUAL_SOLENOID_CONTROL
+static void set_solenoid(const uint8_t num, const bool active) {
+  const uint8_t value = active ? HIGH : LOW;
+  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;
+  }
+}
+
+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

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 <http://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);

Marlin/src/feature/spindle_laser.cpp

@@ -0,0 +1,101 @@
+/**
+ * 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 <http://www.gnu.org/licenses/>.
+ *
+ */
+
+/**
+ * feature/spindle_laser.cpp
+ */
+
+#include "../inc/MarlinConfig.h"
+
+#if HAS_CUTTER
+
+#include "spindle_laser.h"
+
+SpindleLaser cutter;
+
+cutter_power_t SpindleLaser::power; // = 0
+
+#define SPINDLE_LASER_PWM_OFF ((SPINDLE_LASER_PWM_INVERT) ? 255 : 0)
+
+void SpindleLaser::init() {
+  OUT_WRITE(SPINDLE_LASER_ENA_PIN, !SPINDLE_LASER_ACTIVE_HIGH); // Init spindle to off
+  #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(SPINDLE_LASER_PWM)
+
+  /**
+   * ocr_val_mode() is used for debugging and to get the points needed to compute the RPM vs ocr_val line
+   *
+   * it accepts inputs of 0-255
+   */
+  void SpindleLaser::set_ocr(const uint8_t ocr) {
+    WRITE(SPINDLE_LASER_ENA_PIN, SPINDLE_LASER_ACTIVE_HIGH); // turn spindle on (active low)
+    analogWrite(pin_t(SPINDLE_LASER_PWM_PIN), ocr ^ SPINDLE_LASER_PWM_OFF);
+  }
+
+#endif
+
+void SpindleLaser::apply_power(const cutter_power_t inpow) {
+  static cutter_power_t last_power_applied = 0;
+  if (inpow == last_power_applied) return;
+  last_power_applied = inpow;
+  #if ENABLED(SPINDLE_LASER_PWM)
+    if (enabled()) {
+      #define _scaled(F) ((F - (SPEED_POWER_INTERCEPT)) * inv_slope)
+      constexpr float inv_slope = RECIPROCAL(SPEED_POWER_SLOPE),
+                      min_ocr = _scaled(SPEED_POWER_MIN),
+                      max_ocr = _scaled(SPEED_POWER_MAX);
+      int16_t ocr_val;
+           if (inpow <= SPEED_POWER_MIN) ocr_val = min_ocr;       // Use minimum if set below
+      else if (inpow >= SPEED_POWER_MAX) ocr_val = max_ocr;       // Use maximum if set above
+      else ocr_val = _scaled(inpow);                              // Use calculated OCR value
+      set_ocr(ocr_val & 0xFF);                                    // ...limited to Atmel PWM max
+    }
+    else {
+      WRITE(SPINDLE_LASER_ENA_PIN, !SPINDLE_LASER_ACTIVE_HIGH);   // Turn spindle off (active low)
+      analogWrite(pin_t(SPINDLE_LASER_PWM_PIN), SPINDLE_LASER_PWM_OFF);  // Only write low byte
+    }
+  #else
+    WRITE(SPINDLE_LASER_ENA_PIN, (SPINDLE_LASER_ACTIVE_HIGH) ? enabled() : !enabled());
+  #endif
+}
+
+#if ENABLED(SPINDLE_CHANGE_DIR)
+
+  void SpindleLaser::set_direction(const bool reverse) {
+    const bool dir_state = (reverse == SPINDLE_INVERT_DIR); // Forward (M3) HIGH when not inverted
+    #if ENABLED(SPINDLE_STOP_ON_DIR_CHANGE)
+      if (enabled() && READ(SPINDLE_DIR_PIN) != dir_state) disable();
+    #endif
+    WRITE(SPINDLE_DIR_PIN, dir_state);
+  }
+
+#endif
+
+#endif // HAS_CUTTER

Marlin/src/feature/spindle_laser.h

@@ -0,0 +1,95 @@
+/**
+ * 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 <http://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+/**
+ * feature/spindle_laser.h
+ * Support for Laser Power or Spindle Power & Direction
+ */
+
+#include "../inc/MarlinConfig.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)
+
+#if SPEED_POWER_MAX > 255
+  typedef uint16_t cutter_power_t;
+  #define CUTTER_MENU_TYPE uint16_5
+#else
+  typedef uint8_t cutter_power_t;
+  #define CUTTER_MENU_TYPE uint8
+#endif
+
+class SpindleLaser {
+public:
+  static cutter_power_t power;
+  static inline uint8_t powerPercent(const uint8_t pp) { return ui8_to_percent(pp); } // for display
+
+  static void init();
+
+  static inline bool enabled() { return !!power; }
+
+  static inline void set_power(const cutter_power_t pwr) { power = pwr; }
+
+  static inline void refresh() { apply_power(power); }
+
+  static inline void set_enabled(const bool enable) {
+    const bool was = enabled();
+    set_power(enable ? 255 : 0);
+    if (was != enable) power_delay();
+  }
+
+  static void apply_power(const cutter_power_t inpow);
+
+  //static bool active() { return READ(SPINDLE_LASER_ENA_PIN) == SPINDLE_LASER_ACTIVE_HIGH; }
+
+  static void update_output();
+
+  #if ENABLED(SPINDLE_LASER_PWM)
+    static void set_ocr(const uint8_t ocr);
+    static inline void set_ocr_power(const cutter_power_t pwr) { power = pwr; set_ocr(pwr); }
+  #endif
+
+  // Wait for spindle to spin up or spin down
+  static inline void power_delay() {
+    #if SPINDLE_LASER_POWERUP_DELAY || SPINDLE_LASER_POWERDOWN_DELAY
+      safe_delay(enabled() ? SPINDLE_LASER_POWERUP_DELAY : SPINDLE_LASER_POWERDOWN_DELAY);
+    #endif
+  }
+
+  #if ENABLED(SPINDLE_CHANGE_DIR)
+    static void set_direction(const bool reverse);
+  #else
+    static inline void set_direction(const bool) {}
+  #endif
+
+  static inline void disable() { set_enabled(false); }
+  static inline void enable_forward() { set_direction(false); set_enabled(true); }
+  static inline void enable_reverse() { set_direction(true); set_enabled(true); }
+
+};
+
+extern SpindleLaser cutter;

Marlin/src/feature/tmc_util.h

@@ -0,0 +1,403 @@
+/**
+ * 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 <http://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+#include "../inc/MarlinConfig.h"
+#include "../lcd/ultralcd.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 }
+
+#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 {
+      #if HAS_STEALTHCHOP
+        bool stealthChop_enabled = false;
+      #endif
+      #if ENABLED(HYBRID_THRESHOLD)
+        uint8_t hybrid_thrs = 0;
+      #endif
+      #if USE_SENSORLESS
+        int16_t homing_thrs = 0;
+      #endif
+    } 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);
+    }
+
+    #if HAS_STEALTHCHOP
+      inline void refresh_stepping_mode() { this->en_pwm_mode(this->stored.stealthChop_enabled); }
+      inline bool get_stealthChop_status() { return this->en_pwm_mode(); }
+    #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]));
+        #if HAS_LCD_MENU
+          this->stored.hybrid_thrs = thrs;
+        #endif
+      }
+    #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);
+        #if HAS_LCD_MENU
+          this->stored.homing_thrs = sgt_val;
+        #endif
+      }
+      #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(const uint16_t RX, const uint16_t TX, const float RS, const uint8_t, const bool has_rx=true) :
+      TMC2208Stepper(RX, TX, RS, has_rx)
+      {}
+    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);
+    }
+
+    #if HAS_STEALTHCHOP
+      inline void refresh_stepping_mode() { en_spreadCycle(!this->stored.stealthChop_enabled); }
+      inline bool get_stealthChop_status() { return !this->en_spreadCycle(); }
+    #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]));
+        #if HAS_LCD_MENU
+          this->stored.hybrid_thrs = thrs;
+        #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
+    #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, const bool) :
+      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);
+    }
+
+    #if HAS_STEALTHCHOP
+      inline void refresh_stepping_mode() { en_spreadCycle(!this->stored.stealthChop_enabled); }
+      inline bool get_stealthChop_status() { return !this->en_spreadCycle(); }
+    #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]));
+        #if HAS_LCD_MENU
+          this->stored.hybrid_thrs = thrs;
+        #endif
+      }
+    #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);
+        #if HAS_LCD_MENU
+          this->stored.homing_thrs = sgt_val;
+        #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 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);
+    }
+
+    #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);
+        #if HAS_LCD_MENU
+          this->stored.homing_thrs = sgt_val;
+        #endif
+      }
+    #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;
+      #if HAS_CLASSIC_JERK
+        xy_float_t jerk_xy;
+      #endif
+    };
+  #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

Marlin/src/feature/touch/xpt2046.cpp

@@ -0,0 +1,146 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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 <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfigPre.h"
+
+#if ENABLED(TOUCH_BUTTONS)
+
+#include "xpt2046.h"
+#include "../../inc/MarlinConfig.h"
+
+#ifndef TOUCH_INT_PIN
+  #define TOUCH_INT_PIN  -1
+#endif
+#ifndef TOUCH_MISO_PIN
+  #define TOUCH_MISO_PIN MISO_PIN
+#endif
+#ifndef TOUCH_MOSI_PIN
+  #define TOUCH_MOSI_PIN MOSI_PIN
+#endif
+#ifndef TOUCH_SCK_PIN
+  #define TOUCH_SCK_PIN  SCK_PIN
+#endif
+#ifndef TOUCH_CS_PIN
+  #define TOUCH_CS_PIN   CS_PIN
+#endif
+
+XPT2046 touch;
+extern int8_t encoderDiff;
+
+void XPT2046::init() {
+  SET_INPUT(TOUCH_MISO_PIN);
+  SET_OUTPUT(TOUCH_MOSI_PIN);
+  SET_OUTPUT(TOUCH_SCK_PIN);
+  OUT_WRITE(TOUCH_CS_PIN, HIGH);
+
+  #if PIN_EXISTS(TOUCH_INT)
+    // Optional Pendrive interrupt pin
+    SET_INPUT(TOUCH_INT_PIN);
+  #endif
+
+  // Read once to enable pendrive status pin
+  getInTouch(XPT2046_X);
+}
+
+#include "../../lcd/ultralcd.h" // For EN_C bit mask
+
+uint8_t XPT2046::read_buttons() {
+  int16_t tsoffsets[4] = { 0 };
+
+  if (tsoffsets[0] + tsoffsets[1] == 0) {
+    // Not yet set, so use defines as fallback...
+    tsoffsets[0] = XPT2046_X_CALIBRATION;
+    tsoffsets[1] = XPT2046_X_OFFSET;
+    tsoffsets[2] = XPT2046_Y_CALIBRATION;
+    tsoffsets[3] = XPT2046_Y_OFFSET;
+  }
+
+  // We rely on XPT2046 compatible mode to ADS7843, hence no Z1 and Z2 measurements possible.
+
+  if (!isTouched()) return 0;
+  const uint16_t x = uint16_t(((uint32_t(getInTouch(XPT2046_X))) * tsoffsets[0]) >> 16) + tsoffsets[1],
+                 y = uint16_t(((uint32_t(getInTouch(XPT2046_Y))) * tsoffsets[2]) >> 16) + tsoffsets[3];
+  if (!isTouched()) return 0; // Fingers must still be on the TS for a valid read.
+
+  if (y < 175 || y > 234) return 0;
+
+  return WITHIN(x,  14,  77) ? EN_D
+       : WITHIN(x,  90, 153) ? EN_A
+       : WITHIN(x, 166, 229) ? EN_B
+       : WITHIN(x, 242, 305) ? EN_C
+       : 0;
+}
+
+bool XPT2046::isTouched() {
+  return (
+    #if PIN_EXISTS(TOUCH_INT)
+      READ(TOUCH_INT_PIN) != HIGH
+    #else
+      getInTouch(XPT2046_Z1) >= XPT2046_Z1_THRESHOLD
+    #endif
+  );
+}
+
+uint16_t XPT2046::getInTouch(const XPTCoordinate coordinate) {
+  uint16_t data[3];
+
+  OUT_WRITE(TOUCH_CS_PIN, LOW);
+
+  const uint8_t coord = uint8_t(coordinate) | XPT2046_CONTROL | XPT2046_DFR_MODE;
+  for (uint16_t i = 0; i < 3 ; i++) {
+    for (uint8_t j = 0x80; j; j >>= 1) {
+      WRITE(TOUCH_SCK_PIN, LOW);
+      WRITE(TOUCH_MOSI_PIN, bool(coord & j));
+      WRITE(TOUCH_SCK_PIN, HIGH);
+    }
+
+    data[i] = 0;
+    for (uint16_t j = 0x8000; j; j >>= 1) {
+      WRITE(TOUCH_SCK_PIN, LOW);
+      if (READ(TOUCH_MISO_PIN)) data[i] |= j;
+      WRITE(TOUCH_SCK_PIN, HIGH);
+    }
+    WRITE(TOUCH_SCK_PIN, LOW);
+    data[i] >>= 4;
+  }
+
+  WRITE(TOUCH_CS_PIN, HIGH);
+
+  uint16_t delta01 = _MAX(data[0], data[1]) - _MIN(data[0], data[1]),
+           delta02 = _MAX(data[0], data[2]) - _MIN(data[0], data[2]),
+           delta12 = _MAX(data[1], data[2]) - _MIN(data[1], data[2]);
+
+  if (delta01 <= delta02 && delta01 <= delta12)
+    return (data[0] + data[1]) >> 1;
+
+  if (delta02 <= delta12)
+    return (data[0] + data[2]) >> 1;
+
+  return (data[1] + data[2]) >> 1;
+}
+
+bool XPT2046::getTouchPoint(uint16_t &x, uint16_t &y) {
+  if (isTouched()) {
+    x = getInTouch(XPT2046_X);
+    y = getInTouch(XPT2046_Y);
+  }
+  return isTouched();
+}
+
+#endif // TOUCH_BUTTONS

Marlin/src/feature/touch/xpt2046.h

@@ -0,0 +1,53 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * 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 <http://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+#include <stdint.h>
+
+// Relies on XPT2046-compatible mode of ADS7843,
+// hence no Z1 / Z2 measurements are possible.
+
+#define XPT2046_DFR_MODE 0x00
+#define XPT2046_SER_MODE 0x04
+#define XPT2046_CONTROL  0x80
+
+enum XPTCoordinate : uint8_t {
+  XPT2046_X  = 0x10,
+  XPT2046_Y  = 0x50,
+  XPT2046_Z1 = 0x30,
+  XPT2046_Z2 = 0x40
+};
+
+#ifndef XPT2046_Z1_THRESHOLD
+  #define XPT2046_Z1_THRESHOLD 10
+#endif
+
+class XPT2046 {
+public:
+  static void init();
+  static uint8_t read_buttons();
+  bool getTouchPoint(uint16_t &x, uint16_t &y);
+  static bool isTouched();
+  inline void waitForRelease() { while (isTouched()) { /* nada */ } }
+  inline void waitForTouch(uint16_t &x, uint16_t &y) { while (!getTouchPoint(x, y)) { /* nada */ } }
+private:
+  static uint16_t getInTouch(const XPTCoordinate coordinate);
+};
+
+extern XPT2046 touch;

Marlin/src/feature/twibus.cpp

@@ -0,0 +1,180 @@
+/**
+ * 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 <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../inc/MarlinConfig.h"
+
+#if ENABLED(EXPERIMENTAL_I2CBUS)
+
+#include "twibus.h"
+
+#include <Wire.h>
+
+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(addr, bytes) == 0) {
+    debug("request fail", 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();
+  }
+
+#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

Marlin/src/feature/twibus.h

@@ -0,0 +1,241 @@
+/**
+ * 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 <http://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)();
+
+#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
+ *    - http://marlinfw.org/docs/gcode/M260.html
+ *    - http://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
+};

Marlin/src/feature/z_stepper_align.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 <http://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
+    );
+
+    constexpr xyz_pos_t dpo = NOZZLE_TO_PROBE_OFFSET;
+
+    #define LTEST(N) (xy_init[N].x >= _MAX(X_MIN_BED + MIN_PROBE_EDGE_LEFT,  X_MIN_POS + dpo.x) - 0.00001f)
+    #define RTEST(N) (xy_init[N].x <= _MIN(X_MAX_BED - MIN_PROBE_EDGE_RIGHT, X_MAX_POS + dpo.x) + 0.00001f)
+    #define FTEST(N) (xy_init[N].y >= _MAX(Y_MIN_BED + MIN_PROBE_EDGE_FRONT, Y_MIN_POS + dpo.y) - 0.00001f)
+    #define BTEST(N) (xy_init[N].y <= _MIN(Y_MAX_BED - MIN_PROBE_EDGE_BACK,  Y_MAX_POS + dpo.y) + 0.00001f)
+
+    static_assert(LTEST(0) && RTEST(0), "The 1st Z_STEPPER_ALIGN_XY X is unreachable with the default probe X offset.");
+    static_assert(FTEST(0) && BTEST(0), "The 1st Z_STEPPER_ALIGN_XY Y is unreachable with the default probe Y offset.");
+    static_assert(LTEST(1) && RTEST(1), "The 2nd Z_STEPPER_ALIGN_XY X is unreachable with the default probe X offset.");
+    static_assert(FTEST(1) && BTEST(1), "The 2nd Z_STEPPER_ALIGN_XY Y is unreachable with the default probe Y offset.");
+    #if NUM_Z_STEPPER_DRIVERS >= 3
+      static_assert(LTEST(2) && RTEST(2), "The 3rd Z_STEPPER_ALIGN_XY X is unreachable with the default probe X offset.");
+      static_assert(FTEST(2) && BTEST(2), "The 3rd Z_STEPPER_ALIGN_XY Y is unreachable with the default probe Y offset.");
+      #if NUM_Z_STEPPER_DRIVERS >= 4
+        static_assert(LTEST(3) && RTEST(3), "The 4th Z_STEPPER_ALIGN_XY X is unreachable with the default probe X offset.");
+        static_assert(FTEST(3) && BTEST(3), "The 4th Z_STEPPER_ALIGN_XY Y is unreachable with the default probe Y offset.");
+      #endif
+    #endif
+
+  #else // !defined(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 // !defined(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

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 <http://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;